21ic电子技术开发论坛 › 单片机与嵌入式系统 › ST MCU › STM32 使用DMP库处理MPU6050数据
1234下一页
返回列表 发新帖我要提问

[其他ST产品] STM32 使用DMP库处理MPU6050数据

[复制链接]
 楼主| 和下土 发表于 2022-5-28 20:06 | 显示全部楼层
5、代码示例
1、读写数据

复制
  1. /**  
    

  2.   *  功能：IIC写一个字节
    

  3.   *  入口参数：reg，寄存器地址；data，数据
    

  4.   *  返回值：0，正常；1，错误
    

  5.   */
    

  6. u8 MPU_Write_Byte(u8 reg,u8 data)
    

  7. {
    

  8.         MPU_IIC_Start();
    

  9.         MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//ADDR左移一位+写操作
    

  10.         if(MPU_IIC_Wait_Ack())//等待应答
    

  11.         {
    

  12.                 MPU_IIC_Stop();
    

  13.                 return 1;
    

  14.         }
    

  15.         MPU_IIC_Send_Byte(reg);
    

  16.         MPU_IIC_Wait_Ack();
    

  17.         MPU_IIC_Send_Byte(data);
    

  18.         if(MPU_IIC_Wait_Ack())
    

  19.         {
    

  20.                 MPU_IIC_Stop();
    

  21.                 return 1;
    

  22.         }
    

  23.         MPU_IIC_Stop();
    

  24.         return 0;
    

  25. }
    

  26.   
    

  27. /**  
    

  28.   *  功能：IIC读一个字节
    

  29.   *  入口参数：reg，寄存器地址
    

  30.   *  返回值：数据
    

  31.   */
    

  32. u8 MPU_Read_Byte(u8 reg)
    

  33. {
    

  34.         u8 res;
    

  35.         MPU_IIC_Start();
    

  36.         MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//外设地址+写命令
    

  37.         MPU_IIC_Wait_Ack();
    

  38.         MPU_IIC_Send_Byte(reg);//寄存器地址
    

  39.         MPU_IIC_Wait_Ack();
    

  40.         MPU_IIC_Start();
    

  41.         MPU_IIC_Send_Byte((MPU_ADDR<<1)|1);
    

  42.         MPU_IIC_Wait_Ack();
    

  43.         res = MPU_IIC_Read_Byte(0);
    

  44.         MPU_IIC_Stop();
    

  45.         return res;
    

  46. }
    

  47. /**  
    

  48.   *  功能：IIC连续写
    

  49.   *  入口参数：addr，外设地址；reg，寄存器地址；len，写入长度；buf，数据
    

  50.   *  返回值：0，正常；1，错误
    

  51.   */
    

  52. u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
    

  53. {
    

  54.         u8 i;
    

  55.         MPU_IIC_Start();
    

  56.         MPU_IIC_Send_Byte((addr<<1)|0);
    

  57.         if(MPU_IIC_Wait_Ack())
    

  58.         {
    

  59.                 MPU_IIC_Stop();
    

  60.                 return 1;
    

  61.         }
    

  62.         MPU_IIC_Send_Byte(reg);
    

  63.         MPU_IIC_Wait_Ack();
    

  64.         for(i=0;i<len;i++)
    

  65.         {
    

  66.                 MPU_IIC_Send_Byte(buf[i]);
    

  67.                 if(MPU_IIC_Wait_Ack())
    

  68.                 {
    

  69.                         MPU_IIC_Stop();
    

  70.                         return 1;
    

  71.                 }
    

  72.         }
    

  73.         MPU_IIC_Stop();
    

  74.         return 0;
    

  75. }
    

  76. /**  
    

  77.   *  功能：IIC连续读
    

  78.   *  入口参数：addr，外设地址；reg，寄存器地址；len，读取长度；buf，储存数据
    

  79.   *  返回值：0,正常；1，错误
    

  80.   */
    

  81. u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
    

  82. {
    

  83.         MPU_IIC_Start();
    

  84.         MPU_IIC_Send_Byte((addr<<1)|0);
    

  85.         if(MPU_IIC_Wait_Ack())
    

  86.         {
    

  87.                 MPU_IIC_Stop();
    

  88.                 return 1;
    

  89.         }
    

  90.         MPU_IIC_Send_Byte(reg);
    

  91.         MPU_IIC_Wait_Ack();
    

  92.         MPU_IIC_Start();
    

  93.         MPU_IIC_Send_Byte((addr<<1)|1);
    

  94.         MPU_IIC_Wait_Ack();
    

  95.         while(len)
    

  96.         {
    

  97.                 if(len==1)
    

  98.                         *buf=MPU_IIC_Read_Byte(0);
    

  99.                 else                
    

  100.                         *buf=MPU_IIC_Read_Byte(1);                
    

  101.                 len--;
    

  102.                 buf++;
    

  103.         }
    

  104.         MPU_IIC_Stop();
    

  105.         return 0;
    

  106. }
回复 赏

收藏 举报

 楼主| 和下土 发表于 2022-5-28 20:07 | 显示全部楼层
2、陀螺仪初始化代码
复制
  1. /**  
    

  2.   *  功能：初始化MPU6050
    

  3.   *  入口参数：无
    

  4.   *  返回值：0，成功；其他，错误
    

  5.   */
    

  6. u8 MPU_Init(void)
    

  7. {
    

  8.         u8 res;
    

  9.         GPIO_InitTypeDef GPIO_InitStructure;
    

  10.         
    

  11.         RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph_GPIOA,ENABLE);
    

  12.         
    

  13.         GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15;
    

  14.         GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    

  15.         GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    

  16.         GPIO_Init(GPIOA,&GPIO_InitStructure);
    

  17.         
    

  18.         GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);//禁止JTAG，使PA15变为普通IO
    

  19.         
    

  20.         MPU_AD0_CTRL = 0;//控制MPU6050的AD0为低电平，从机地址=0x68；
    

  21.                                          //                                         高电平，                  0x69；
    

  22.         MPU_IIC_Init();
    

  23.         MPU_Write_Byte(MPU_PWR_MGMT1_REG,0x80);//复位从机
    

  24.         delay_ms(100);
    

  25.         MPU_Write_Byte(MPU_PWR_MGMT1_REG,0x00);//唤醒从机
    

  26.         MPU_Set_Gyro_Fsr(3);//陀螺仪传感器 +-2000dps
    

  27.         MPU_Set_Accel_Fsr(0);//加速度传感器 +-2g
    

  28.         MPU_Set_Rate(50);
    

  29.         MPU_Write_Byte(MPU_INT_EN_REG,0x00);//关闭所有中断
    

  30.         MPU_Write_Byte(MPU_USER_CTRL_REG,0x00);//IIC主模式关闭
    

  31.         MPU_Write_Byte(MPU_FIFO_EN_REG,0x00);//关闭FIFO
    

  32.         MPU_Write_Byte(MPU_INTBP_CFG_REG,0x80);//INT低电平有效
    

  33.         res = MPU_Read_Byte(MPU_DEVICE_ID_REG);
    

  34.         if(res == MPU_ADDR)
    

  35.         {
    

  36.                 MPU_Write_Byte(MPU_PWR_MGMT1_REG,0x01);//设置CLKSEL PLL X轴为参考
    

  37.                 MPU_Write_Byte(MPU_PWR_MGMT2_REG,0x00);//加速度计和陀螺仪使能
    

  38.                 MPU_Set_Rate(50);
    

  39.         }
    

  40.         else
    

  41.                 return 1;
    

  42.         return 0;
    

  43. }
    

  44. /**  
    

  45.   *  功能：设置陀螺仪传感器满量程范围
    

  46.   *  入口参数：fsr,0,+-250dps
    

  47.   *                                   1,+-500dps
    

  48.   *                                   2,+-1000dps
    

  49.   *                                   3,+-2000dps        
    

  50.   *  返回值：0,成功；1，失败
    

  51.   */
    

  52. u8 MPU_Set_Gyro_Fsr(u8 fsr)
    

  53. {
    

  54.         return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);
    

  55. }
    

  56. /**  
    

  57.   *  功能：设置加速度传感器满量程范围
    

  58.   *  入口参数：fsr，0,+-2g
    

  59.   *                                    1,+-4g
    

  60.   *                                    2,+-8g
    

  61.   *                                    3,+-16g        
    

  62.   *  返回值：0,成功；1，失败
    

  63.   */
    

  64. u8 MPU_Set_Accel_Fsr(u8 fsr)
    

  65. {
    

  66.         return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);
    

  67. }
    

  68. /**  
    

  69.   *  功能：设置数字低通滤波器
    

  70.   *  入口参数：lpf，低通滤波器频率（Hz）
    

  71.   *  返回值：0,成功；1，失败
    

  72.   */
    

  73. u8 MPU_Set_LPF(u16 lpf)
    

  74. {
    

  75.         u8 data=0;
    

  76.         if(lpf>=188)data=1;
    

  77.         else if(lpf>=98)data=2;
    

  78.         else if(lpf>=42)data=3;
    

  79.         else if(lpf>=20)data=4;
    

  80.         else if(lpf>=10)data=5;
    

  81.         else data=6;
    

  82.         return MPU_Write_Byte(MPU_CFG_REG,data);
    

  83. }
    

  84. /**  
    

  85.   *  功能：设置采样率（Fs=1KHz）
    

  86.   *  入口参数：rate=[4,1000]Hz
    

  87.   *  返回值：0,设置成功；1，设置失败
    

  88.   */
    

  89. u8 MPU_Set_Rate(u16 rate)
    

  90. {
    

  91.         u8 data;
    

  92.         if(rate>1000)rate=1000;
    

  93.         if(rate<4)rate=4;
    

  94.         data=1000/rate-1;
    

  95.         data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data);
    

  96.         return MPU_Set_LPF(rate/2);
    

  97. }
回复 赏

收藏 举报

 楼主| 和下土 发表于 2022-5-28 20:07 | 显示全部楼层
3、获取原始数据代码
复制
  1. /**  
    

  2.   *  功能：获取MPU内部温度
    

  3.   *  入口参数：无
    

  4.   *  返回值：温度*100
    

  5.   */
    

  6. short MPU_Get_Temperature(void)
    

  7. {
    

  8.         u8 buf[2];
    

  9.         short raw;
    

  10.         float temp;
    

  11.         MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf);
    

  12.         raw=((u16)buf[0]<<8)|buf[1];
    

  13.         temp = 36.53+((double)raw)/340;
    

  14.         return temp*100;
    

  15. }
    

  16. /**  
    

  17.   *  功能：获得陀螺仪原始值
    

  18.   *  入口参数：gx,gy,gz,陀螺仪XYZ轴原始数据（带符号）
    

  19.   *  返回值：0，成功；1，失败
    

  20.   */
    

  21. u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
    

  22. {
    

  23.         u8 buf[6],res;
    

  24.         res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
    

  25.         if(res == 0)
    

  26.         {
    

  27.                 *gx=((u16)buf[0]<<8)|buf[1];
    

  28.                 *gy=((u16)buf[2]<<8)|buf[3];
    

  29.                 *gz=((u16)buf[4]<<8)|buf[5];
    

  30.         }
    

  31.         return res;
    

  32. }
    

  33. /**  
    

  34.   *  功能：获得加速度原始值
    

  35.   *  入口参数：ax,ay,az,加速度计XYZ的原始数据（）
    

  36.   *  返回值：0，正常；1，错误
    

  37.   */
    

  38. u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
    

  39. {
    

  40.         u8 buf[6],res;
    

  41.         res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
    

  42.         if(res ==0)
    

  43.         {
    

  44.                 *ax=((u16)buf[0]<<8)|buf[1];
    

  45.                 *ay=((u16)buf[2]<<8)|buf[3];
    

  46.                 *az=((u16)buf[4]<<8)|buf[5];
    

  47.         }
    

  48.         return res;
    

  49. }
回复 赏

收藏 举报

 楼主| 和下土 发表于 2022-5-28 20:08 | 显示全部楼层
4、DMP库函数调用
复制
  1. //mpu6050,dmp初始化
    

  2. //返回值:0,正常
    

  3. //    其他,失败
    

  4. u8 mpu_dmp_init(void)
    

  5. {
    

  6.         u8 res=0;
    

  7.         MPU_IIC_Init();         //初始化IIC总线
    

  8.         if(mpu_init()==0)        //初始化MPU6050
    

  9.         {         
    

  10.                 res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器
    

  11.                 if(res)return 1; 
    

  12.                 res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置FIFO
    

  13.                 if(res)return 2; 
    

  14.                 res=mpu_set_sample_rate(DEFAULT_MPU_HZ);        //设置采样率
    

  15.                 if(res)return 3; 
    

  16.                 res=dmp_load_motion_driver_firmware();                //加载dmp固件
    

  17.                 if(res)return 4; 
    

  18.                 res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
    

  19.                 if(res)return 5; 
    

  20.                 res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP|        //设置dmp功能
    

  21.                     DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
    

  22.                     DMP_FEATURE_GYRO_CAL);
    

  23.                 if(res)return 6; 
    

  24.                 res=dmp_set_fifo_rate(DEFAULT_MPU_HZ);        //设置DMP输出速率(最大不超过200Hz)
    

  25.                 if(res)return 7;   
    

  26.                 res=run_self_test();                //自检
    

  27.                 if(res)return 8;    
    

  28.                 res=mpu_set_dmp_state(1);        //使能DMP
    

  29.                 if(res)return 9;     
    

  30.         }else return 10;
    

  31.         return 0;
    

  32. }
    

  33. //得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
    

  34. //pitch:俯仰角 精度:0.1°   范围:-90.0° <---> +90.0°
    

  35. //roll:横滚角  精度:0.1°   范围:-180.0°<---> +180.0°
    

  36. //yaw:航向角   精度:0.1°   范围:-180.0°<---> +180.0°
    

  37. //返回值:0,正常
    

  38. //    其他,失败
    

  39. u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
    

  40. {
    

  41.         float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
    

  42.         unsigned long sensor_timestamp;
    

  43.         short gyro[3], accel[3], sensors;
    

  44.         unsigned char more;
    

  45.         long quat[4]; 
    

  46.         if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;         
    

  47.         /* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
    

  48.          * This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
    

  49.         **/
    

  50.         /*if (sensors & INV_XYZ_GYRO )
    

  51.         send_packet(PACKET_TYPE_GYRO, gyro);
    

  52.         if (sensors & INV_XYZ_ACCEL)
    

  53.         send_packet(PACKET_TYPE_ACCEL, accel); */
    

  54.         /* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
    

  55.          * The orientation is set by the scalar passed to dmp_set_orientation during initialization. 
    

  56.         **/
    

  57.         if(sensors&INV_WXYZ_QUAT) 
    

  58.         {
    

  59.                 q0 = quat[0] / q30;        //q30格式转换为浮点数
    

  60.                 q1 = quat[1] / q30;
    

  61.                 q2 = quat[2] / q30;
    

  62.                 q3 = quat[3] / q30; 
    

  63.                 //计算得到俯仰角/横滚角/航向角
    

  64.                 *pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;        // pitch
    

  65.                 *roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;        // roll
    

  66.                 *yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;        //yaw
    

  67.         }else return 2;
    

  68.         return 0;
    

  69. }
回复 赏

收藏 举报

 楼主| 和下土 发表于 2022-5-28 20:10 | 显示全部楼层
6、实验现象
回复 赏

收藏 举报

 楼主| 和下土 发表于 2022-5-28 20:11 | 显示全部楼层
29630629211425de5c.png
回复 赏

收藏 举报

 楼主| 和下土 发表于 2022-5-28 20:12 | 显示全部楼层
7、总结
对于MPU6050数据的处理，使用DMP库是一种比较简单的办法，如果理论扎实，可以自行读取原始数据，通过四元数解算获取欧拉角。下一次我们将会把陀螺仪得到的数据发送到上位机，下次主要分析上位机的通讯协议。
回复 赏

收藏 举报

elsaflower 发表于 2022-7-8 10:29 | 显示全部楼层
有计步的功能吗   
回复 赏

收藏 举报

beacherblack 发表于 2022-7-8 16:23 | 显示全部楼层
MPU6050的功能还是比较强大的。   
回复 赏

收藏 举报

mickit 发表于 2022-7-9 12:55 | 显示全部楼层
可以拓展hmc5883吗   
回复 赏

收藏 举报

mattlincoln 发表于 2022-7-9 13:27 | 显示全部楼层
DMP库处理MPU6050速度还可以。   
回复 赏

收藏 举报

fengm 发表于 2022-7-9 14:20 | 显示全部楼层
可以使用ahrs吗   
回复 赏

收藏 举报

phoenixwhite 发表于 2022-7-9 15:20 | 显示全部楼层
硬件iic吗   
回复 赏

收藏 举报

janewood 发表于 2022-7-9 18:16 | 显示全部楼层
为什么不使用四元数计算呢   
回复 赏

收藏 举报

youtome 发表于 2022-7-9 20:13 | 显示全部楼层
这个iic函数呢？   
回复 赏

收藏 举报

happy_10 发表于 2022-7-10 14:00 | 显示全部楼层
有计步的功能吗   
回复 赏

收藏 举报

xxrs 发表于 2022-7-10 14:02 | 显示全部楼层
MPU6050的功能还是比较强大的。   
回复 赏

收藏 举报

stly 发表于 2022-7-10 14:04 | 显示全部楼层
可以拓展hmc5883吗   
回复 赏

收藏 举报

llljh 发表于 2022-7-10 14:06 | 显示全部楼层
DMP库处理MPU6050速度还可以。   
回复 赏

收藏 举报

llljh 发表于 2022-7-10 14:50 | 显示全部楼层
可以使用ahrs吗   
回复 赏

收藏 举报

下一页 »
1234下一页
返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 注册

本版积分规则

快速回复 在线客服 返回列表 返回顶部