如何使用DMP库对MPU6050进行初始化呢

　　1、实验分析
　　MPU6050包含一个三轴陀螺仪，三轴加速度计，并且可以通过AUX_CL和AUX_DA再扩展一个磁力计，内部设有一个可扩展的数字运动处理器DMP，可以将欧拉角以四元数的形式输出。本次实验我们使用DMP库对MPU6050初始化并且通过DMP中的库函数读取加速度值和陀螺仪值显示在LCD上
　　2、实验前准备
　　使用DMP库需要在MPU官方下载固件并将下面六个导入工程
　　
　　并且在固件库中需要进行一定的设定。
　　在inv_mpu.c文件中预留有调用文件的接口，需要我们提前设定一下
　　首先
　　#define MPU6050 //定义我们使用的传感器为MPU6050
　　#define MOTION_DRIVER_TARGET_MSP430 //定义驱动部分，采用MSP430的驱动（移植到STM32F1）
　　其次
　　要对接接口函数
　　/* The following functions must be defined for this platform：
　　1. i2c_write（unsigned char slave_addr， unsigned char reg_addr，
　　2. unsigned char length， unsigned char const *data）
　　3. i2c_read（unsigned char slave_addr， unsigned char reg_addr，
　　4. unsigned char length， unsigned char *data）
　　5. delay_ms（unsigned long num_ms）
　　6. get_ms（unsigned long *count）
　　7. reg_int_cb（void （*cb）（void）， unsigned char port， unsigned char pin）
　　8. labs（long x）
　　9. fabsf（float x）
　　10. min（int a， int b）
　　*/
　　#define i2c_write MPU_Write_Len
　　#define i2c_read MPU_Read_Len
　　#define delay_ms delay_ms
　　#define get_ms mget_ms
　　#define log_i printf //打印信息
　　#define log_e printf //打印信息
　　按照官方注释的要求，接口函数的入口参数必须按照要求对齐，否则将无法使用DMP库函数。
　　3、实验分析
　　上一次我们编写了软模拟IIC的程序，本次需要增加IIC连续写和连续读，这样我们才可以直接操作MPU中的寄存器。
　　操作步骤：
　　IIC初始化-》MPU的初始化-》DMP库初始化-》使用库函数读取数据
　　（1）MPU初试步骤
　　设置AD0引脚电平（低电平，从机地址为0x68；高电平，从机地址为0x69）
　　IIC初始化
　　复位从机，唤醒从机
　　设置陀螺仪、加速度计、设置采样速率
　　关闭所有中断、IIC主模式关闭、关闭FIFO
　　设置INT引脚低电平有效
　　判断从机地址是否正确
　　设置正方向
　　使能陀螺仪和加速度计
　　4、MPU6050寄存器分析
　　1、自检寄存器
　　
　　MPU自检寄存器，会产生自检反馈：STR=Gyroscope Output with Self Test Enabled — Gyroscope Output with Self Test Disabled。可以判断自检成功or失败
　　2、采样频率分频器
　　
　　传感器寄存器输出， FITO 输出， DMP 采样， Motion 检测， Zero Motion 检测和 Free Fall检测都基于这个采样频率。
　　采样频率=陀螺仪输出频率/（ 1+SMPLRT_DIV）
　　陀螺仪的输出频率是1KHz或者8KHz，与数字低通滤波器（DLPF）有关，当DLPF_CFG=0/7时，频率为8K，其他为1K，而且 DLPF 滤波频率一般设置为采样率的一半。采样率，我们假定设置为 50Hz，那么SMPLRT_DIV=1000/50-1=19。
　　3、陀螺仪配置寄存器
　　
　　这个寄存器我们只需要设置FS_SEL位，用于设置陀螺仪满量程范围：0，±250°/s；1，±500°/s；2，±1000°/s；3，±2000°/s
　　陀螺仪为16位的ADC，所以灵敏度可以达到65536/4000=16.4LSB/（° /S）。
　　4、加速度计配置寄存器
　　
　　和陀螺仪一样，只需要设置AFS_SEL位，0，±2g； 1，±4g； 2，±8g； 3，±16g；我们一般设置为 0，即±2g，因为加速度传感器的 ADC，也是 16 位，所以得到灵敏度为： 65536/4=16384LSB/g。
　　5、FIFO使能寄存器
　　
　　该寄存器用于控制 FIFO 使能，在简单读取传感器数据的时候，可以不用 FIFO，设置对应位为 0 即可禁止 FIFO，设置为 1，则使能 FIFO。注意加速度传感器的 3 个轴，全由 1 个位（ACCEL_FIFO_EN）控制，只要该位置 1，则加速度传感器的三个通道都开启 FIFO。
　　6、配置寄存器
　　
　　这里的DLPF_CFG位就是数字低通滤波器的设置位
　　
　　一般我们设置角速度传感器的带宽为其采样率的一半，比如前面设置的采样率为100Hz，那么带宽就应该设置50Hz，取表中角速度近似值42Hz，就应该设置DLPF_CFG=011;
　　7、电源管理寄存器1
　　
　　DEVICE_RESET 位用来控制复位，设置为 1，复位 MPU6050，复位结束后， MPU硬件自动清零该位。
　　SLEEEP 位用于控制 MPU6050 的工作模式，复位后，该位为 1，即进入了睡眠模式（低功耗），所以我们要清零该位，以进入正常工作模式。
　　TEMP_DIS 用于设置是否使能温度传感器，设置为 0，则使能。
　　CLKSEL［2:0］用于选择系统时钟源
　　
　　内部8M晶振，不太准确，一般采用X轴的PLL作为时钟源
　　8、电源管理寄存器2
　　
　　LP_WAKE_CTRL 用于控制低功耗时的唤醒频率，用不到。
　　剩下的 6 位，分别控制加速度和陀螺仪的 x/y/z 轴是否进入待机模式，这里我们全部都不进入待机模式，所以全部设置为 0 即可。
　　9、数据输出寄存器
　　陀螺仪数据输出寄存器，总共有 6 个寄存器组成，
　　地址为： 0X43~0X48，
　　通过读取这 6 个寄存器，就可以读到陀螺仪 x/y/z 轴的值，
　　比如 x 轴的数据，可以通过读取 0X43（高 8 位）和 0X44（低 8 位）寄存器得到，其他轴以此类推。
　　同样，加速度传感器数据输出寄存器，也有 6 个，地址为： 0X3B~0X40，
　　通过读取这 6 个寄存器，就可以读到加速度传感器 x/y/z 轴的值，
　　比如读 x 轴的数据，可以通过读取 0X3B（高8 位）和 0X3C（低 8 位）寄存器得到，其他轴以此类推。
　　10、温度传感器
　　温度传感器的值，可以通过读取 0X41（高 8 位）和 0X42（低 8 位）寄存器得到，温度换算公式为：
　　Temperature = 36.53 + regval/340
　　Temperature 为计算得到的温度值，单位为℃， regval 为从 0X41 和 0X42 读到的温度传感器值。
　　5、代码示例
　　1、读写数据
　　/**
　　* 功能：IIC写一个字节
　　* 入口参数：reg，寄存器地址；data，数据
　　* 返回值：0，正常；1，错误
　　*/
　　u8 MPU_Write_Byte（u8 reg，u8 data）
　　{
　　MPU_IIC_Start（）;
　　MPU_IIC_Send_Byte（（MPU_ADDR《《1）|0）;//ADDR左移一位+写操作
　　if（MPU_IIC_Wait_Ack（））//等待应答
　　{
　　MPU_IIC_Stop（）;
　　return 1;
　　}
　　MPU_IIC_Send_Byte（reg）;
　　MPU_IIC_Wait_Ack（）;
　　MPU_IIC_Send_Byte（data）;
　　if（MPU_IIC_Wait_Ack（））
　　{
　　MPU_IIC_Stop（）;
　　return 1;
　　}
　　MPU_IIC_Stop（）;
　　return 0;
　　}
　　/**
　　* 功能：IIC读一个字节
　　* 入口参数：reg，寄存器地址
　　* 返回值：数据
　　*/
　　u8 MPU_Read_Byte（u8 reg）
　　{
　　u8 res;
　　MPU_IIC_Start（）;
　　MPU_IIC_Send_Byte（（MPU_ADDR《《1）|0）;//外设地址+写命令
　　MPU_IIC_Wait_Ack（）;
　　MPU_IIC_Send_Byte（reg）;//寄存器地址
　　MPU_IIC_Wait_Ack（）;
　　MPU_IIC_Start（）;
　　MPU_IIC_Send_Byte（（MPU_ADDR《《1）|1）;
　　MPU_IIC_Wait_Ack（）;
　　res = MPU_IIC_Read_Byte（0）;
　　MPU_IIC_Stop（）;
　　return res;
　　}
　　/**
　　* 功能：IIC连续写
　　* 入口参数：addr，外设地址；reg，寄存器地址；len，写入长度；buf，数据
　　* 返回值：0，正常；1，错误
　　*/
　　u8 MPU_Write_Len（u8 addr，u8 reg，u8 len，u8 *buf）
　　{
　　u8 i;
　　MPU_IIC_Start（）;
　　MPU_IIC_Send_Byte（（addr《《1）|0）;
　　if（MPU_IIC_Wait_Ack（））
　　{
　　MPU_IIC_Stop（）;
　　return 1;
　　}
　　MPU_IIC_Send_Byte（reg）;
　　MPU_IIC_Wait_Ack（）;
　　for（i=0;i《len;i++）
　　{
　　MPU_IIC_Send_Byte（buf［i］）;
　　if（MPU_IIC_Wait_Ack（））
　　{
　　MPU_IIC_Stop（）;
　　return 1;
　　}
　　}
　　MPU_IIC_Stop（）;
　　return 0;
　　}
　　/**
　　* 功能：IIC连续读
　　* 入口参数：addr，外设地址；reg，寄存器地址；len，读取长度；buf，储存数据
　　* 返回值：0，正常；1，错误
　　*/
　　u8 MPU_Read_Len（u8 addr，u8 reg，u8 len，u8 *buf）
　　{
　　MPU_IIC_Start（）;
　　MPU_IIC_Send_Byte（（addr《《1）|0）;
　　if（MPU_IIC_Wait_Ack（））
　　{
　　MPU_IIC_Stop（）;
　　return 1;
　　}
　　MPU_IIC_Send_Byte（reg）;
　　MPU_IIC_Wait_Ack（）;
　　MPU_IIC_Start（）;
　　MPU_IIC_Send_Byte（（addr《《1）|1）;
　　MPU_IIC_Wait_Ack（）;
　　while（len）
　　{
　　if（len==1）
　　*buf=MPU_IIC_Read_Byte（0）;
　　else
　　*buf=MPU_IIC_Read_Byte（1）;
　　len--;
　　buf++;
　　}
　　MPU_IIC_Stop（）;
　　return 0;
　　}
　　2、陀螺仪初始化代码
　　/**
　　* 功能：初始化MPU6050
　　* 入口参数：无
　　* 返回值：0，成功；其他，错误
　　*/
　　u8 MPU_Init（void）
　　{
　　u8 res;
　　GPIO_InitTypeDef GPIO_InitStructure;
　　RCC_APB2PeriphClockCmd（RCC_APB2Periph_AFIO | RCC_APB2Periph_GPIOA，ENABLE）;
　　GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15;
　　GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
　　GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
　　GPIO_Init（GPIOA，&GPIO_InitStructure）;
　　GPIO_PinRemapConfig（GPIO_Remap_SWJ_JTAGDisable，ENABLE）;//禁止JTAG，使PA15变为普通IO
　　MPU_AD0_CTRL = 0;//控制MPU6050的AD0为低电平，从机地址=0x68；
　　// 高电平， 0x69；
　　MPU_IIC_Init（）;
　　MPU_Write_Byte（MPU_PWR_MGMT1_REG，0x80）;//复位从机
　　delay_ms（100）;
　　MPU_Write_Byte（MPU_PWR_MGMT1_REG，0x00）;//唤醒从机
　　MPU_Set_Gyro_Fsr（3）;//陀螺仪传感器 +-2000dps
　　MPU_Set_Accel_Fsr（0）;//加速度传感器 +-2g
　　MPU_Set_Rate（50）;
　　MPU_Write_Byte（MPU_INT_EN_REG，0x00）;//关闭所有中断
　　MPU_Write_Byte（MPU_USER_CTRL_REG，0x00）;//IIC主模式关闭
　　MPU_Write_Byte（MPU_FIFO_EN_REG，0x00）;//关闭FIFO
　　MPU_Write_Byte（MPU_INTBP_CFG_REG，0x80）;//INT低电平有效
　　res = MPU_Read_Byte（MPU_DEVICE_ID_REG）;
　　if（res == MPU_ADDR）
　　{
　　MPU_Write_Byte（MPU_PWR_MGMT1_REG，0x01）;//设置CLKSEL PLL X轴为参考
　　MPU_Write_Byte（MPU_PWR_MGMT2_REG，0x00）;//加速度计和陀螺仪使能
　　MPU_Set_Rate（50）;
　　}
　　else
　　return 1;
　　return 0;
　　}
　　/**
　　* 功能：设置陀螺仪传感器满量程范围
　　* 入口参数：fsr，0，+-250dps
　　* 1，+-500dps
　　* 2，+-1000dps
　　* 3，+-2000dps
　　* 返回值：0，成功；1，失败
　　*/
　　u8 MPU_Set_Gyro_Fsr（u8 fsr）
　　{
　　return MPU_Write_Byte（MPU_GYRO_CFG_REG，fsr《《3）;
　　}
　　/**
　　* 功能：设置加速度传感器满量程范围
　　* 入口参数：fsr，0，+-2g
　　* 1，+-4g
　　* 2，+-8g
　　* 3，+-16g
　　* 返回值：0，成功；1，失败
　　*/
　　u8 MPU_Set_Accel_Fsr（u8 fsr）
　　{
　　return MPU_Write_Byte（MPU_ACCEL_CFG_REG，fsr《《3）;
　　}
　　/**
　　* 功能：设置数字低通滤波器
　　* 入口参数：lpf，低通滤波器频率（Hz）
　　* 返回值：0，成功；1，失败
　　*/
　　u8 MPU_Set_LPF（u16 lpf）
　　{
　　u8 data=0;
　　if（lpf》=188）data=1;
　　else if（lpf》=98）data=2;
　　else if（lpf》=42）data=3;
　　else if（lpf》=20）data=4;
　　else if（lpf》=10）data=5;
　　else data=6;
　　return MPU_Write_Byte（MPU_CFG_REG，data）;
　　}
　　/**
　　* 功能：设置采样率（Fs=1KHz）
　　* 入口参数：rate=［4，1000］Hz
　　* 返回值：0，设置成功；1，设置失败
　　*/
　　u8 MPU_Set_Rate（u16 rate）
　　{
　　u8 data;
　　if（rate》1000）rate=1000;
　　if（rate《4）rate=4;
　　data=1000/rate-1;
　　data=MPU_Write_Byte（MPU_SAMPLE_RATE_REG，data）;
　　return MPU_Set_LPF（rate/2）;
　　}
　　3、获取原始数据代码
　　/**
　　* 功能：获取MPU内部温度
　　* 入口参数：无
　　* 返回值：温度*100
　　*/
　　short MPU_Get_Temperature（void）
　　{
　　u8 buf［2］;
　　short raw;
　　float temp;
　　MPU_Read_Len（MPU_ADDR，MPU_TEMP_OUTH_REG，2，buf）;
　　raw=（（u16）buf［0］《《8）|buf［1］;
　　temp = 36.53+（（double）raw）/340;
　　return temp*100;
　　}
　　/**
　　* 功能：获得陀螺仪原始值
　　* 入口参数：gx，gy，gz，陀螺仪XYZ轴原始数据（带符号）
　　* 返回值：0，成功；1，失败
　　*/
　　u8 MPU_Get_Gyroscope（short *gx，short *gy，short *gz）
　　{
　　u8 buf［6］，res;
　　res=MPU_Read_Len（MPU_ADDR，MPU_GYRO_XOUTH_REG，6，buf）;
　　if（res == 0）
　　{
　　*gx=（（u16）buf［0］《《8）|buf［1］;
　　*gy=（（u16）buf［2］《《8）|buf［3］;
　　*gz=（（u16）buf［4］《《8）|buf［5］;
　　}
　　return res;
　　}
　　/**
　　* 功能：获得加速度原始值
　　* 入口参数：ax，ay，az，加速度计XYZ的原始数据（）
　　* 返回值：0，正常；1，错误
　　*/
　　u8 MPU_Get_Accelerometer（short *ax，short *ay，short *az）
　　{
　　u8 buf［6］，res;
　　res=MPU_Read_Len（MPU_ADDR，MPU_ACCEL_XOUTH_REG，6，buf）;
　　if（res ==0）
　　{
　　*ax=（（u16）buf［0］《《8）|buf［1］;
　　*ay=（（u16）buf［2］《《8）|buf［3］;
　　*az=（（u16）buf［4］《《8）|buf［5］;
　　}
　　return res;
　　}
　　

4、DMP库函数调用
　　//mpu6050，dmp初始化
　　//返回值：0，正常
　　// 其他，失败
　　u8 mpu_dmp_init（void）
　　{
　　u8 res=0;
　　MPU_IIC_Init（）; //初始化IIC总线
　　if（mpu_init（）==0） //初始化MPU6050
　　{
　　res=mpu_set_sensors（INV_XYZ_GYRO|INV_XYZ_ACCEL）;//设置所需要的传感器
　　if（res）return 1;
　　res=mpu_configure_fifo（INV_XYZ_GYRO|INV_XYZ_ACCEL）;//设置FIFO
　　if（res）return 2;
　　res=mpu_set_sample_rate（DEFAULT_MPU_HZ）; //设置采样率
　　if（res）return 3;
　　res=dmp_load_motion_driver_firmware（）; //加载dmp固件
　　if（res）return 4;
　　res=dmp_set_orientation（inv_orientation_matrix_to_scalar（gyro_orientation））;//设置陀螺仪方向
　　if（res）return 5;
　　res=dmp_enable_feature（DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| //设置dmp功能
　　DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
　　DMP_FEATURE_GYRO_CAL）;
　　if（res）return 6;
　　res=dmp_set_fifo_rate（DEFAULT_MPU_HZ）; //设置DMP输出速率（最大不超过200Hz）
　　if（res）return 7;
　　res=run_self_test（）; //自检
　　if（res）return 8;
　　res=mpu_set_dmp_state（1）; //使能DMP
　　if（res）return 9;
　　}else return 10;
　　return 0;
　　}
　　//得到dmp处理后的数据（注意，本函数需要比较多堆栈，局部变量有点多）
　　//pitch：俯仰角 精度：0.1° 范围：-90.0° 《---》 +90.0°
　　//roll：横滚角 精度：0.1° 范围：-180.0°《---》 +180.0°
　　//yaw：航向角 精度：0.1° 范围：-180.0°《---》 +180.0°
　　//返回值：0，正常
　　// 其他，失败
　　u8 mpu_dmp_get_data（float *pitch，float *roll，float *yaw）
　　{
　　float q0=1.0f，q1=0.0f，q2=0.0f，q3=0.0f;
　　unsigned long sensor_timestamp;
　　short gyro［3］， accel［3］， sensors;
　　unsigned char more;
　　long quat［4］;
　　if（dmp_read_fifo（gyro， accel， quat， &sensor_timestamp， &sensors，&more））return 1;
　　/* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
　　* This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
　　**/
　　/*if （sensors & INV_XYZ_GYRO ）
　　send_packet（PACKET_TYPE_GYRO， gyro）;
　　if （sensors & INV_XYZ_ACCEL）
　　send_packet（PACKET_TYPE_ACCEL， accel）; */
　　/* Unlike gyro and accel， quaternions are written to the FIFO in the body frame， q30.
　　* The orientation is set by the scalar passed to dmp_set_orientation during initialization.
　　**/
　　if（sensors&INV_WXYZ_QUAT）
　　{
　　q0 = quat［0］ / q30; //q30格式转换为浮点数
　　q1 = quat［1］ / q30;
　　q2 = quat［2］ / q30;
　　q3 = quat［3］ / q30;
　　//计算得到俯仰角/横滚角/航向角
　　*pitch = asin（-2 * q1 * q3 + 2 * q0* q2）* 57.3; // pitch
　　*roll = atan2（2 * q2 * q3 + 2 * q0 * q1， -2 * q1 * q1 - 2 * q2* q2 + 1）* 57.3; // roll
　　*yaw = atan2（2*（q1*q2 + q0*q3），q0*q0+q1*q1-q2*q2-q3*q3） * 57.3; //yaw
　　}else return 2;
　　return 0;
　　}
　　6、实验现象
　　7、总结
　　对于MPU6050数据的处理，使用DMP库是一种比较简单的办法，如果理论扎实，可以自行读取原始数据，通过四元数解算获取欧拉角。下一次我们将会把陀螺仪得到的数据发送到上位机，下次主要分析上位机的通讯协议。