经过上一节的介绍，我们可以读出 MPU6050的加速度传感器和角速度传感器的原始数据。不过这些原始数据，对我们来说，用处不大，我们期望得到的是姿态数据，也就是欧拉角：航向角（yaw）、横滚角（roll）和俯仰角（pitch）。有了这三个角，我们就可以得到当前物体的姿态，这才是我们想要的结果。
1、较浅显的姿态解算介绍
在飞行器中，飞行姿态是非常重要的参数，以飞机自身的中心建立坐标系，当飞机绕坐标轴旋转的时候，会分别影响偏航角、横滚角及俯仰角，坐标系描述如下所示：
我们的任务其实就是把这些角度给他解算出来，这里我们肯定就是基于我们之前用陀螺仪检测到的加速度，角速度来进行解算的，但是加速度和角速度不能之间变成我们需要的这些角度，所以下面就来说下到底是怎么变成我们的欧拉角的。
- 1、比较直观的角度检测器就是陀螺仪了
它可以检测物体绕坐标轴转动的“角速度”，如同将速度对时间积分可以求出路程一样，将角速度对时间积分就可以计算出旋转的“角度”，由于陀螺仪测量角度时使用积分，会存在积分误差，若积分时间 Dt 越小，误差就越小。这十分容易理解，例如计算路程时，假设行车时间为 1 小时，我们随机选择行车过程某个时刻的速度 Vt 乘以 1 小时，求出的路程误差是极大的，因为行车的过程中并不是每个时刻都等于该时刻速度的，如果我们每 5 分钟检测一次车速，可得到 Vt1、 Vt2、Vt3-Vt12 这 12 个时刻的车速，对各个时刻的速度乘以时间间隔(5 分钟)，并对这 12 个结果求和，就可得出一个相对精确的行车路程了，不断提高采样频率，就可以使积分时间 Dt 变小，降低误差。
但是我们要注意一点啊，积分是会有累积误差的，这个在PID里面我们也做了很多了解，这个是肯定的，所以我们如果长时间用角速度积分来计算欧拉角肯定是不行的。 - 2、使用加速度计来获取欧拉角
如图所示水平仪，在重力作用下气泡会运动，在电子设备中，一般使用加速度传感器来检测倾角，它通过检测器件在各个方向的形变情况而采样得到受力数据，根据 F=ma 转换，传感器直接输出加速度数据，因而被称为加速度传感器。由于地球存在重力场，所以重力在任何时刻都会作用于传感器，当传感器静止的时候(实际上加速度为 0)，传感器会在该方向检测出加速度 g，不能认为重力方向测出的加速度为 g，就表示传感器在该方向作加速度为 g 的运动。
这个原理的缺点就很明显了，检测不了yaw轴啊，因为他是根据重力来推导的，yaw轴不受重力影响。，但是这个不会有累积误差，因为重力总是标准的，他也不存在积分得过程，所以综合来看，实际过程中我们来测量欧拉角一般是融合的方式，假如我们同时使用这两种传感器，并设计一个滤波算法，当物体处于静止状态时，增大加速度数据的权重，当物体处于运动状时， 增大陀螺仪数据的权重，从而获得更准确的姿态数据。
2、姿态解算是怎么来的
1、加速度反求
首先mpu6050本身有一个坐标系：（注意6轴陀螺仪测量的是加速度和角速度）
当传感器的正方向 Z 轴垂直指向天空时， 由于此时受到地球重力的作用，此时加速度计 Z 轴的读数应为正， 而且理想情况下应为g(这是地球的重力造成的加速度)，下面来设想我们的陀螺仪角度发生了偏转：
从上面可以看到，重力产生的加速度将会分解为两个方向，因此我们可以得到：
其中ACC_Y还有ACC_Z是已知量，因此可以反求我们需要的角度：
上面是求x轴方向时的角度，我们也可以来求y方向的，也是同理，如果都不是的话也只是两个方向的投影角度了，综合来看两个方向的欧拉角如下所示：
但是这样的方法会有两个缺点：
- 1、本身求解过程会存在误差，因为本身求得的加速度是读取adc获取的，获取的过程就会有误差存在
- 2、这个方法无法求得水平方向的偏航角（重力加速度）
2、角速度积分
还有一个数据就是可以通过角速度积分就可以获取角度了，但是角速度积分会存在积分的通病，就是会产生累计误差，从而造成偏移，因此在水平方向就会产生一个误差，这个误差是六轴传感器无法避免的。
3、DMP介绍
DMP就是MPU6050内部的运动引擎，全称Digital Motion Processor，直接输出四元数，可以减轻外围微处理器的工作负担且避免了繁琐的滤波和数据融合。Motion Driver是Invensense针对其运动传感器的软件包，并非全部开源，核心的算法部分是针对ARM处理器和MSP430处理器编译成了静态链接库，适用于MPU6050、MPU6500、MPU9150、MPU9250等传感器。
使用 MPU6050 的 DMP 输出的四元数是 q30 格式的，也就是浮点数放大了 2 的 30 次方倍。在换算成欧拉角之前，必须先将其转换为浮点数，也就是除以 2 的 30 次方，然后再进行计算，计算公式为：
q0=quat[0] / q30; //q30 格式转换为浮点数
q1=quat[1] / q30;
q2=quat[2] / q30;
q3=quat[3] / q30;
其中 quat[0]~ quat[3]是 MPU6050 的 DMP 解算后的四元数， q30 格式，所以要除以一个 2的 30 次方，其中 q30 是一个常量： 1073741824，即 2 的 30 次方，然后带入公式，计算出欧拉角。上述计算公式的 57.3 是弧度转换为角度，即 180/π，这样得到的结果就是以度（°）为单位的。
pitch = asin(-2 * q1 * q3 + 2 * q0* q2)
roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)
yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3)
4、DMP移植
这里我看了下，觉得正点原子写得好，所以我决定抄正点原子的！！！
然后我们找到正点原子的这个放DMP的文件夹，把这里的文件都复制到我们的工程里面（注意可能会乱码，因为是GBK，然后我用的是UTF-8的编码，不过不影响使用）
这里因为单片机的不同，我们需要修改几个宏参数
其中这里面iic的发送和读取函数改用硬件iic如下所示：
加入DMP初始化函数
加入DMP读数数据函数，这里我们获取到的为四元数，将四元数按照我们上面提到的解算函数解算为欧拉角
之后就可以在主函数中进行调用了
之后我们将程序下载到开发板中就可以看到数据了，移植成功！！！！
5、源码
mpu6050.c
/*
* mpu6050.c
*
* Created on: Mar 5, 2022
* Author: LX
*/
#include "mpu6050.h"
#include "../DMP/inv_mpu.h"
#include "../DMP/inv_mpu_dmp_motion_driver.h"
#include <math.h>
extern I2C_HandleTypeDef hi2c1;
IMU_Parameter IMU_Data;
#define MPU_ADDR 0xD0
float gyrox, gyroy, gyroz, accelx, accely, accelz, temp;
uint8_t MPU_Write_Byte(uint8_t reg,uint8_t data)
{
if(HAL_I2C_Mem_Write(&hi2c1,MPU_ADDR,reg,1,&data,1,0xff) == HAL_OK)
return 0;
else
return 1;
}
uint8_t MPU_Read_Byte(uint8_t reg)
{
if(HAL_I2C_Mem_Read(&hi2c1, 0xD1, reg, 1, ®, 1, 0xff) == HAL_OK)
return 0;
else
return 1;
}
//设置MPU6050陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
// 其他,设置失败
uint8_t MPU_Set_Gyro_Fsr(uint8_t fsr)
{
return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围
}
//设置MPU6050加速度传感器满量程范围
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,设置成功
// 其他,设置失败
uint8_t MPU_Set_Accel_Fsr(uint8_t fsr)
{
return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围
}
//设置MPU6050的数字低通滤波器
//lpf:数字低通滤波频率(Hz)
//返回值:0,设置成功
// 其他,设置失败
uint8_t MPU_Set_LPF(uint16_t lpf)
{
uint8_t data=0;
if(lpf>=188)data=1;
else if(lpf>=98)data=2;
else if(lpf>=42)data=3;
else if(lpf>=20)data=4;
else if(lpf>=10)data=5;
else data=6;
return MPU_Write_Byte(MPU_CFG_REG,data);//设置数字低通滤波器
}
//设置MPU6050的采样率(假定Fs=1KHz)
//rate:4~1000(Hz)
//返回值:0,设置成功
// 其他,设置失败
uint8_t MPU_Set_Rate(uint16_t rate)
{
uint8_t data;
if(rate>1000)rate=1000;
if(rate<4)rate=4;
data=1000/rate-1;
data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data); //设置数字低通滤波器
return MPU_Set_LPF(rate/2); //自动设置LPF为采样率的一半
}
uint8_t MPU6050_Init(void)
{
uint8_t res=0;
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); //复位MPU6050
HAL_Delay(100);
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); //唤醒MPU6050
MPU_Set_Gyro_Fsr(3); //陀螺仪传感器,±2000dps
MPU_Set_Accel_Fsr(0); //加速度传感器,±2g
MPU_Set_Rate(50); //设置采样率50Hz
MPU_Write_Byte(MPU_INT_EN_REG,0X00); //关闭所有中断
MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //I2C主模式关闭
MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); //关闭FIFO
MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); //INT引脚低电平有效
res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
if(res==MPU_ADDR)//器件ID正确
{
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01); //设置CLKSEL,PLL X轴为参考
MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00); //加速度与陀螺仪都工作
MPU_Set_Rate(50); //设置采样率为50Hz
}else return 1;
return 0;
}
void MPU6050_GET_Data(void)
{
uint8_t buf[14]={0};
HAL_I2C_Mem_Read(&hi2c1,MPU_ADDR,MPU_ACCEL_XOUTH_REG,1,buf,14,1000);
accelx = (float) (((int16_t) (buf[0] << 8) + buf[1])/16384.0f);
accely = (float) (((int16_t) (buf[2] << 8) + buf[3])/16384.0f);
accelz = (float) (((int16_t) (buf[4] << 8) + buf[5])/16384.0f);
temp = (float) (((int16_t) (buf[6] << 8) + buf[7])/340 + 36.53f);
gyrox = (float) (((int16_t) (buf[8] << 8) + buf[9])/131.0f);
gyroy = (float) (((int16_t) (buf[10] << 8) + buf[11])/131.0f);
gyroz = (float) (((int16_t) (buf[12] << 8) + buf[13])/131.0f);
IMU_Data.Accel_X = accelx - IMU_Data.offset_Accel_X;
IMU_Data.Accel_Y = accely - IMU_Data.offset_Accel_Y;
IMU_Data.Accel_Z = accelz - IMU_Data.offset_Accel_Z;
IMU_Data.Temp = temp;
IMU_Data.Gyro_X = gyrox - IMU_Data.offset_Gyro_X;
IMU_Data.Gyro_Y = gyroy - IMU_Data.offset_Gyro_Y;
IMU_Data.Gyro_Z = gyroz - IMU_Data.offset_Gyro_Z;
}
#define DEFAULT_MPU_HZ (100)//定义输出速度
// 陀螺仪方向设置
static signed char gyro_orientation[9] = { 1, 0, 0,
0, 1, 0,
0, 0, 1};
#define q30 1073741824.0f
//方向转换
unsigned short inv_row_2_scale(const signed char *row)
{
unsigned short b;
if (row[0] > 0)
b = 0;
else if (row[0] < 0)
b = 4;
else if (row[1] > 0)
b = 1;
else if (row[1] < 0)
b = 5;
else if (row[2] > 0)
b = 2;
else if (row[2] < 0)
b = 6;
else
b = 7; // error
return b;
}
//MPU6050自测试
//返回值:0,正常
// 其他,失败
unsigned char 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)
{
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[0] *= accel_sens;
accel[1] *= accel_sens;
accel[2] *= accel_sens;
dmp_set_accel_bias(accel);
return 0;
}else return 1;
}
//陀螺仪方向控制
uint8_t inv_orientation_matrix_to_scalar(const signed char *mtx)
{
unsigned short scalar;
scalar = inv_row_2_scale(mtx);
scalar |= inv_row_2_scale(mtx + 3) << 3;
scalar |= inv_row_2_scale(mtx + 6) << 6;
return scalar;
}
uint8_t mpu_dmp_init(void)
{
uint8_t res=0;
if(mpu_init()==0) //初始化MPU6050
{
res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器
if(res)return 1;
res=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);//设置FIFO
if(res)return 2;
res=mpu_set_sample_rate(DEFAULT_MPU_HZ); //设置采样率
if(res)return 3;
res=dmp_load_motion_driver_firmware(); //加载dmp固件
if(res)return 4;
res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
if(res)return 5;
res=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);
if(res)return 6;
res=dmp_set_fifo_rate(DEFAULT_MPU_HZ); //设置DMP输出速率(最大不超过200Hz)
if(res)return 7;
res=run_self_test(); //自检
if(res)return 8;
res=mpu_set_dmp_state(1); //使能DMP
if(res)return 9;
}
return 0;
}
//得到dmp处理后的数据
//pitch:俯仰角 精度:0.1° 范围:-90.0° <---> +90.0°
//roll:横滚角 精度:0.1° 范围:-180.0°<---> +180.0°
//yaw:航向角 精度:0.1° 范围:-180.0°<---> +180.0°
//返回值:0,正常
// 其他,失败
uint8_t mpu_dmp_get_data(IMU_Parameter *IMU_Data)
{
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.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;
if(sensors&INV_WXYZ_QUAT)
{
q0 = quat[0] / q30; //q30格式转换为浮点数
q1 = quat[1] / q30;
q2 = quat[2] / q30;
q3 = quat[3] / q30;
//计算得到俯仰角/横滚角/航向角
IMU_Data->Angle_X = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
IMU_Data->Angle_Y = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
IMU_Data->Angle_Z = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
}else return 2;
return 0;
}
uint8_t HAL_i2c_write(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char const *data)
{
HAL_I2C_Mem_Write(&hi2c1, ((slave_addr<<1)|0), reg_addr, 1, (unsigned char *)data, length, HAL_MAX_DELAY);
return 0;
}
uint8_t HAL_i2c_read(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char const *data)
{
HAL_I2C_Mem_Read(&hi2c1, ((slave_addr<<1)|1), reg_addr, 1, (unsigned char *)data, length, HAL_MAX_DELAY);
return 0;
}
mpu6050.h
/*
* mpu6050.h
*
* Created on: Mar 5, 2022
* Author: LX
*/
#ifndef MPU6050_H_
#define MPU6050_H_
#include "main.h"
#define MPU_SELF_TESTX_REG 0X0D //自检寄存器X
#define MPU_SELF_TESTY_REG 0X0E //自检寄存器Y
#define MPU_SELF_TESTZ_REG 0X0F //自检寄存器Z
#define MPU_SELF_TESTA_REG 0X10 //自检寄存器A
#define MPU_SAMPLE_RATE_REG 0X19 //采样频率分频器
#define MPU_CFG_REG 0X1A //配置寄存器
#define MPU_GYRO_CFG_REG 0X1B //陀螺仪配置寄存器
#define MPU_ACCEL_CFG_REG 0X1C //加速度计配置寄存器
#define MPU_MOTION_DET_REG 0X1F //运动检测阀值设置寄存器
#define MPU_FIFO_EN_REG 0X23 //FIFO使能寄存器
#define MPU_I2CMST_CTRL_REG 0X24 //IIC主机控制寄存器
#define MPU_I2CSLV0_ADDR_REG 0X25 //IIC从机0器件地址寄存器
#define MPU_I2CSLV0_REG 0X26 //IIC从机0数据地址寄存器
#define MPU_I2CSLV0_CTRL_REG 0X27 //IIC从机0控制寄存器
#define MPU_I2CSLV1_ADDR_REG 0X28 //IIC从机1器件地址寄存器
#define MPU_I2CSLV1_REG 0X29 //IIC从机1数据地址寄存器
#define MPU_I2CSLV1_CTRL_REG 0X2A //IIC从机1控制寄存器
#define MPU_I2CSLV2_ADDR_REG 0X2B //IIC从机2器件地址寄存器
#define MPU_I2CSLV2_REG 0X2C //IIC从机2数据地址寄存器
#define MPU_I2CSLV2_CTRL_REG 0X2D //IIC从机2控制寄存器
#define MPU_I2CSLV3_ADDR_REG 0X2E //IIC从机3器件地址寄存器
#define MPU_I2CSLV3_REG 0X2F //IIC从机3数据地址寄存器
#define MPU_I2CSLV3_CTRL_REG 0X30 //IIC从机3控制寄存器
#define MPU_I2CSLV4_ADDR_REG 0X31 //IIC从机4器件地址寄存器
#define MPU_I2CSLV4_REG 0X32 //IIC从机4数据地址寄存器
#define MPU_I2CSLV4_DO_REG 0X33 //IIC从机4写数据寄存器
#define MPU_I2CSLV4_CTRL_REG 0X34 //IIC从机4控制寄存器
#define MPU_I2CSLV4_DI_REG 0X35 //IIC从机4读数据寄存器
#define MPU_I2CMST_STA_REG 0X36 //IIC主机状态寄存器
#define MPU_INTBP_CFG_REG 0X37 //中断/旁路设置寄存器
#define MPU_INT_EN_REG 0X38 //中断使能寄存器
#define MPU_INT_STA_REG 0X3A //中断状态寄存器
#define MPU_ACCEL_XOUTH_REG 0X3B //加速度值,X轴高8位寄存器
#define MPU_ACCEL_XOUTL_REG 0X3C //加速度值,X轴低8位寄存器
#define MPU_ACCEL_YOUTH_REG 0X3D //加速度值,Y轴高8位寄存器
#define MPU_ACCEL_YOUTL_REG 0X3E //加速度值,Y轴低8位寄存器
#define MPU_ACCEL_ZOUTH_REG 0X3F //加速度值,Z轴高8位寄存器
#define MPU_ACCEL_ZOUTL_REG 0X40 //加速度值,Z轴低8位寄存器
#define MPU_TEMP_OUTH_REG 0X41 //温度值高八位寄存器
#define MPU_TEMP_OUTL_REG 0X42 //温度值低8位寄存器
#define MPU_GYRO_XOUTH_REG 0X43 //陀螺仪值,X轴高8位寄存器
#define MPU_GYRO_XOUTL_REG 0X44 //陀螺仪值,X轴低8位寄存器
#define MPU_GYRO_YOUTH_REG 0X45 //陀螺仪值,Y轴高8位寄存器
#define MPU_GYRO_YOUTL_REG 0X46 //陀螺仪值,Y轴低8位寄存器
#define MPU_GYRO_ZOUTH_REG 0X47 //陀螺仪值,Z轴高8位寄存器
#define MPU_GYRO_ZOUTL_REG 0X48 //陀螺仪值,Z轴低8位寄存器
#define MPU_I2CSLV0_DO_REG 0X63 //IIC从机0数据寄存器
#define MPU_I2CSLV1_DO_REG 0X64 //IIC从机1数据寄存器
#define MPU_I2CSLV2_DO_REG 0X65 //IIC从机2数据寄存器
#define MPU_I2CSLV3_DO_REG 0X66 //IIC从机3数据寄存器
#define MPU_I2CMST_DELAY_REG 0X67 //IIC主机延时管理寄存器
#define MPU_SIGPATH_RST_REG 0X68 //信号通道复位寄存器
#define MPU_MDETECT_CTRL_REG 0X69 //运动检测控制寄存器
#define MPU_USER_CTRL_REG 0X6A //用户控制寄存器
#define MPU_PWR_MGMT1_REG 0X6B //电源管理寄存器1
#define MPU_PWR_MGMT2_REG 0X6C //电源管理寄存器2
#define MPU_FIFO_CNTH_REG 0X72 //FIFO计数寄存器高八位
#define MPU_FIFO_CNTL_REG 0X73 //FIFO计数寄存器低八位
#define MPU_FIFO_RW_REG 0X74 //FIFO读写寄存器
#define MPU_DEVICE_ID_REG 0X75 //器件ID寄存器
typedef struct
{
float Accel_X;
float Accel_Y;
float Accel_Z;
float offset_Accel_X;
float offset_Accel_Y;
float offset_Accel_Z;
float Gyro_X;
float Gyro_Y;
float Gyro_Z;
float offset_Gyro_X;
float offset_Gyro_Y;
float offset_Gyro_Z;
float Angle_X;
float Angle_Y;
float Angle_Z;
float Temp;
}IMU_Parameter;
uint8_t HAL_i2c_write(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char const *data);
uint8_t HAL_i2c_read(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char const *data);
uint8_t MPU6050_Init(void);
void MPU6050_GET_Data(void);
uint8_t mpu_dmp_init(void);
uint8_t mpu_dmp_get_data(IMU_Parameter *IMU_Data);
#endif /* MPU6050_H_ */
如果需要移植文件的话我放到gitee了，可以自行查看：lx外设库