MPU6500模块调试

VIO里的惯性数据，这里通过MPU6500输出姿态数据。

1. 配置方式和数据读取方式

SPI或者IIC都可以，但对我算法来说，低速的IIC就可以，400KHz.

/*
 * 函数名：MPU6500_Init
 * 描述  ：MPU6500初始化函数
 * 输入  ：无
 * 输出  ：0：初始化失败 1：初始化成功
 */ 
u8 MPU6500_Init(void)
{
	SPI1_Init();																//MPU6500 IO口和SPI初始化
	
	if(MPU6500_Read_Reg(WHO_AM_I) == 0x70)			//正确读取到6500的地址
	{		
		MPU6500_Write_Reg(PWR_MGMT_1,0X80);   		//电源管理,复位MPU6500
		Delay_Ms(100);
		MPU6500_Write_Reg(SIGNAL_PATH_RESET,0X07);//陀螺仪、加速度计、温度计复位
		Delay_Ms(100);
		MPU6500_Write_Reg(PWR_MGMT_1,0X01);   		//选择时钟源
		MPU6500_Write_Reg(PWR_MGMT_2,0X00);   		//使能加速度计和陀螺仪
		MPU6500_Write_Reg(CONFIG,0X02);						//低通滤波器 0x02 92hz (3.9ms delay) fs=1khz
		MPU6500_Write_Reg(SMPLRT_DIV,0X00);				//采样率1000/(1+0)=1000HZ
		MPU6500_Write_Reg(GYRO_CONFIG,0X18);  		//陀螺仪测量范围 0X18 正负2000度
		MPU6500_Write_Reg(ACCEL_CONFIG,0x10); 		//加速度计测量范围 0X00 正负8g
		MPU6500_Write_Reg(ACCEL_CONFIG2,0x00);		//加速度计速率1khz 滤波器460hz (1.94ms delay)
		return 1;
	}
	else return 0;
}

初始化及配置方式，基本如上，然后，读取需要的寄存器即可。

2. 姿态解算

通过IMU的六轴传感器数据，三轴陀螺仪和三轴加速度计，使用四元数进行姿态解算。

主要分为两步：

（一）

（二）

3. DMP

MPU6500内部集成了DMP，并且，官方提供了针对DMP的运动驱动库，通过IIC接口配置MPU6500，即可得到四元数数据（FIFO），然后，转化为YAW、PITCH、ROLL数据。

（三）

4. UI

添加串口驱动，按照AHRS_Captain_Flight上位机串口通信协议，设计UART接口程序。

（四）

配置上位机软件，实现3D模拟显示。

到处，完成了MPU6500接口调试的基本工作。

参考博文：

http://blog.sina.com.cn/s/blog_c5a00db10102wd7d.html