[资源共享] 移植MPU6050-DMP库实现姿态角PRY解算

[复制链接]
2522|36
 楼主| 回复就哭哭 发表于 2022-6-30 13:58 | 显示全部楼层
mpu_dmp_get_data 函数代码如下:
  1. //得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
  2. //pitch:俯仰角 精度:0.1°   范围:-90.0° <---> +90.0°
  3. //roll:横滚角  精度:0.1°   范围:-180.0°<---> +180.0°
  4. //yaw:航向角   精度:0.1°   范围:-180.0°<---> +180.0°
  5. //返回值:0,正常
  6. //    其他,失败
  7. u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
  8. {
  9.         float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
  10.         unsigned long sensor_timestamp;
  11.         short gyro[3], accel[3], sensors;
  12.         unsigned char more;
  13.         long quat[4];
  14.         if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;         
  15.         /* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
  16.          * This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
  17.         **/
  18.         /*if (sensors & INV_XYZ_GYRO )
  19.         send_packet(PACKET_TYPE_GYRO, gyro);
  20.         if (sensors & INV_XYZ_ACCEL)
  21.         send_packet(PACKET_TYPE_ACCEL, accel); */
  22.         /* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
  23.          * The orientation is set by the scalar passed to dmp_set_orientation during initialization.
  24.         **/
  25.         if(sensors&INV_WXYZ_QUAT)
  26.         {
  27.                 q0 = quat[0] / q30;        //q30格式转换为浮点数
  28.                 q1 = quat[1] / q30;
  29.                 q2 = quat[2] / q30;
  30.                 q3 = quat[3] / q30;
  31.                 //计算得到俯仰角/横滚角/航向角
  32.                 *pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;        // pitch
  33.                 *roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;        // roll
  34.                 *yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;        //yaw
  35.         }else return 2;
  36.         return 0;
  37. }
 楼主| 回复就哭哭 发表于 2022-6-30 13:59 | 显示全部楼层
这里就用到了我们前面介绍的四元数转欧拉角公式,将 dmp_read_fifo 函数读到的 q30 格式四元数转换成欧拉角。

其中 quat[0]~ quat[3]是 MPU6050 的 DMP 解算后的四元数, q30 格式,所以要除以一个2 的 30 次方,其中 q30 是一个常量: 1073741824,即 2 的 30 次方,然后带入公式,计算出欧拉角。

上述计算公式的 57.3 是弧度转换为角度,即 180/π,这样得到的结果就是以度(°)为单位的。

关于四元数与欧拉角的公式推导,这里我们不进行讲解,感兴趣的朋友,可以自行查阅相关资料学习。

此函数用于得到 DMP 姿态解算后的俯仰角、横滚角和航向角。
 楼主| 回复就哭哭 发表于 2022-6-30 14:02 | 显示全部楼层
  1. #include <stdio.h>
  2. #include <stdint.h>
  3. #include <stdlib.h>
  4. #include <string.h>
  5. #include <math.h>
  6. #include "inv_mpu.h"
  7. #include "inv_mpu_dmp_motion_driver.h"
  8. #include "dmpKey.h"
  9. #include "dmpmap.h"

  10. #include "SEEKFREE_IIC.h"
  11. #include "zf_systick.h"

  12. //定义目标板采用MSP430
  13. #define  MOTION_DRIVER_TARGET_MSP430

  14. /* The following functions must be defined for this platform:
  15. * i2c_write(unsigned char slave_addr, unsigned char reg_addr,
  16. *      unsigned char length, unsigned char const *data)
  17. * i2c_read(unsigned char slave_addr, unsigned char reg_addr,
  18. *      unsigned char length, unsigned char *data)
  19. * delay_ms(unsigned long num_ms)
  20. * get_ms(unsigned long *count)
  21. */
  22. #if defined MOTION_DRIVER_TARGET_MSP430
  23. //#include "msp430.h"
  24. //#include "msp430_clock.h"
  25. #define delay_ms    systick_delay_ms
  26. #define get_ms      mget_ms
  27. #define log_i                 printf
  28. #define log_e                  printf
 楼主| 回复就哭哭 发表于 2022-6-30 14:03 | 显示全部楼层
4、 inv_mpu_dmp_motion_driver.c移植
这个文件只需要修改头文件调用和函数重新定义,不需要修改函数。
 楼主| 回复就哭哭 发表于 2022-6-30 14:03 | 显示全部楼层
三、MM32程序编写
  1. 软件 IIC 引脚定义连接
  2. #define SEEKFREE_SCL        B13                                                                                                                        // 定义SCL引脚
  3. #define SEEKFREE_SDA        B15                                                                                                                        // 定义SDA引脚  
 楼主| 回复就哭哭 发表于 2022-6-30 14:06 | 显示全部楼层
1、MPU6050初始化
  1. //-------------------------------------------------------------------------------------------------------------------
  2. // @brief                初始化MPU6050
  3. // @param                NULL
  4. // @return                void
  5. //-------------------------------------------------------------------------------------------------------------------
  6. void mpu6050_init(void)
  7. {
  8.         simiic_init();                                                                                                                        //模拟IIC的初始化
  9.         systick_delay_ms(100);                                                                                                        // 上电延时

  10.         mpu6050_self1_check();                                                                                                        //上电自检
  11.         simiic_write_reg(MPU6050_DEV_ADDR, PWR_MGMT_1, 0x00);                                        // 解除休眠状态
  12.         simiic_write_reg(MPU6050_DEV_ADDR, SMPLRT_DIV, 0x07);                                        // 125HZ采样率
  13.         simiic_write_reg(MPU6050_DEV_ADDR, MPU6050_CONFIG, 0x04);                                //
  14.         simiic_write_reg(MPU6050_DEV_ADDR, GYRO_CONFIG, 0x18);                                        // 2000
  15.         simiic_write_reg(MPU6050_DEV_ADDR, ACCEL_CONFIG, 0x10);                                        // 8g
  16.         simiic_write_reg(MPU6050_DEV_ADDR, User_Control, 0x00);
  17.         simiic_write_reg(MPU6050_DEV_ADDR, INT_PIN_CFG, 0x02);
  18. }
 楼主| 回复就哭哭 发表于 2022-6-30 14:07 | 显示全部楼层
2、主函数编写
1、引入头文件
2、调用mpu6050初始化
3、dmp初始化
4、得到加速度值
5、解算欧拉角
6、屏幕显示
  1. #include "headfile.h"
  2. #include "inv_mpu.h"
  3. #include "inv_mpu_dmp_motion_driver.h"
  4. // **************************** 代码区域 ****************************

  5. int main(void)
  6. {
  7.         float pitch,roll,yaw;                 //欧拉角
  8.         mpu6050_init();
  9.         while(mpu_dmp_init()!=0)
  10.         {
  11.                 LCD_ShowString(0,0,"dmp-error",MAGENTA);
  12.         }
  13.         LCD_ShowString(0,0,"dmp-init-ok",MAGENTA);

  14.         while(1)
  15.         {
  16.                 get_gyro();
  17.                 mpu_dmp_get_data(&pitch,&roll,&yaw);

  18.                 LCD_ShowFloat(0,40,pitch,5,BLACK);
  19.                 LCD_ShowFloat(0,60,roll,5,BLACK);
  20.                 LCD_ShowFloat(0,80,yaw,5,BLACK
  21.         }
  22. }
 楼主| 回复就哭哭 发表于 2022-6-30 14:08 | 显示全部楼层
四、工程源码
最后工程文件我将放置在百度网盘链接: https://pan.baidu.com/s/1QRNr87n38MjN-yKf30Uiww提取码:jr2t

不足之处多多指教,工程的dmp我是根据正点原子的dmp库移植而来。
tpgf 发表于 2022-7-5 14:51 | 显示全部楼层
PRY解算如何解释呀
labasi 发表于 2022-7-5 15:00 | 显示全部楼层
mpu_init 函数来初始化哪一部分啊
paotangsan 发表于 2022-7-5 15:09 | 显示全部楼层
传感器的精度一般需要达到多少啊
renzheshengui 发表于 2022-7-5 15:15 | 显示全部楼层
姿态解算需要什么参数呢
wakayi 发表于 2022-7-5 15:25 | 显示全部楼层
需要更改的东西多吗
wowu 发表于 2022-7-5 15:32 | 显示全部楼层
可以采集什么种类的模拟信号啊
chenqianqian 发表于 2022-7-6 08:08 来自手机 | 显示全部楼层
谢谢分享,学习一下。
daichaodai 发表于 2022-7-7 08:33 来自手机 | 显示全部楼层
学习了,谢谢分享。
您需要登录后才可以回帖 登录 | 注册

本版积分规则

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