STM32Cube HAL库——MPU6050 DMP姿态解算
文章主要讲述了mpu6050的姿态获取原理,使用HAL库编程时Cube MX的配置和Cube IDE编程,DMP解算MPU6050姿态的过程,附带开源代码。
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[11]; //Y轴陀螺仪值
mpu_raw_msg_buff->mpu_gyro[2] = ((uint16_t)msg_buff[12]<<8) | msg_buff[13]; //Z轴陀螺仪值
mpu_temperature_biff = ((uint16_t)msg_buff[6]<<8) | msg_buff[7]; //MPU温度值
mpu_raw_msg_buff->mpu_temperature = 36.53f+((double)mpu_temperature_biff)/340.0f; //计算后的真实温度值
return mpu_ok;
}
read_dmp函数:(只需要了解获取参数的方式即可,无需深究)
该函数最主要的是四元数解算,该函数的代码如下:
/* ------------------------------------------------ 读取MPU内置DMP的姿态 ------------------------------------------------------------ */
/**
* @brief read_dmp() 读取MPU内置DMP的姿态.
* @param *pose_msg 传入姿态的地址
* @note 参考野火、原子、平衡之家等开源资料
* @retval HAL status 0/1/2
*/
int i=0;
uint8_t read_dmp(pose_msg_t *pose_msg)
{
uint8_t more;
long quat[4]; //四元数
int16_t gyro[3], accel[3], sensors;
unsigned long sensor_timestamp;//传感器时间戳
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
//读取FIFO中的数据
if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more)){
i=i+1;
printf("%d\r\n",i);
return 1;
}
if(sensors & INV_WXYZ_QUAT)
{
q0=quat[0] / q30;
q1=quat[1] / q30;
q2=quat[2] / q30;
q3=quat[3] / q30;
pose_msg->pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3f; //四元数解算, 俯仰角(Pitch)-->绕着X轴旋转
pose_msg->roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3f; //四元数解算, 翻滚角(Roll)-->绕着Y轴旋转
pose_msg->yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3f; //四元数解算, 偏航角(Yaw)-->绕着Z轴旋转
return 0;
}
else
return 2;
}
/* *********************************************************** 内置DMP的应用 CODE END ************************************************************************ */
2.main函数编程。
(1)初始化:
包含GPIO初始化,IIC初始化(这里我们使用的是IIC2),USART初始化(这里我们使用USART2),OLED初始化。
其中,我们使用了USART串口通信,目的是通过透传在串口调试助手上查看DMP解算过程中代码的运行进展,方便查找和修改代码错误。main中初始化代码如下:
MX_GPIO_Init();
MX_USART2_UART_Init();
MX_I2C2_Init();
OLED_Init();
OLED_Clear();
OLED_Display_On();
最重要的是DMP初始化,在DMP解算MPU6060参数时最先要完成初始化函数。main中DMP初始化语句:
while(w_mpu_init() != mpu_ok)
{
HAL_Delay(100);
}
dmp_init(); //dmp初始化
我们再来查看DMP初始化的函数内容,发现步骤较多,非常容易出现问题,这是就需要串口调试助手,实时查看代码的运行情况。代码如下(非main中函数,只需要了解dmp初始化有哪些步骤,看中文注释即可,无需深究):
void dmp_init(void)
{
uint8_t temp[1]={0};
dmp_i2c_read(HW_S_HW_MPU_ADDR,_MPU_DEVICE_ID,I2C_MEMADD_SIZE_8BIT,temp);
// temp[0] = w_mpu_read_device_address();
printf("mpu_set_sensor complete ......\r\n");
printf("mpu设置传感器完成 ......\r\n");
printf("\r\n");
printf("Device_ID: 0x%x\r\n", temp[0]);
printf("设备ID: 0x%x\r\n", temp[0]);
printf("\r\n");
//检查ID, 不对系统复位
if(temp[0]!=0x68)
NVIC_SystemReset();
if(!mpu_init())
{
//重启内部寄存器到默认,
if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))
{
printf("mpu_set_sensor complete ......\r\n");
// printf("mpu设置传感器完成 ......\r\n");
printf("\r\n");
}
//配置FIFO
if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL))
{
printf("mpu_configure_fifo complete ......\r\n");
// printf("mpu配置FIFO完成 ......\r\n");
printf("\r\n");
}
//配置采样频率分频寄存器
if(!mpu_set_sample_rate(DEFAULT_MPU_HZ))
{
printf("mpu_set_sample_rate complete ......\r\n");
// printf("mpu设定的采样率完成 ......\r\n");
printf("\r\n");
}
//DMP加载动作驱动程序固件完成
if(!dmp_load_motion_driver_firmware())
{
printf("dmp_load_motion_driver_firmware complete ......\r\n");
// printf("DMP加载动作驱动程序固件完成 ......\r\n");
printf("\r\n");
}
//DMP设置陀螺仪方向完成
if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))
{
printf("dmp_set_orientation complete ......\r\n");
// printf("DMP设置陀螺仪方向完成 ......\r\n");
printf("\r\n");
}
//DMP使功能完整
if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
DMP_FEATURE_GYRO_CAL))
{
printf("dmp_enable_feature complete ......\r\n");
// printf("DMP使功能完整 ......\r\n");
printf("\r\n");
}
//DMP设定FIFO速率完成
if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ))
{
printf("dmp_set_fifo_rate complete ......\r\n");
// printf("DMP设定FIFO速率完成 ......\r\n");
printf("\r\n");
}
//自测自身数据,测试通过. 我们可以相信陀螺仪的数据,把它放下来到DMP
run_self_test();
//mpu设置DMP状态完成
if(!mpu_set_dmp_state(1))
{
printf("mpu_set_dmp_state complete ......\r\n");
// printf("mpu设置DMP状态完成 ......\r\n");
printf("\r\n\r\n\r\n");
}
my_hal_delay(200); //等待
printf(" ################### ...... 测试MPU姿态角度 ...... ################### \r\n");
my_hal_delay(200);
printf("\r\n\r\n\r\n");
//先读取姿态角一段时间, 先稳定
for(uint16_t rd=0; rd<200; rd++) //6s
{
//读取MPU内置DMP的姿态
read_dmp(&mpu_pose_msg);
printf("Pitch: %f\t ", mpu_pose_msg.pitch);
printf("Roll: %f\t ", mpu_pose_msg.roll);
printf("Yaw: %f\t ", mpu_pose_msg.yaw);
printf("\r\n");
my_hal_delay(20);
}
my_hal_delay(200); //等待
printf(" ################### ...... MPU配置完成, DMP功能完成 ...... ################### \r\n");
printf("\r\n\r\n\r\n");
}
}
DMP初始化过程中串口调试助手显示如下,我们可以看到初始化每个步骤都完成了,MPU配置完成,DMP功能完成。接下来执行其它功能。
(2)读取参数的函数
在while循环中首先添加以下代码,这两个函数可以获取加速度值,陀螺仪值,解算值和温度值。
read_dmp(&mpu_pose_msg);//读取mpu姿态
w_mpu_read_all_raw_data(&mpu_raw_msg); //读取mpu原始数据
3.加速度值OLED显示
代码如下:
OLED_ShowString(0,3,"Ax:");
OLED_ShowFloat(mpu_raw_msg.mpu_acce[0], 1, 20, 0);
OLED_ShowString(0,18,"Ay:");
OLED_ShowFloat(mpu_raw_msg.mpu_acce[1], 1, 20, 15);
OLED_ShowString(0,33,"Az:");
OLED_ShowFloat(mpu_raw_msg.mpu_acce[2], 1, 20, 30);
OLED_ShowString(0,48,"Temp:");
OLED_ShowFloat(mpu_raw_msg.mpu_temperature, 1, 50, 45);//温度
HAL_Delay(100);
OLED_Refresh_Gram();
实测效果如下:
4.陀螺仪值OLED显示
代码如下:
OLED_ShowString(0,3,"Gx:");
OLED_ShowFloat(mpu_raw_msg.mpu_gyro[0], 1, 20, 0);
OLED_ShowString(0,18,"Gy:");
OLED_ShowFloat(mpu_raw_msg.mpu_gyro[1], 1, 20, 15);
OLED_ShowString(0,33,"Gz:");
OLED_ShowFloat(mpu_raw_msg.mpu_gyro[2], 1, 20, 30);
OLED_ShowString(0,48,"Temp:");
OLED_ShowFloat(mpu_raw_msg.mpu_temperature, 1, 50, 45);//温度
HAL_Delay(100);
OLED_Refresh_Gram();
实测效果如下:
5.DMP解算值OLED显示
代码如下:
OLED_ShowString(0,3,"Pitch:");
OLED_ShowFloat(mpu_pose_msg.pitch, 1, 50, 0);
OLED_ShowString(0,18,"Roll:");
OLED_ShowFloat(mpu_pose_msg.roll, 1, 50, 15);
OLED_ShowString(0,33,"Yaw:");
OLED_ShowFloat(mpu_pose_msg.yaw, 1, 50, 30);
OLED_ShowString(0,48,"Temp:");
OLED_ShowFloat(mpu_raw_msg.mpu_temperature, 1, 50, 45);//温度
HAL_Delay(100);
OLED_Refresh_Gram();
实测效果如下:
四、注意事项:
1、想要完成这个项目,需要学会使用蓝牙透传模块,OLED显示屏。
2、该程序,DMP初始化需要一小段时间,大约6s,从初始化完成到OLED显示屏显示大约6s,烧录代码后,按下复位键等少许片刻。
3、参数刷新频率可按照自己的需求设置。
4、上传的代码中也掺杂了其它未提及的代码,但已被注释或删减。
五、总结
该文章主要讲述了mpu6050的姿态获取原理,使用HAL库编程时Cube MX的配置和Cube IDE编程,对于只实现功能的学习者了解DMP解算的实现流程,知道不同参数的实际意义,可获取参数即可。如想要进一步理解解算过程,可查看代码中的注释(代码中注释较为详细)。
大家一起学习一起进步,变得更强,希望大家点赞、关注或打赏,下期将带来PID控制相关的内容。
源码下载连接:STM32Cube HAL库-MPU6050 DMP姿态解算源码
更多推荐
所有评论(0)