移植DMP到MPU6050获取欧拉角

文章目录

1、较浅显的姿态解算介绍
2、姿态解算是怎么来的
- 1、加速度反求
- 2、角速度积分
3、DMP介绍
4、DMP移植
5、源码

经过上一节的介绍，我们可以读出 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 0xD0float 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;elsereturn 1;
}
uint8_t MPU_Read_Byte(uint8_t reg)
{if(HAL_I2C_Mem_Read(&hi2c1, 0xD1, reg, 1, &reg, 1, 0xff) == HAL_OK)return 0;elsereturn 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); //复位MPU6050HAL_Delay(100);MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00);   //唤醒MPU6050MPU_Set_Gyro_Fsr(3);                 //陀螺仪传感器,±2000dpsMPU_Set_Accel_Fsr(0);                  //加速度传感器,±2gMPU_Set_Rate(50);                       //设置采样率50HzMPU_Write_Byte(MPU_INT_EN_REG,0X00); //关闭所有中断MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //I2C主模式关闭MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); //关闭FIFOMPU_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;elseb = 7;      // errorreturn 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);//设置FIFOif(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);  //使能DMPif(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;   // pitchIMU_Data->Angle_Y  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;   // rollIMU_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外设库