SMT32学习笔记--MPU6050的DMP
DMP
DMP就是对三轴加速度计和三轴陀螺仪的ADC数据进行融合计算，得出四元数，再利用四元数计算姿态角。想要实现DMP一般有2种方式：硬件DMP和软件DMP。
硬件DMP
MPU6050芯片内部有一个DMP模块，该模块能够利用三轴加速度计和三轴陀螺仪的ADC数据，计算四元数，而用户只需要读取四元数，并转换成姿态角就可以了。这样的方式节省了主控芯片的资源，比较可靠、方便。但是MPU6050内部的DMP模块是没有对外公开的，既代码不开源，官方给的代码是针对STM8 MSP430芯片的，幸运的是网上大佬比较多，我们有机会把官方代码移植到STM32中。
以下是移植库代码教程。
》》》》”小马哥STM32课程系列直播-如何两个月做出自己的暴力空心杯小四轴”
链接：http://www.moore8.com/courses/1406
后面的移植笔记不是本人的，是在网上找到的，放到这里是方便随时查看。
原文链接：https://blog.csdn.net/qq646497210/article/details/83096361
记录一下MPU6050的dmp代码移植的过程
1、准备四个东西
1）mpu6050硬件
2）mpu6050寄存器手册，
3）stm32f10x单片机
4）dmp固件库(我的是5.1版的)
PS:固件库只需要提取下面7个文件即可，其他文件没啥用
“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“
把前6个文件通过keil导入文件的方式，导入的keil中。第7个文件不是代码文件，而是适用于msp430的main函数文件，我们把第七个文件当作参照，来自己写一份关于stm32的main文件即可.
2、观察它文件内容的结构
1）整体文件分析
在”inv_mpu.c” “inv_mpu_dmp_motion_driver.c”这两个文件里面全是大片大片的英文，但是仔细看，你会发现，这么多的英文，其实结构很清晰，都是单个的模块的函数内容定义的集合以及一些条件预定义而已，并不是什么杂乱无章的东西，。打开“motion_driver_test.c“，你也会发现也是大量的函数定义以及条件预定义和一个main函数，而这个main函数是针对MSP430的一个代码，刚好是用于读取实时的欧拉角代码，首先我们对main函数进行分析，我们知道一个main函数一般分为两部分，各个内外设的初始化，和while循环内的无限操作函数，对于我们的要求也是也一样，我们无非也就是需要两个代码，mpu6050的初始化，以及无限的读出欧拉角，刚好和main函数对应上了，所以开始分析代码，因为main上自带英文注释，咱先把它通过谷歌翻译在”inv_mpu.c” “inv_mpu_dmp_motion_driver.c”这两个文件里面全是大片大片的英文，但是仔细看，你会发现，这么多的英文，其实结构很清晰，都是单个的模块的函数内容定义的集合以及一些条件预定义而已，并不是什么杂乱无章的东西，。打开“motion_driver_test.c“，你也会发现也是大量的函数定义以及条件预定义和一个main函数，而这个main函数是针对MSP430的一个代码，刚好是用于读取实时的欧拉角代码，首先我们对main函数进行分析，我们知道一个main函数一般分为两部分，各个内外设的初始化，和while循环内的无限操作函数，对于我们的要求也是也一样，我们无非也就是需要两个代码，mpu6050的初始化，以及无限的读出欧拉角，刚好和main函数对应上了，所以开始分析代码，因为main上自带英文注释，咱先把它通过谷歌翻译
2）main文件分析
初始化：{
- 初始化MSP430芯片
- 初始化mpu6050
- 检测返回值，如果返回值>0,重启MSP430
- 开启陀螺仪和加速计
- 加载mpu6050内部的陀螺仪和加速计的FIFO-根据内部参数配置
- 设置mpu6050内部采样率
- 返回刚刚配置的内部的一系列的值
- 数组置数函数-c语言特有，将hal数组内的所有值都置为0
- Hal.sesors=ACCEL_ON | GYRO_ON，标志-加速计和陀螺仪都开了
- 上报四元数
- 加载DMP功能
- 将陀螺仪和加速度方向矩阵推入DMP
- 手势回调函数-和我们的意图无关手势回调函数-和我们的意图无关
- 安卓回调函数-和我们的意图无关
- 选择dmp要加载的功能-和这些传入参数的值有关
- 加载已选功能
- 设置FIFO缓存的速率
- 开启DMP
- hal.dmp_on = 1 标志-DMP开启
- 开启MPS430的中断
}
While(1){
1.USB相关函数-和上位机有关，和我们的意图无关-
2.获得时间戳=当时的时间，和我们的意图无关-
3.运动中断函数，和我们的意图无关-
4.控制MSP430芯片休眠函数，和我们的意图无关-
5.查询DMP开了没和是否有数据更新了(两个都要满足)-
6.向DMP的fifo读取并且计算四元数
7.再检测FIFO内是否还存在有剩余的数据-如果没有，数据更新标志位清零
8.返回四元数
}
3、翻译”inv_mpu.c”和“motion_driver_test.c“的函数大概内容
1、“inv_mpu.c”
static int set_int_enable(unsigned char enable) 模块中断使能函数
int mpu_reg_dump(void) 测试打印函数
int mpu_read_reg(unsigned char reg, unsigned char *data) 3.向芯片读寄存器值，除了MEMERY和FIFO
int mpu_init(void) MPU6050的初始化
int mpu_lp_accel_mode(unsigned char rate) 进入低功耗模式
int mpu_get_gyro_reg(short *data, unsigned long *timestamp) 获取新的原始陀螺仪数据
int mpu_get_accel_reg(short *data, unsigned long *timestamp获取新的原始加速度数据
int mpu_get_temperature(long *data, unsigned long *timestamp) 获取新的温度数据
int mpu_set_accel_bias(const long *accel_bias) 偏差配置函数
int mpu_reset_fifo(void) 重置FIFO函数
int mpu_get_gyro_fsr(unsigned short *fsr) 获得陀螺仪全尺寸范围函数
int mpu_set_gyro_fsr(unsigned short fsr) 设置陀螺仪全尺寸范围函数
int mpu_get_accel_fsr(unsigned char *fsr) 获得加速度全尺寸范围函数
int mpu_set_accel_fsr(unsigned char fsr) 配置加速度全尺寸范围函数
int mpu_get_lpf(unsigned short *lpf) .获得DLPF范围函数
int mpu_set_lpf(unsigned short lpf) 配置DLPF范围函数
int mpu_get_sample_rate(unsigned short *rate) 获得采样频率范围函数
int mpu_set_sample_rate(unsigned short rate) 配置采样频率范围函数
int mpu_get_compass_sample_rate(unsigned short *rate) 获得罗盘采样频率范围函数
int mpu_set_compass_sample_rate(unsigned short rate) 配置罗盘采样频率范围函数
int mpu_get_gyro_sens(float *sens) 获得陀螺仪灵敏度比例因子函数
int mpu_get_accel_sens(unsigned short *sens) 获得加速计灵敏度比例因子函数
int mpu_get_fifo_config(unsigned char *sensors) 获得开启的FIFO通道函数
int mpu_configure_fifo(unsigned char sensors) 配置开启FIFO通道函数
int mpu_get_power_state(unsigned char *power_on) 获得芯片工作状态
int mpu_set_sensors(unsigned char sensors) 配置传感器的时钟和工作状态函数
int mpu_get_int_status(short *status).获得中断状态函数
int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp,unsigned char *sensors, unsigned char *more) 获得FIFO数据函数
int mpu_read_fifo_stream(unsigned short length, unsigned char *data,unsigned char *more) 获得FIFO数据长度函数
int mpu_set_bypass(unsigned char bypass_on) 设置旁路模式函数
int mpu_set_int_level(unsigned char active_low) 设置中断优先级函数
int mpu_set_int_latched(unsigned char enable) 设置中断锁存函数-
设置自检函数
static int get_st_biases(long *gyro, long *accel, unsigned char hw_test) 获取所有的偏差值函数
int mpu_run_self_test(long *gyro, long *accel) 行自检值函数
int mpu_write_mem(unsigned short mem_addr, unsigned short length,unsigned char *data) 向DMP写记忆函数
int mpu_read_mem(unsigned short mem_addr, unsigned short length,unsigned char *data) 向DMP读记忆函数
int mpu_load_firmware(unsigned short length, const unsigned char *firmware,unsigned short start_addr, unsigned short sample_rate) 加载并验证DMP映像函数
int mpu_set_dmp_state(unsigned char enable) DMP状态控制函数
int mpu_get_dmp_state(unsigned char *enabled) DMP状态读取函数
2、“motion_driver_test.c“
static __inline unsigned short inv_row_2_scale(const signed char row) 矩阵转换方向标量函数
static void setup_gyro(void) 陀螺仪配置函数
void STM32F103_Reset(void) 外部控制芯片复位函数，我们是stm32，所以重写
static __inline void run_self_test(void) 自检函数，用于坐标原点标定
static void gyro_data_ready_cb(void) 数据获得后的状态回调函数
4、”inv_mpu.c”和“inv_mpu_dmp_motion_driver.c“的文件格式的修改
5、根据“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.推送陀螺仪和加速度计的方向矩阵到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;
}
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;
}
6、注意事项：
模拟i2c的内容:
附件内有模拟I2C的代码，因为i2c内的延迟函数是通过while（time–）实现的，所以延迟长度可以通过time的值控制，为什么要考虑这个呢，因为在测试的时候发现单片机会 在“u8 DMP_Init(void) “函数中的 if(dmp_load_motion_driver_firmware()) /11.加载并验证DMP映像函数等待要很久，而这个函数的本质就是不断的通过I2C向6050收发数据，而我当时的I2C代码因为怕时钟线跳变时有延迟，所以给他每次跳变都加了很大的延迟，所以导致对于这个6050的这个函数也会延迟恒久，但是当我把软件延迟的变量变小，发现一下这个函数就通过了
关于mpu6050返回四元数的途径:
我们知道获取外部的数据有两种方式，一种是单片机一直通过询问标志位的方式去获得外部数据，另外一种是外部给单片机引脚一个电平跳变方式，来告诉单片机读取数据，也就是外部中断，而且第二中种效率高，但是对于6050，如果设置的dmp，他的默认中断触发是50us的高电平(应该是的)，us对于单片机来说可能会出bug，所以选择巡查的方式来读出数据，所以在测试中，我用的是滴答定时器在固定的时间间隔来读取数据，因为6050产出的数据速度远远大于每次单片机去读取的时间，所以不用担心会读出空数据，但是，会有一个问题，就是如果单片机的读取时间过慢，比如说1s读一次，那么就会死机，这个好像是因为fifo的会阻塞问题把。具体的不清楚，
编译结束时，warning和error都要关注:
有时候warning也会让程序出bug
Keil特有的格式:
c99的结构体无法识别，要把它该成GUI的结构体
static _inline 关键字无法识别 要改成static __inline
mpu6050的功能报错:
因为它没有 accel_cfg2，lp_accel_odr，accel_intel 这三个功能，所以在inv_mpu.c内的struct gyro_reg_s 中把它注释掉
硬件库的移植是一个极其痛苦的过程，稍有不慎就是一大堆报错，还有可能移植不成功。所以推荐直接用正点原子给的MPU6050的例程来改吧，它把移植都已经做好了，我们要做的就是再把这个移植到自己的工程，然后参考它的主函数写自己的程序，当然主控必须是STM32F10系列，不然只能自己哭着移植官方库了。另外需要注意的是，硬件库中有一段代码是在开机时令MPU6050进行自检，用于判断芯片是否损坏以及设定原点，此时需要把MPU6050正面向上水平放置，这样才能通过自检，不然会报错的。MPU6050硬件DMP库会把每次开机时的MPU状态视作原点，想要修改这一现象需要把“inv.mpu.c”文件中的“run_self_test”函数改成如下
u8 run_self_test(void)
{
int result;
//char test_packet[4] = {0};
long gyro[3], accel[3];
result = mpu_run_self_test(gyro, accel);
if (result == 0x3)
{
/* Test passed. We can trust the gyro data here, so let's push it down
* to the DMP.
*/
float sens;
unsigned short accel_sens;
mpu_get_gyro_sens(&sens);
gyro[0] = (long)(gyro[0] * sens);
gyro[1] = (long)(gyro[1] * sens);
gyro[2] = (long)(gyro[2] * sens);
dmp_set_gyro_bias(gyro);
mpu_get_accel_sens(&accel_sens);
accel_sens = 0；//添加词句完成修改，MPU不自动设定原点
accel[0] *= accel_sens;
accel[1] *= accel_sens;
accel[2] *= accel_sens;
dmp_set_accel_bias(accel);
return 0;
}else return 1;
}
软件DMP
网上一直流行着一种软件DMP算法，具体内容放下面了，具体为啥这样做我是没看懂，代码倒是试了一下，也是没感觉好用，甚至感觉我得出的数据是错的 … …，具体为啥没时间去找了，先把内容贴出来吧，以后有机会再尝试下。
先来看看如何用欧拉角描述一次平面旋转(坐标变换)：
MPU6050的四元数解算姿态方法
设坐标系绕旋转α角后得到坐标系，在空间中有一个矢量在坐标系中的投影为，在内的投影为由于旋转绕进行，所以Z坐标未变，即有。
转换成矩阵形式表示为：
整理一下
所以从旋转到可以写成上面仅仅是绕一根轴的旋转，如果三维空间中的欧拉角旋转要转三次:
上面得到了一个表示旋转的方向余弦矩阵。
不过要想用欧拉角解算姿态，其实我们套用欧拉角微分方程就行了：
上式中左侧，是本次更新后的欧拉角，对应row、pit、yaw。右侧，是上个周期测算出来的角度，三个角速度由直接安装在四轴飞行器的三轴陀螺仪在这个周期转动的角度，单位为弧度，计算间隔时T陀螺角速度，比如0.02秒0.01弧度/秒=0.0002弧度。间因此求解这个微分方程就能解算出当前的欧拉角。
前面介绍了什么是欧拉角，而且欧拉角微分方程解算姿态关系简单明了，概念直观容易理解，那么我们为什么不用欧拉角来表示旋转而要引入四元数呢？
一方面是因为欧拉角微分方程中包含了大量的三角运算，这给实时解算带来了一定的困难。而且当俯仰角为90度时方程式会出现神奇的“GimbalLock”。所以欧拉角方法只适用于水平姿态变化不大的情况，而不适用于全姿态飞行器的姿态确定。
四元数法只求解四个未知量的线性微分方程组，计算量小，易于操作，是比较实用的工程方法。
我们知道在平面(x,y)中的旋转可以用复数来表示，同样的三维中的旋转可以用单位四元数来描述。我们来定义一个四元数：
我们可以把它写成,其中,。那么是矢量，表示三维空间中的旋转轴。w是标量，表示旋转角度。那么就是绕轴旋转w度，所以一个四元数可以表示一个完整的旋转。只有单位四元数才可以表示旋转，至于为什么，因为这就是四元数表示旋转的约束条件。
而刚才用欧拉角描述的方向余弦矩阵用四元数描述则为：
所以在软件解算中，我们要首先把加速度计采集到的值(三维向量)转化为单位向量，即向量除以模，传入参数是陀螺仪x、y、z值和加速度计x、y、z值：
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
float vx, vy, vz;
float ex, ey, ez;
norm = sqrt(axax + ayay + azaz);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
下面把四元数换算成方向余弦中的第三行的三个元素。刚好vx、vy、vz 。其实就是上一次的欧拉角（四元数）的机体坐标参考系换算出来的重力的单位向量。
vx = 2(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
axyz是机体坐标参照系上，加速度计测出来的重力向量，也就是实际测出来的重力向量。
axyz是测量得到的重力向量，vxyz是陀螺积分后的姿态来推算出的重力向量，它们都是机体坐标参照系上的重力向量。
那它们之间的误差向量，就是陀螺积分后的姿态和加计测出来的姿态之间的误差。
向量间的误差，可以用向量叉积（也叫向量外积、叉乘）来表示，exyz就是两个重力向量的叉积。
这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。（你可以自己拿东西想象一下）由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。
ex=（ayvz-azvy）;
ey=（azvx-axvz）;
ez=（axvy-ayvx）;
用叉积误差来做PI修正陀螺零偏
exInt = exInt + exKi;
eyInt = eyInt + eyKi;
ezInt = ezInt + ezKi; // adjusted gyroscope measurements
gx = gx + Kpex + exInt;
gy = gy + Kpey + eyInt;
gz = gz + Kpez + ezInt;
四元数微分方程，其中T为测量周期，为陀螺仪角速度，以下都是已知量，这里使用了一阶龙哥库塔求解四元数微分方程：
q0 = q0 + (-q1gx - q2gy - q3gz)halfT;
q1 = q1 + (q0gx + q2gz - q3gy)halfT;
q2 = q2 + (q0gy - q1gz + q3gx)halfT;
q3 = q3 + (q0gz + q1gy - q2*gx)*halfT;
最后根据四元数方向余弦阵和欧拉角的转换关系，把四元数转换成欧拉角：
所以有：
ANGLE.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2q2 - 2 * q3 q3 + 1)* 57.3; // yaw
ANGLE.Y= asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
ANGLE.X= atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
完整程序
//变量定义
#define Kp1 00.0f//比例增益支配率收敛到加速度计/磁强计
#define Ki 0.002f//积分增益支配率的陀螺仪偏见的衔接
#define halfT 0.001f//采样周期的一半
float q0=1，q1=0，q2=0，q3=0;//四元数的元素，代表估计方向
float exInt=0，eyInt=0，ezInt=0;//按比例缩小积分误差
float Yaw，Pitch，Roll;//偏航角，俯仰角，翻滚角
voidIMUupdate（float gx，float gy，float gz，float ax，float ay，float az）
{
float norm;
float vx，vy，vz;
float ex，ey，ez;
//测量正常化
norm=sqrt（ax*ax+ay*ay+az*az）;
ax=ax/norm;//单位化
ay=ay/norm;
az=az/norm;
//估计方向的重力
vx=2*（q1*q3-q0*q2）;
vy=2*（q0*q1+q2*q3）;
vz=q0*q0-q1*q1-q2*q2+q3*q3;
//错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
ex=（ay*vz-az*vy）;
ey=（az*vx-ax*vz）;
ez=（ax*vy-ay*vx）;
//积分误差比例积分增益
exInt=exInt+ex*Ki;
eyInt=eyInt+ey*Ki;
ezInt=ezInt+ez*Ki;
//调整后的陀螺仪测量
gx=gx+Kp*ex+exInt;
gy=gy+Kp*ey+eyInt;
gz=gz+Kp*ez+ezInt;
//整合四元数率和正常化
q0=q0+（-q1*gx-q2*gy-q3*gz）*halfT;
q1=q1+（q0*gx+q2*gz-q3*gy）*halfT;
q2=q2+（q0*gy-q1*gz+q3*gx）*halfT;
q3=q3+（q0*gz+q1*gy-q2*gx）*halfT;
//正常化四元
norm=sqrt（q0*q0+q1*q1+q2*q2+q3*q3）;
q0=q0/norm;
q1=q1/norm;
q2=q2/norm;
q3=q3/norm;
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;//rollv
Yaw=atan2（2*（q1*q2+q0*q3），q0*q0+q1*q1-q2*q2-q3*q3）*57.3;//Yaw
}