四轴控制算法（PID、Mahony互补滤波算法等）

四轴控制算法（PID、Mahony互补滤波算法等）

第一章 四轴控制算法及实现之如何获取MPU9250的原始数据

文章目录

• 四轴控制算法（PID、Mahony互补滤波算法等）
• 前言
• 一、MPU9250简介
• • 1.概述
  • 2.通信协议-I2C
• 二、MPU9250获取原始数据
• • 1.初始化流程
  • 2.读取原始数据
• 三、代码实现结果
• 总结

前言

MPU9250 是一款9轴运动跟踪装置，他在 3X3X1mm 的封装中融合了 3轴加速度、3 轴陀螺仪、3 轴磁力计以及数字运动处理器(DMP)并且兼容 MPU6515。其完美的 I2C 方案，可直接输出 9 轴的全部数据。因此它也是四轴姿态解算的基础，所以正确获取 MPU9250 的原始数据显得尤为重要。

一、MPU9250简介

1.概述

MPU9250是一个 QFN 封装的复合芯片(MCM)，它由2部分组成。一组是 3轴加速度还有3轴陀螺仪，另一组则是 AKM 公司的 AK8963 3轴磁力计。
MPU9250 的具有三个 16 位加速度 AD 输出，三个 16 位陀螺仪 AD 输出，三个6位磁力计 AD输出。 精密的慢速和快速运动跟踪，提供给客户全量程的可编程陀螺仪参数选择(士250，土500，土1000，and士2000。/秒(dps))，可编程的加速度参数选择士2g,士4g,士8g,士16g,以及最大磁力计可达到士4800uT。

2.通信协议-I2C

MPU9250与 MCU 通信时，一般都是作为 IIC从机，MCU 为主机。IIC的一主机对多从机通信时，是需要通过地址匹配然后确立与哪个从机进行通信的。所以在进行MCU与MPU9250 通信时，首先要确定MPU9250 的从机地址。
MPU9250 作为从机时的地址为7位110100X(B)。这个地址的LSB位由AD0 引脚的电平确定，这样就使得一个系统中可以同时连接2个MPU9250了。(ADO 为低电平时 X为 0，高电平 X 则为 1)。如果只用了一个 MPU9250，可知 AD0 接高电平时，所以X为 1。那么 MPU9250 的地址为 1101001(B),十六进制表示为 0x69。

二、MPU9250获取原始数据

1.初始化流程

1. 检查 MCU 与 MPU9250 是否通信成功。
2. 设置 MPU9250 为复位状态+100ms 复位延时
3. 唤醒 MPU9250,选择 PLL 为时钟源
4. 配置六轴数据(3 轴加速度+3 轴陀螺仪)全输出
5. 禁止所有中断
6. 设置 MPU9250 内部采样频率
7. 设置陀螺仪和加速度的满量程范围
8. 设置陀螺仪和加速度输出低通滤波

初始化MPU9250代码如下（示例）：

void MPU9250_Init(void)
{
    MPU9250_Check(); //检查MPU9250是否连接
	
	MPU9250_WriteByte(MPU9250_RA_PWR_MGMT_1, 0x80); //复位MPU9250
    delay_ms(100);
    MPU9250_WriteByte(MPU9250_RA_PWR_MGMT_1, 0x01); //唤醒MPU9250，并选择PLL为时钟源
	MPU9250_WriteByte(MPU9250_RA_PWR_MGMT_2, 0x00); //设置输出三轴陀螺仪与三轴加速度数据
	MPU9250_WriteByte(MPU9250_RA_INT_ENABLE, 0x00); //禁止中断
    MPU9250_WriteByte(MPU9250_RA_SMPLRT_DIV, 0x00); //采样分频  （采样频率 = 陀螺仪输出频率 / (1+DIV)，采样频率1000hz）
	MPU9250_WriteByte(MPU9250_RA_GYRO_CONFIG, 0x18); //陀螺仪满量程+-2000度/秒     (最低分辨率 = 2^16/4000 = 16.4LSB/度/秒 
	MPU9250_WriteByte(MPU9250_RA_ACCEL_CONFIG, 0x08); //加速度满量程+-4g            (最低分辨率 = 2^16/8g = 8196LSB/g )
	MPU9250_WriteByte(MPU9250_RA_ACCEL_CONFIG_2,0x04); //设置加速度滤波为DLPF=20Hz  
    MPU9250_WriteByte(MPU9250_RA_CONFIG, MPU9250_DLPF_BW_98);//设置陀螺的输出为1kHZ,DLPF=92Hz  
	MPU9250_WriteByte(MPU9250_RA_INT_PIN_CFG, 0x02);//MPU 可直接访问MPU9250辅助I2C
}

2.读取原始数据

主函数读取原始数据代码如下（示例）：

int main()
{
	LED_init();         //LED初始化化
	USART_init(115200); //USART1初始化
	Delay_init();       //延时初始化
	IIC_Init();
	MPU9250_Init();
	
	while(1)
	{
		MPU9250_GyroRead(GYRO);
		MPU9250_AccRead(ACC);
		MPU9250_TempRead(&Temperature);	
		
		printf("GYRO_X:%d  GYRO_Y:%d GYRO_Z:%d\r\n",GYRO[0],GYRO[1],GYRO[2]);
		printf(" ACC_X:%d   ACC_Y:%d   ACC_Z:%d\r\n",ACC[0],ACC[1],ACC[2]);
		printf("Temperature:%0.2f\r\n\r\n",Temperature);
		LED5_Run(); //D5闪烁
		delay_ms(500);//延时500ms
	}
}

void MPU9250_AccRead(int16_t *accData)
{
    uint8_t buf[6]; 
   	MPU9250_ReadMultBytes(MPU9250_RA_ACCEL_XOUT_H,6,buf);
    accData[0] = (int16_t)((buf[0] << 8) | buf[1]);
    accData[1] = (int16_t)((buf[2] << 8) | buf[3]);
    accData[2] = (int16_t)((buf[4] << 8) | buf[5]);
}
void MPU9250_GyroRead(int16_t *gyroData)
{
    uint8_t buf[6];
	
	  MPU9250_ReadMultBytes(MPU9250_RA_GYRO_XOUT_H, 6, buf);
    gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]) ;
    gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]) ;
    gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) ;
}
void MPU9250_TempRead(float *tempdata)
{
	uint8_t buf[2];
	short data;
	MPU9250_ReadMultBytes(MPU9250_RA_TEMP_OUT_H, 2, buf);
	data = (int16_t)((buf[0] << 8) | buf[1]) ;
	*tempdata = (data/333.87)+21;
}
uint8_t MPU9250_ReadMultBytes(uint8_t reg,uint8_t len,uint8_t *buf)
{
	if(IIC_ReadMultByteFromSlave(MPU9250Addr,reg,len,buf))
	   return 1;
	else
	   return 0;
}

三、代码实现结果

运行结果

总结

以上就是获取IMU原始数据的基本步骤，如需工程代码可留言。