这一篇我们就讲姿态解算
先说一下，我们之前一直没说ADC的正负都是指什么方向
看图，这是陀螺仪的，右手定则判断
加速度的我没找到图
用DMP来姿态解算
用DMP库的话，我们的目的是用mpu_dmp_get_data函数获取四元数。具体的很多东西我都不懂，所以看这里你只能跟我移植一遍，基本学不到东西
移植官方的DMP库，可以看这里
https://blog.csdn.net/WalterBrien/article/details/123303182
关于库，可以去这里下下载地址，去最下面找MPU，然后我下的是emd6.12
创建文件夹
首先，建一个文件夹，把DMP库F4的core里面的文件，都导入一下；文件位置是\motion_driver_6.12\arm\STM32F4_MD6\Projects\eMD6\core
这个库是用其他软件写的，我们用的话，mpl文件夹里面这个.a文件，要把他换掉
删了换成这个，位置是\motion_driver_6.12\mpl libraries\arm\Keil
为什么是M3，那就看用的什么芯片了，F4的话就得用M4，这是看芯片内核的
然后在keil工程里创建文件夹
把上面文件夹的.c文件都导入进去
导入归导入，我们是从新文件夹里面放进去的，所以要在魔法棒里面，告诉项目我们头文件都在哪些文件夹里面。
也就是把刚才复制粘贴的文件夹，在这里选一边
这个时候，整个代码是跑不通的，后面要改一些东西
让代码能运行
Define
我们直接用的是这两个文件
如果进去看，会看到很多#if语句
那么在魔术棒增加几个宏定义：
,USE_DMP,MPL_LOG_NDEBUG=0,EMPL,MPU6050,EMPL_TARGET_STM32F4
解释下，
USE_DMP：第一个是告诉下面代码说使用DMP，
MPL_LOG_NDEBUG=0：第二个是是不是用他的调试工具，和我们用的匿名地面工作站差不多好像，我们不用，让他=0
EMPL：这个好像是使用EMPL库还是什么东西里的函数吧，不清楚，我没看
MPU6050：这个文件有很多模块都用，这里告诉文件我们是用6050芯片
EMPL_TARGET_STM32F4：文件本来是给MSP430写的，这里我们用的是给stm32F4写的，不过F1该用也一样用(可以看到很多教程，都把F4写成F1，然后程序里面很多地方都改成F1，那样是好看，但是太费劲了)
编译后挨个改error报错
头文件的问题
去掉垃圾警告【可以不看，但改了看起来舒服】
这里顺便说一句，很多垃圾警告像最后一行没有空行，怎么去掉
我们看警告后面跟的代号，然后再魔法棒里面写
--diag_suppress=1,870
图片里只看到代号1的警告是最后一行没空行。
那里面的870是什么：870是用汉字会出现的警告，建议一块干掉
error: #169: expected a declaration
完全按照我的操作来的不会有这个报错
error: #169: expected a declaration
这个报错，如果你像其他人一样，F4改成了F1，那么你就去两个文件里面，把里面的EMPL_TARGET_STM32F4，换成EMPL_TARGET_STM32F1
error: #5: cannot open source input file “board-st_discovery.h”: No such file or directory
这两个文件里都有引入这个头文件
我们没有用到这个，我也不知道是什么，直接删了就行
error: #5: cannot open source input file “stm32f4xx.h”: No such file or directory
我们用的是f1的hal库，但DMP写的时候好像用的是f4的标准库，我们把这里改一下就好
改成我们的hal库头文件
#include "stm32f1xx_hal.h"
一些我们自己的头文件
还是刚才那个文件，我们的头文件是usart.h，但他的叫uart.h，那么我们改成我们的
#include "usart.h"
另外，如果你自己写的i2c.h不叫这个，那你也得改一下，#include “你的i2c的.h文件”
一些要改动的函数
…\Core\IMU\driver\stm32L\log_stm32.c(209): error: #165: too few arguments in function call
直接注释掉就好，这个函数是我们用他的上位机软件时，给上位机发数据的。我们也不用这个。
【如果你想用】可以看BV1Ut4y1673t的P51的23分钟
如果想用的话，也就是重写一个串口发送函数，并且把fputc（）换成我们自己写的串口发送函数。
几个双击定位不到的报错
这几个都在inv_mpu_dmp_motion_driver.c和inv_mpu.c里
inv_mpu.c里
应该能看懂，mpu库为了方便移植，用宏定义的方式使用我们的函数，但这里的函数名字和我们的不一样
下面那个reg_int_cb我不知道是什么，直接注释掉
#define i2c_write IIC_WriteMultibyteFromSlave
#define i2c_read IIC_ReadMultibyteFromSlave
#define delay_ms HAL_Delay
#define get_ms HAL_GetTick
#define log_i printf
#define log_e printf
说下都是什么
i2c_write I2C连续写
i2c_read I2C连续读
delay_ms 延时
get_ms 获取时间【我们用不到，写一个空函数就行】【在其他芯片上，用dmp好像有用到的，但6050，9250之类不用】【本来的作用是获取两此函数调用时的时间戳，相减获得中间相隔的时间】【我们放上去HAL_GetTick，其实这个函数已经挂了，两个获取参数的方法不一样】
下面两个打印我们用不到，其实不改也不报错，但我手贱
改完以后，会有…\Core\IMU\driver\eMPL\inv_mpu.c(905): error: #140: too many arguments in function call
原因是HAL_GetTick（void）；而本来的get_ms（参数）是有形参的，那么我们去报错里面把参数删掉就好
inv_mpu_dmp_motion_driver.c里
__no_operation()函数在keil里面是没有的，他是另一个软件的空函数
相当于我们的__nop()
只要改成__nop()就好了
几个宏定义
这里就和上面一样了，把用到的宏定义改成我们自己的
然后get_ms还是和上面一样，编译后去bug里删掉参数。
初始化
上面的操作下，我们的代码就可以跑通了，但我们还需要初始化DMP，从而得到数据
代码写在mpu6050.c里面
我们先引入两个用到的头文件，再定义一个函数dmp_init()，然后再里面写
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
uint8_t dmp_init()
{
}
另外下面为什么都写一个b=函数，因为后面可以打印出来b的结果，看看函数是否使用成功，一般是等于0就成功
打开FIFO
我们之前初始化6050的时候，没有使用fifo，但是用dmp的话就必须用fifo了
不过我测试不写也能用
IIC_WriteByteFromSlave(0x68,0x23,0x78); //打开FIFO
设置传感器
这个是告诉要打开哪个传感器；
我们要打开加速度计和陀螺仪
b=mpu_set_sensors(INV_XYZ_ACCEL|INV_XYZ_GYRO); //设置sensors
那么有人就要问了，为什么我初始化mpu6050的时候已经打开过了，这里还要设置；
我也不知道，反正测试是不写就不能用
设置FIFO
不写这个好像也能用，测试过；不知道和上面哪个是不是重复
上面我们打开过FIFO，这里好像还得写一次；我们就打开加速度计和陀螺仪的fifo
b=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); //设置fifo
设置采样率
同样是不知道为什么，上面设置过采样率，这里还得重新设置。
函数定义上面的注释里有写，采样率在4到1kHz之间，我们设置100，设置其他大小也行
/**
- @brief Set sampling rate.
- Sampling rate must be between 4Hz and 1kHz.
- @param[in] rate Desired sampling rate (Hz).
- @return 0 if successful.
*/
b=mpu_set_sample_rate(100);
设置陀螺仪全尺寸范围函数
同样上面设置过，这里还得重新设置，很烦
我们还是设置2000°/s
b=mpu_set_gyro_fsr(2000);
加载并验证DMP映像函数
这个不懂，原话Load the DMP with this image.，大概就是得用这个才能用DMP吧
b=dmp_load_motion_driver_firmware();
设置陀螺仪方向
我们mpu6050在pcb的时候，为了方便画pcb，mpu6050的摆放方向就很多样，在dmp库里使用这句函数，可以告诉程序我们的mpu6050的放置方法，从而改变我们设置的前后左右
b=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));
这一句用了两个函数，具体不用看了，反正就设置一下本来陀螺仪的方向，后续如果不对的话就改变量gyro_orientation
signed char gyro_orientation[9] = {1, 0, 0,0, 1, 0,0, 0, 1};
这个数组是怎么来的可以说一说
我们打开下载的dmp库文件夹\motion_driver_6.12\documentation文件夹下的Orientation Matrix Transformation chart.pdf
这里面已经写了，按照自己的陀螺仪对应的方向，设置自己的矩阵
设置dmp功能
这个不知道说啥
b=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);
设置DMP输出速率
b=dmp_set_fifo_rate(100);
自检
定义两个变量，然后使用自检函数
这个函数可以矫正零偏；我们知道mpu6050在板子上产生各种各样的误差，在这里矫正零偏（不过后面有还是很多误差）
long gyroData[3],accData[3];
b = mpu_run_self_test(gyroData,accData);
使能DMP
到这结束了，使能dmp就不介绍了，参数写1就是使能，0就是不使能
b=mpu_set_dmp_state(1);
总体
我在函数里写了printf，打印各种函数的设置情况，方便检查，删了printf不影响功能
uint8_t dmp_init()
{
signed char gyro_orientation[9] = { 1, 0, 0,
0, 1, 0,
0, 0, 1};
uint8_t b;
long gyroData[3],accData[3];
IIC_WriteByteFromSlave(0x68,0x23,0x78); //打开FIFO
b=mpu_set_sensors(INV_XYZ_ACCEL|INV_XYZ_GYRO); //设置sensors
printf("设置sensors：%d\r\n",b);
b=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); //设置fifo
printf("设置fifo：%d\r\n",b);
b=mpu_set_sample_rate(100);
printf("设置采样率：%d\r\n",b);
b=mpu_set_gyro_fsr(2000);
printf("设置陀螺仪全尺寸范围函数：%d\r\n",b);
b=dmp_load_motion_driver_firmware();
printf("加载并验证DMP映像函数%d\r\n",b);
b=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
printf("设置陀螺仪方向:%d\r\n",b);
b=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);
printf("设置dmp功能:%d\r\n",b);
b=dmp_set_fifo_rate(100); //设置DMP输出速率(最大不超过200Hz)
printf("设置DMP输出速率:%d\r\n",b);
b = mpu_run_self_test(gyroData,accData);
printf("自检:%d\r\n",b);
b=mpu_set_dmp_state(1); //使能DMP
printf("使能DMP:%d\r\n",b);
}
通过dmp得到的四元数获取欧拉角
这里大概之间复制粘贴吧
具体就是四元数与欧拉角都能形成一个相同的矩阵，那么通过矩阵里的参数逆变换，就得到了欧拉角
dmp_read_fifo函数功能就是通过dmp获取四元数等数据，这里我们只关注四元数，拿到四元数后进行计算。
uint8_t mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
{
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f,q30=1073741824.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;
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
return 0;
}
到这就结束了，去主函数做个测试吧，先初始化dmp【dmp_init()】，再获取欧拉角【mpu_dmp_get_data()】
自己写姿态解算【未完成】
自己写比较难，我建议先用DMP来做，熟悉下流程。
用mahony互补滤波算法
首先明确一点东西
一些定义
坐标系
导航坐标系(NED坐标系)
N（north）——北轴指向地球北；
E（east）——东轴指向地球东；
D（down）——地轴垂直于地球表面向下。【U（up）——地轴垂直地面向上】
那么有两种（地轴是向下的D还是向上的U）：xyz:NED（北东地）；xyz:ENU（东北天）
组合坐标系时常用n来表示
机体坐标系
载体坐标系的定义为，以载体为中心固联于载体上，以载体质心为坐标原点，X（或Y ）轴沿载体运动方向， Z轴沿载体竖轴向下（或向上）， X轴与Y形成右手系。
F（forward）——沿载体运动方向
R（right）——载体运动方向右边
D（down）——沿载体竖轴向下【U（up）——沿载体竖轴向上】
也是两种：xyz:FRD（前右下）；xyz:RFU（右前上）
组合坐标系时常用b来表示
两者组合起来
（图摘自BV12W4y1177k）
导航系xyz：NED则机体系用xyz：FRD
导航系xyz：ENU则机体系用xyz：RFU
然后描述载体坐标系的时候，用到我们之前说的姿态解算的欧拉角：横滚角（Roll）、航偏角（Yaw）、俯仰角（Pitch）
姿态
上面我们说了坐标系，因为实际上我们姿态解算，就是找这两个坐标系之间的角度关系。
但实际用的时候，我们要将坐标系变成我们能直观理解的数据
姿态解算的核心在于旋转，一般旋转有4种表示方式：矩阵表示、欧拉角表示、轴角表示和四元数表示。矩阵表示适合变换向量，欧拉角最直观，轴角表示则适合几何推导，而在组合旋转方面，四元数表示最佳。因为姿态解算需要频繁组合旋转和用旋转变换向量，所以采用四元数保存组合姿态、辅以矩阵来变换向量的方案。
欧拉角
上面图不用仔细看，知道欧拉角是沿三个轴的转动就完事了，下面的图更直观
大家都在用欧拉角，下面都是解算出欧拉角的
Yaw 表示绕y轴旋转的角度，Pitch表示绕x轴旋转的角度，Roll表示绕z轴旋转的角度
看上面的图，应该能想到，加速度计绕Z轴转的时候，重力加速度始终在一个方向，是测不出来姿态的，另外，加速度计只有静态的时候测的角度才能是比较准的
四元数
这玩意表示四维空间，本来像说说，但我也不懂，下面用到的话再说吧
看定义，就是一组标准正交基，四个单位向量，两两正交
欧拉角的计算
三个角的计算方法是一样的
我们来看一下沿z轴旋转的欧拉角Roll，学习一下，OF向量不动，让坐标轴旋转α°，从1坐标轴变成2坐标轴，沿z轴转，z轴不变没问题吧
我们用ry1rx1来表示ry2rx2
有了这三个式子
把他们写成矩阵形式，进行运算
写简化点就是r2=Cz*r1
同样的方法，按x轴按y轴按z轴，计算出三个旋转矩阵Cnb(从b系变成n系)
然后三个矩阵相乘Cnb=Cx * Cy * Cz，就得到了三个轴全部旋转的旋转矩阵
我这算出答案了，但我建议你们自己算算，我去网上抄，tnnd一群人，一个人一种答案，有Cx * Cy * Cz错了，但答案没问题的，有Cx * Cy * Cz没问题，答案错了的，给我整麻了,然后算完了下面告诉我我这对不对
然后正交矩阵的转置等于他的逆矩阵，那么（Cnb）T=Cbn
现在我们知道有这么个矩阵，但他里面的元素具体是多少是不知道的，如果我们知道，那么就能通过里面的元素，反解出对应的角度
要怎么得到里面的元素，那就回到四元数上了，
四元数的四个值，可以通过不同组合，形成上面这个矩阵