代码收藏家 STM32 HAL库成功移植MPU6050 DMP库的详细教程
STM32移植MPU6050 DMP库（跟着做就能成功）
0.前言
最近捣鼓FreeRTOS时需要使用一个MPU6050姿态传感器，到处扒拉例程时发现各式各样的教程都不太满意，感觉差点意思，所以自己慢慢摸索整了一个移植教程，有疏漏的地方还请各位读者进行指正。
一、使用CubeMX建立工程
有关的理论部分这里就不进行赘述了，此篇文章目的是实现STM32F103RCT6驱动MPU6050传感器，并通过串口发送解析完成的数据，在I2C1总线上挂载MPU6050外设和OLED屏幕，实时读取并显示数据。
以下是CubeMX的配置：
使能I2C1：
使能UART1串口：
时钟及其他外设请按需使能，这里笔者额外添加了FreeRTOS的CMSIS_V2版本，实际并未使用相关组件，可忽略。
二、移植DMP库
MotionDriver_V6.1下载：https://os.mbed.com/users/oprospero/code/MotionDriver_6_1
下载后会得到一个基于MSP430的工程，主要需要移植以下文件：
需要将红框中的文件添加进工程，motion_driver_test.c为测试程序，不需要添加，不过可以仿照其编写自己的测试程序。
修改：
在官方的MSP430相关的代码基础上进行修改，首先在inv_mpu.h中添加所需要的宏，并排除掉影响不大的报错：
其中STM32_MPU6050为后续需要使用的自定义宏，MPU6050为DMP库中指定的陀螺仪型号，下方的结构体没有使用到，添加此参数排除报错。
在inv_mpu.c中修改原本的宏定义，并实现自己的相关函数：
如果未使用FreeRTOS则将毫秒延时定义为HAL_Delay()函数，并去除此文件中的reg_int_cb()部分（未使用）。
同样在inv_mpu_dmp_motion_driver.c文件中也修改为自己的操作函数，并将__no_operation()函数改为__NOP()：
至此，DMP的初步移植工作已完成，能够编译通过。
注意：
经过笔者实测，如果想要成功读取传感器数据，还需要把inv_mpu.c中的MPU6050设备地址修改为0xD0，否则会找不到设备。
使用
参考motion_driver_test.c文件中的相关流程，新建my_mpu6050.c和my_mpu6050.h文件，实现MPU初始化和数据获取。
my_mpu6050.c：
#include "my_mpu6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "math.h"
/* The sensors can be mounted onto the board in any orientation. The mounting
* matrix seen below tells the MPL how to rotate the raw data from thei
* driver(s).
* TODO: The following matrices refer to the configuration on an internal test
* board at Invensense. If needed, please modify the matrices to match the
* chip-to-body matrix for your particular set up.
*/
static signed char gyro_orientation[9] = {-1, 0, 0,
0,-1, 0,
0, 0, 1};
/* These next two functions converts the orientation matrix (see
* gyro_orientation) to a scalar representation for use by the DMP.
* NOTE: These functions are borrowed from Invensense's MPL.
*/
static 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;
}
static unsigned short inv_orientation_matrix_to_scalar(
const signed char *mtx)
{
unsigned short scalar;
/*
XYZ 010_001_000 Identity Matrix
XZY 001_010_000
YXZ 010_000_001
YZX 000_010_001
ZXY 001_000_010
ZYX 000_001_010
*/
scalar = inv_row_2_scale(mtx);
scalar |= inv_row_2_scale(mtx + 3) << 3;
scalar |= inv_row_2_scale(mtx + 6) << 6;
return scalar;
}
static int run_self_test(void)
{
int result;
long gyro[3], accel[3];
result = mpu_run_self_test(gyro, accel);
if (result == 0x3) {
/* Test passed. We can trust the gyro data here, so let's push it down
* to the DMP.
*/
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);
} else {
return -1;
}
return 0;
}
int MPU6050_DMP_init(void)
{
int ret;
struct int_param_s int_param;
//mpu_init
ret = mpu_init(&int_param);
if(ret != 0)
{
return ERROR_MPU_INIT;
}
//设置传感器
ret = mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
if(ret != 0)
{
return ERROR_SET_SENSOR;
}
//设置fifo
ret = mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
if(ret != 0)
{
return ERROR_CONFIG_FIFO;
}
//设置采样率
ret = mpu_set_sample_rate(DEFAULT_MPU_HZ);
if(ret != 0)
{
return ERROR_SET_RATE;
}
//加载DMP固件
ret = dmp_load_motion_driver_firmware();
if(ret != 0)
{
return ERROR_LOAD_MOTION_DRIVER;
}
//设置陀螺仪方向
ret = dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));
if(ret != 0)
{
return ERROR_SET_ORIENTATION;
}
//设置DMP功能
ret = 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);
if(ret != 0)
{
return ERROR_ENABLE_FEATURE;
}
//设置输出速率
ret = dmp_set_fifo_rate(DEFAULT_MPU_HZ);
if(ret != 0)
{
return ERROR_SET_FIFO_RATE;
}
//自检
ret = run_self_test();
if(ret != 0)
{
return ERROR_SELF_TEST;
}
//使能DMP
ret = mpu_set_dmp_state(1);
if(ret != 0)
{
return ERROR_DMP_STATE;
}
return 0;
}
int MPU6050_DMP_Get_Date(float *pitch, float *roll, float *yaw)
{
float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
short gyro[3];
short accel[3];
long quat[4];
unsigned long timestamp;
short sensors;
unsigned char more;
if(dmp_read_fifo(gyro, accel, quat, ×tamp, &sensors, &more))
{
return -1;
}
if(sensors & INV_WXYZ_QUAT)
{
q0 = quat[0] / Q30;
q1 = quat[1] / Q30;
q2 = quat[2] / Q30;
q3 = quat[3] / Q30;
*pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3; // pitch
*roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3; // roll
*yaw = atan2(2 * (q0 * q3 + q1 * q2), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3; // yaw
}
return 0;
}
my_mpu6050.h：
#ifndef __MY_MPU6050_H__
#define __MY_MPU6050_H__
#define ERROR_MPU_INIT -1
#define ERROR_SET_SENSOR -2
#define ERROR_CONFIG_FIFO -3
#define ERROR_SET_RATE -4
#define ERROR_LOAD_MOTION_DRIVER -5
#define ERROR_SET_ORIENTATION -6
#define ERROR_ENABLE_FEATURE -7
#define ERROR_SET_FIFO_RATE -8
#define ERROR_SELF_TEST -9
#define ERROR_DMP_STATE -10
#define DEFAULT_MPU_HZ 100
#define Q30 1073741824.0f
int MPU6050_DMP_init(void);
int MPU6050_DMP_Get_Date(float *pitch, float *roll, float *yaw);
#endif
然后在主函数中对数据进行获取即可：
到此即移植完成。
三、总结
本文中还存在一些优化空间，例如没有使用DMA进行数据传输，笔者在使用DMA时出现I2C总线读写超时的问题，经过多方查找也没有解决，猜测是每次读写的间隔太小导致，有一种办法是调用HAL_I2C_Mem_Transfer_DMA()后使用while()阻塞等待传输完成信号，不过这样就和直接使用I2C没有区别了，所以最后也没有使用。除此之外也没有对偏航角进行补偿，有需求的还需要自己花时间研究。
后续打算使用拓展卡尔曼进行姿态解算，所以本篇暂时先做到这里。
作者：Amonter
