STM32Cube HAL库——MPU6050 DMP姿态解算

北辰远_code

已于 2024-03-21 22:07:16 修改

阅读量1.4w

点赞数 75

CC 4.0 BY-SA版权

分类专栏： STM32Cube HAL库学习教程文章标签： stm32 嵌入式硬件单片机算法硬件工程科技开源软件

于 2024-03-21 22:00:56 首次发布

本文链接：https://blog.csdn.net/weixin_52916289/article/details/135142374

STM32Cube HAL库——MPU6050 DMP姿态解算

（注意：源文件下载请查看文末链接。）

一、MPU6050介绍

1.简介

MPU6050是6轴运动处理传感器，它集成了3轴MEMS陀螺仪，3轴MEMS加速度计，以及一个可扩展的数字运动处理器DMP。使用它就是为了得到待测物体（如四轴飞行器、平衡小车）x、y、z轴的倾角（俯仰角Pitch、翻滚角Roll、偏航角Yaw）。我们通过 I2C 读取到 MPU6050 的六个数据（三轴加速度 AD 值、三轴角速度 AD 值）经过姿态融合后就可以得到 Pitch、Roll、Yaw角。在这里插入图片描述
作为测量值的方向参考，传感器坐标方向定义如上图所示，属于右手坐标系（右手拇指指向 x 轴的正方向，食指指向 y 轴的正方向，中指能指向 z 轴的正方向）。

2.原理

（1）陀螺仪传感器（Gyroscope sensor）
传感器在它的内部有一个陀螺，因为陀螺效应始终与初始方向平行，这样就能通过与初始方向的偏差计算出旋转方向与角度。

（2）加速度计传感器（Accelerometer Sensor）
加速度传感器是一种能够测量加速度的传感器。它采用的是压电效应的原理。传感器在加速过程中，通过对质量块所受惯性力的测量，利用牛顿第二定律获取加速度值。

（3）数字运动处理器（Digital Motion Processor）
DMP是MPU6050芯片中的数据处理模块，（内置卡尔曼滤波算法）获取陀螺仪和加速度传感器数据，并处理输出四元数，可以减轻外围微处理器的工作负担且避免了繁琐的滤波和数据融合。

3.系统结构图

在这里插入图片描述

4.特性参数

在这里插入图片描述

5.引脚输出和信号描述

在这里插入图片描述

6.姿态获取与处理

理论上只用陀螺仪就可以完成姿态导航的任务，只需要对3个轴的陀螺仪角度进行积分，得到3个方向的旋转角度的姿态数据，就可以了。但实际上存在着误差噪声等，对陀螺仪积分并不能得到完全准确的姿态，所以我们就需要用加速度计传感器进行辅助矫正。
（1）数据获取
1、MPU6050的陀螺仪采集物体转动的角速度信号，通过ADC（模拟数字转换器）转换成数字信号采集回来。再通过通信传输给单片机。
2、加速度计则是采集物体加速度信号，并传输回来。
（2）数据处理
通过以上的步骤，我们可以分别得出线加速度传感器与角加速度传感器的数据，接下来就要进行数据的处理与融合。

具体步骤如下：
1、校准数据（零点漂移）：传感器安装在设备上总有一个初始的角度，我们设这个角度为0度，我们每一次的数据都要减去这个初始数据，得到一个相对的角度。
2、把测量值换算成相应的单位：原始数据除以它在该量程下的灵敏度就可以获得实际的物理单位。加速度的物理单位为g，角速度的物理单位为°/s。
3、滤波和数据融合：常见方法有三种：互补滤波、卡尔曼滤波、硬件DMP解算四元数。
——互补滤波：因为加速度计有高频噪声，陀螺仪有低频噪声，需要互补滤波融合得到较可靠的角度值。
——卡尔曼滤波：利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。
——硬件DMP解算四元数：DMP将原始数据直接转换成四元数输出，运用欧拉角转换算法，从而得到yaw、roll和pitch。

二、STM32CubeMX配置

本例中依旧使用单片机STM32F405，将测出的加速度值，陀螺仪原始值和DMP解算值显示在OLED显示屏上，显示屏相关代码，请参考本专栏STM32Cube HAL库——OLED（6针）屏幕显示教程这篇文章，烧录接口配置，时钟配置等基础配置，请查看本专栏其它文章。

1.确定引脚

查看原理图，确定MPU6050的SDA,SCL,INT（AD0接地）引脚分别接PB11,PB10,PB15,或者按照自己的需求接线。在这里插入图片描述

在这里插入图片描述

2.CubeMX配置

该程序是在完成调试接口口任务的基础上进行的（所有编程都在这个基础上进行，没有完成则无法烧录）。
蓝牙串口，OLED显示，ADC采集，PWM控制电机，编码器测速都可以在这个专栏中查找到。
注意：MPU6050 DMP姿态解算的步骤较多，细节也很多，主要流程可以阅读此文章，如果不想按照步骤自己完成，也可以下载文末的源码。
1.首先配置PB15引脚。选择GPIO，GPIO的模式和名字如图所示。在这里插入图片描述
2.配置PB11和PB10引脚，这里的通讯协议是IIC。先设置PB10和PB11的引脚分别为I2C2_SCL和I2C2_SDA，如图所示。
参数配置，速度模式为快速模式，I2C时钟频率为400000Hz。具体配置如下图：

至此，MX中的配置完成，生成代码即可。

三、STM32CubeIDE编程及结果。

1.DMP姿态解算代码移植。

DMP解算有现成的代码，这里不需要再自己编写，只需要移植，共8个文件。
在这里插入图片描述
这些文件中最需要注意的是my_mpu6050.c中的w_mpu_read_all_raw_data和read_dmp两个函数，这两个函数分别可以获取原始数据和解算后的数据。

w_mpu_read_all_raw_data函数：（只需要了解获取参数的方式即可，无需深究）
其实，加速度计的值，温度值，陀螺仪值存储起来，我们使用时只需要选择存储的数组（地址）即可。读取MPU所有原始数据的代码如下，注释很详细，不作赘述。

/* ------------------------------------------------ 读取MPU所有原始数据 ------------------------------------------------------------ */
/**
 * @brief  mpu_read_raw_data()  		读取MPU所有原始数据.
 * @param  *mpu_raw_msg_buff       传入接收原始数据的地址
 * @retval HAL status							mpu_ok/mpu_err
 */
mpu_state_t w_mpu_read_all_raw_data(mpu_msg_t *mpu_raw_msg_buff)
{
   
   

	//0x3B, （加速度计 --> 温度 --> 陀螺仪）数据地址先后顺序

	uint8_t msg_buff[14];								//存入缓冲区
	static short mpu_temperature_biff;	//温度缓冲区

	if( w_mpu_read_byte_len(_MPU_RAW_DATA_REG, msg_buff, sizeof(msg_buff)) ==  mpu_err)
		//		return mpu_busy;								//iic忙碌（0x02/0x2）
		return mpu_err;

	//整合MPU原始数据
	mpu_raw_msg_buff->mpu_acce[0] 			= ((uint16_t)msg_buff[0]<<8)	| msg_buff[1];		//X轴加速度值
	mpu_raw_msg_buff->mpu_acce[1]			= ((uint16_t)msg_buff[2]<<8)	| msg_buff[3];		//Y轴加速度值
	mpu_raw_msg_buff->mpu_acce[2]			= ((uint16_t)msg_buff[4]<<8)	| msg_buff[5];		//Z轴加速度值

	mpu_raw_msg_buff->mpu_gyro[0]			= ((uint16_t)msg_buff[8]<<8)	| msg_buff[9];		//X轴陀螺仪值
	mpu_raw_msg_buff->mpu_gyro[1]			= ((uint16_t)msg_buff[10]<<8) | msg_buff[