移植DMP到MPU6050获取欧拉角

经过上一节的介绍，我们可以读出 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 


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, &reg, 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外设库