MPU6050内部DMP固件移植，获取欧拉角

只看该作者 · 2022-9-27 16:07

其实我们主要是想通过MPU6050得到欧拉角和四元数（可以用算法实现，但是比较复杂）
要通过MPU6050得到四元数和欧拉角，这个过程有两种方法，一种是用原始数据（三轴加速度，三轴角速度），通过一些（卡尔曼滤波、积分运算、减少误差零点漂移等）姿态融合算法转化即可
另一种是直接用MPU6050内部的自带的数字运动处理器（即DMP），我们要用这个DMP功能的话，就要实现加载固件
那么DMP怎么拿呢，Invensense公司有提供了一个MPU6050的嵌入式运动驱动库，进而我们可以通过这个库移植很方便得出欧拉角
不过Invensense公司提供的MPU6050运动驱动库是基于MSP430芯片的，需要将其移植到其他芯片上，（比如STM32F1系列），当然就要移植修改代码了。

只看该作者 · 2022-9-27 16:33

移植

官方原版驱动下载地址：

http://drivers.softpedia.com/get/Other-DRIVERS-TOOLS/Others/InvenSense-Embedded-Motion-Tracker-Driver-51.shtml

只看该作者 · 2022-9-27 22:47

下载后分析一下

我们移植需要准备的四个东西：
1.MPU6050硬件
2.MPU6050寄存器手册
3.STM32F10X单片机
4.DMP固件库（PS:固件库其实只需要提取下面7个文件即可，其他文件没啥用）

只看该作者 · 2022-9-27 22:47

复制

“inv_mpu_dmp_motion_driver.h”

 “inv_mpu_dmp_motion_driver.c”

 “inv_mpu.h”

 “inv_mpu.c”

 “dmpmap.h”

 “dmpKey.h”（前6个在同一个目录下） 和“motion_driver_test.c“

只看该作者 · 2022-9-27 22:48

把前6个文件通过keil导入文件的方式，导入的keil中。第7个文件不是代码文件，而是适用于msp430的main函数文件，我们把第七个文件当作参照，来自己写一份关于stm32的main文件即可.

只看该作者 · 2022-9-27 23:06

2、观察它文件内容的结构
1.整体文件分析

（1）在”inv_mpu.c“和”inv_mpu_dmp_motion_driver.c“ 这两个文件里面全是大片大片的英文，但是仔细看会发现，这么多的英文，其实结构很清晰，都是单个的模块的函数内容定义的集合以及一些条件预定义而已，并不是什么杂乱无章的东西。打开”motion_driver_test.c“，你也会发现也是大量的函数定义以及条件预定义和一个main函数，而这个main函数是针对MSP430的一个代码，刚好是用于读取实时的欧拉角代码，首先我们对main函数进行分析，我们知道一个main函数一般分为两部分，各个内外设的初始化，和while循环内的无限操作函数，对于我们的要求也是一样的，我们无非也就是需要两个代码，MPU6050的初始化，以及无限的读出欧拉角，刚好和main函数对应上，所以开始分析代码，因为main上自带英文注释，我们先把它通过谷歌翻译

只看该作者 · 2022-9-27 23:07

2、main文件分析

      1、初始化：

            {

                     1、初始化MSP430芯片

                     2、初始化MPU6050

                     3、检测返回值，如果返回值>0，重启MSP430

                     4、开启陀螺仪和加速计

                     5、加载MPU6050内部的陀螺仪和加速计的FIFO-根据内部参数配置

                     6、设置MPU6050内部采样率

                     7、返回刚刚配置的内部的一系列的值

                     8、数组置数函数-c语言特有，将hal数组内的所有值都置为0

                     9、Hal.sensors = ACCEL_ON | GYRO_ON,标志加速计和陀螺仪都打开了

                     10、上报四元数

                     11、加载DMP功能

                     12、将陀螺仪和加速度方向矩阵推入DMP

                     13、手势回调函数-和我们的意图无关

                     14、安卓回调函数-和我们的意图无关

                     15、选择DMP要加载的功能-和这些传入参数的值有关

                     16、加载已选功能

                     17、设置FIFO缓存的速率

                     18、开启DMP

                     19、hal.dmp_on =1标志-DMP开启

                     20、开启MSP430的中断}

只看该作者 · 2022-9-27 23:08

2、while(1){

            1、USB相关函数和上位机有关，和我们的意图无关

            2、获得时间戳=当前的时间，和我们的意图无关

            3、运动中断函数，和我们的意图无关

            4、控制MSP430芯片休眠函数，和我们的意图无关

            5、查询DMP开了没有和是否有数据更新，（两个都要满足）、

                     1、向DMP的FIFO读取并且计算四元数

                     2、再检测FIFO内是否还存在有剩余的数据-如果没有，数据更新标志位清0

                     3、返回四元数}

只看该作者 · 2022-9-27 23:09

3、翻译”inv_mpu.c“和”motion_driver_test.c”的函数大概内容

1、“inv_mpu.c”

复制

1.static int set_int_enable(unsigned char enable)模块中断使能函数

2.int mpu_reg_dump(void)测试打印函数

3.int mpu_read_reg(unsigned char reg, unsigned char *data)向芯片读寄存器值，除了MEMORY和FIFO

4.int mpu_init(struct int_param_s *int_param) MPU6050的初始化

5.int mpu_lp_accel_mode(unsigned char rate) 进入低功耗模式

6.int mpu_get_gyro_reg(short *data, unsigned long *timestamp) 获取新的原始陀螺仪数据

7.int mpu_get_accel_reg(short *data, unsigned long *timestamp获取新的原始加速度数据

8.int mpu_get_temperature(long *data, unsigned long *timestamp) 获取新的温度数据

9.int mpu_set_accel_bias(const long *accel_bias) 偏差配置函数

10.int mpu_reset_fifo(void) 重置FIFO函数

11.int mpu_get_gyro_fsr(unsigned short *fsr) 获得陀螺仪全尺寸范围函数

12.int mpu_set_gyro_fsr(unsigned short fsr) 设置陀螺仪全尺寸范围函数

13.int mpu_get_accel_fsr(unsigned char *fsr) 获得加速度全尺寸范围函数

14.int mpu_set_accel_fsr(unsigned char fsr) 配置加速度全尺寸范围函数

15.int mpu_get_lpf(unsigned short *lpf) .获得DLPF范围函数

16.int mpu_set_lpf(unsigned short lpf) 配置DLPF范围函数

17.int mpu_get_sample_rate(unsigned short *rate) 获得采样频率范围函数

18.int mpu_set_sample_rate(unsigned short rate) 配置采样频率范围函数

19.int mpu_get_compass_sample_rate(unsigned short *rate) 获得罗盘采样频率范围函数

20.int mpu_set_compass_sample_rate(unsigned short rate) 配置罗盘采样频率范围函数

21.int mpu_get_gyro_sens(float *sens) 获得陀螺仪灵敏度比例因子函数

22.int mpu_get_accel_sens(unsigned short *sens) 获得加速计灵敏度比例因子函数

23.int mpu_get_fifo_config(unsigned char *sensors) 获得开启的FIFO通道函数

24.int mpu_configure_fifo(unsigned char sensors) 配置开启FIFO通道函数

25.int mpu_get_power_state(unsigned char *power_on) 获得芯片工作状态

26.int mpu_set_sensors(unsigned char sensors) 配置传感器的时钟和工作状态函数

27.int mpu_get_int_status(short *status).获得中断状态函数

28.int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp,unsigned char *sensors, unsigned char *more) 获得FIFO数据函数

29.int mpu_read_fifo_stream(unsigned short length, unsigned char *data,unsigned char *more) 获得FIFO数据长度函数

30.int mpu_set_bypass(unsigned char bypass_on) 设置旁路模式函数

31.int mpu_set_int_level(unsigned char active_low) 设置中断优先级函数

32.int mpu_set_int_latched(unsigned char enable) 设置中断锁存函数-

33.设置自检函数

34.static int get_st_biases(long *gyro, long *accel, unsigned char hw_test) 获取所有的偏差值函数

35.int mpu_run_self_test(long *gyro, long *accel) 行自检值函数

36.int mpu_write_mem(unsigned short mem_addr, unsigned short length,unsigned char *data) 向DMP写**函数

37.int mpu_read_mem(unsigned short mem_addr, unsigned short length,unsigned char *data) 向DMP读**函数

38.int mpu_load_firmware(unsigned short length, const unsigned char *firmware,unsigned short start_addr, unsigned short sample_rate) 加载并验证DMP映像函数

39.int mpu_set_dmp_state(unsigned char enable) DMP状态控制函数

40.int mpu_get_dmp_state(unsigned char *enabled) DMP状态读取函数

只看该作者 · 2022-9-27 23:10

4、根据“motion_driver_test.c”创建兼容STM32F10X的代码

            1、DMP初始化函数
u8 DMP_Init(void)
{
int result;
unsigned char accel_fsr;
unsigned short gyro_rate, gyro_fsr;
unsigned long timestamp;
result = mpu_init();                                  //1.6050初始化,成功=0，失败=1

   if (result)
   {
   STM32F103_Reset();                               //2.如果失败，重新复位stm32

   return 8;
   }

if(mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))    //3.配置陀螺仪和加速计传感器的时钟和工作状态函数
{return 1;}

if(mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL))  //4.配置陀螺仪和加速计开启FIFO通道函数
{return 2;}

if(mpu_set_sample_rate(DEFAULT_MPU_HZ))             //5.配置默认的采样率
{return 3;}

mpu_get_sample_rate(&gyro_rate);                   //6.获得陀螺仪采样频率范围函数

mpu_get_gyro_fsr(&gyro_fsr);                         //7.获得陀螺仪全尺寸范围函数

mpu_get_accel_fsr(&accel_fsr);                      //8.获得加速计全尺寸范围函数

memset(&hal, 0, sizeof(hal));                      //9.数组填数函数

hal.sensors = ACCEL_ON | GYRO_ON;                   //10.标志位-"开启传感器"设置为加速计和陀螺仪

if(dmp_load_motion_driver_firmware())                //11.加载并验证DMP映像函数
   {return 4;}

  if(dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) //12.推送陀螺仪和<a  target="_blank">加速度计</a>的方向矩阵到DMP
{return 5;}

  hal.dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
  DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
  DMP_FEATURE_GYRO_CAL;                                                          //13.DMP的功能选项标志位设置，用来告诉DMP要开启的功能

  if(dmp_enable_feature(hal.dmp_features))             //14.使能上述功能
{return 6;}

  if(dmp_set_fifo_rate(DEFAULT_MPU_HZ))                //15.配置DMP的FIFO速率
{return 7;}

  mpu_set_dmp_state(1);                               //16.开启DMP

  hal.dmp_on = 1;                                     //17.标志位-"DMP状态"为开启

   run_self_test();                                     //18.DMP的自检，就是标定现在的状态为坐标原点

return 0;

只看该作者 · 2022-9-27 23:11

只看该作者 · 2022-9-27 23:12

2、DMP欧拉角获取
short gyro[3], accel[3], sensors;
unsigned char more;
long quat[4];
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
float pitch,roll,yaw;
   u8 DMP_DATA_UPDATA(void)
{
unsigned long sensor_timestamp;
gyro_data_ready_cb();                //数据采集结束标志位

if (hal.new_gyro && hal.dmp_on)       //用于计算四元数函数
{
if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))
{return 1;}

if (!more)
hal.new_gyro = 0;
if(sensors & INV_WXYZ_QUAT)
{
q0=quat[0]/q30;
q1=quat[1]/q30;
q2=quat[2]/q30;
q3=quat[3]/q30;
pitch=asin((-2)*q1*q3+2*q0*q2)*57.3;
roll=atan2(2*q2*q3+2*q0*q1,(-2)*q1*q1-2*q2*q2+1)*57.3;
yaw=atan2(2*(q1*q2+q0*q3),q0*q0+q1*q1-q2*q2-q3*q3)*57.3;
}
}

return 0;

只看该作者 · 2022-9-27 23:13

只看该作者 · 2022-9-27 23:24

注意事项：
1.
模拟i2c的内容:
附件内有模拟I2C的代码，因为i2c内的延迟函数是通过while（time–）实现的，所以延迟长度可以通过time的值控制，为什么要考虑这个呢，因为在测试的时候发现单片机会在“u8 DMP_Init(void) “函数中的 if(dmp_load_motion_driver_firmware()) /11.加载并验证DMP映像函数等待要很久，而这个函数的本质就是不断的通过I2C向6050收发数据，而我当时的I2C代码因为怕时钟线跳变时有延迟，所以给他每次跳变都加了很大的延迟，所以导致对于这个6050的这个函数也会延迟恒久，但是当我把软件延迟的变量变小，发现一下这个函数就通过了

只看该作者 · 2022-9-27 23:24

关于mpu6050返回四元数的途径:
我们知道获取外部的数据有两种方式，一种是单片机一直通过询问标志位的方式去获得外部数据，另外一种是外部给单片机引脚一个电平跳变方式，来告诉单片机读取数据，也就是外部中断，而且第二中种效率高，但是对于6050，如果设置的dmp，他的默认中断触发是50us的高电平(应该是的)，us对于单片机来说可能会出bug，所以选择巡查的方式来读出数据，所以在测试中，我用的是滴答定时器在固定的时间间隔来读取数据，因为6050产出的数据速度远远大于每次单片机去读取的时间，所以不用担心会读出空数据，但是，会有一个问题，就是如果单片机的读取时间过慢，比如说1s读一次，那么就会死机，这个好像是因为fifo的会阻塞问题把。具体的不清楚，