背景
简单记录下STM32与MPU6050通信实现中遇到的一些坑。目前网上能查到的资料基本都是软件模拟I2C，并且DMP driver的版本较旧。我这里直接使用Cube IDE 生成的HAL库I2C通信，DMP motion driver 使用5.1.3版本。
开发环境
- Ubuntu 20.04 LTS
- STM32CubeIDE 1.9.0
- MPU6050 DMP 驱动目前最新版本 motion_driver 5.1.3 Software Downloads | InvenSense Developers
- 芯片STM32 F103C8T6
移植motion_driver 5.1.3
移植过程主要参考了B站这个视频 STM32移植MPU6050DMP库1_哔哩哔哩_bilibili 感谢UP主 一键三连这次一定。
复制 motion_driver-5.1.3/core/driver/eMPL/下的6个文件
- dmpKey.h
- inv_mpu.c
- inv_mpu_dmp_motion_driver.h
- dmpmap.h
- inv_mpu_dmp_motion_driver.c
- inv_mpu.h
修改inv_mpu.h 和 inv_mpu.c
1. 在inv_mpu.h新增STM32设备的宏定义
#define STM32_MPU6050 //新增32设备宏定义 在.c中改写原msp430的宏
#define MPU6050 //必须这样命名 用于驱动库文件中开启MPU60502. 在inv_mpu.c 改写msp430的宏，改为头文件新增的STM32_MPU6050。去掉msp430相关头文件及结构体。
- 新增包含HAL库的i2c.h
- 用HAL库的I2C读写函数HAL_I2C_Mem_Write和HAL_I2C_Mem_Read改写原 i2c_write和i2c_read宏函数。这里需要注意，inv_mpu.c 第491行，对MPU6050地址的定义的是0x68，i2c需要高七位地址，改写读写函数时需要左移1位。如果MPU6050的AD0引脚接VDD时，需要修改成0x69
#if defined STM32_MPU6050 //新增32设备宏定义
// 去掉MSP430的头文件
//#include "msp430.h"
//#include "msp430_i2c.h"
//#include "msp430_clock.h"
//#include "msp430_interrupt.h"
// 用HAL库重写宏
#define i2c_write(mpuaddr, regaddr, dsize, pdata) HAL_I2C_Mem_Write(&hi2c1, mpuaddr << 1, regaddr, I2C_MEMADD_SIZE_8BIT, pdata, dsize, HAL_MAX_DELAY)
#define i2c_read(mpuaddr, regaddr, dsize, pdata) HAL_I2C_Mem_Read(&hi2c1, mpuaddr << 1, regaddr, I2C_MEMADD_SIZE_8BIT, pdata, dsize, HAL_MAX_DELAY)
#define delay_ms(ms) HAL_Delay(ms)
#define get_ms(p) do { *p = HAL_GetTick();} while(0)
// 去掉MSP430相关结构体
//static inline int reg_int_cb(struct int_param_s *int_param)
//{
// return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit,
// int_param->active_low);
//}
#define log_i(...) do {} while (0)
#define log_e(...) do {} while (0)
/* labs is already defined by TI's toolchain. */
/* fabs is for doubles. fabsf is for floats. */
#define fabs fabsf
#define min(a,b) ((a<b)?a:b)写用于初始化及读取数据的MPU6050.h MPU6050.c
1. 初始化函数 int MPU6050_Init(void);
参照官方例程 motion_driver-5.1.3/simple_apps/msp430/ motion_driver_test.c 写初始化函数 int MPU6050_Init(void)
int MPU6050_Init(void)
{
int result;
struct int_param_s int_param;
struct hal_s hal = {0};
HAL_Delay(100); // 一定要加延时！！
result = mpu_init(&int_param); //MPU 初始化
if (result != 0)
return -1;
result = mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL); // 设置传感器
if (result != 0)
return -2;
result = mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); // 设置fifo
if (result != 0)
return -3;
result = mpu_set_sample_rate(DEFAULT_MPU_HZ); // 设置采样率
if (result != 0)
return -4;
result = dmp_load_motion_driver_firmware(); // 加载DMP固件
if (result != 0)
return -5;
result = dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)); // 设置陀螺仪方向
if (result != 0)
return -6;
hal.dmp_features = 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;
result = dmp_enable_feature(hal.dmp_features); // 设置DMP功能
if (result != 0)
return -7;
result = dmp_set_fifo_rate(DEFAULT_MPU_HZ); // 设置输出速率
if (result != 0)
return -8;
result = run_self_test(); // 自检
if (result != 0)
return -9;
result = mpu_set_dmp_state(1); // 使能DMP
if (result != 0)
return -10;
return 0;
}- 特别注意，我一直遇到 mpu_init 初始化失败的情况。在 mpu_init 之前加入100毫秒延时后成功初始化。大概是MPU上电后需要等待一定时间
- 从 inv_mpu.h 复制结构体 int_param_s，用于初始化函数 mpu_init
- 为采样率设置函数 mpu_set_sample_rate 定义 #define DEFAULT_MPU_HZ (100) 其频率可设置区间为4Hz 至 1kHz
- 为方向设置函数 dmp_set_orientation 从官方例程 motion_driver_test.c 中复制函数inv_row_2_scale 和 inv_orientation_matrix_to_scalar 及数组 gyro_orientation[9]
- 增加自检函数，从官方例程 motion_driver_test.c 中复制函数 run_self_test ，特别注意UP主的视频及网上的资料基本使用的是老的driverv版本，在12行判断result处，老版本需要改为0x03，但在motion_driver 5.1.3中，在 inv_mpu.c 的2747行我们可以看到默认 result |= 0x04; 这样结果还是0x07，不需要修改官方例程的自检函数了
static int run_self_test(void)
{
int result;
long gyro[3], accel[3];
unsigned char i = 0;
#if defined (MPU6500) || defined (MPU9250)
result = mpu_run_6500_self_test(gyro, accel, 0);
#elif defined (MPU6050) || defined (MPU9150)
result = mpu_run_self_test(gyro, accel);
#endif
if (result == 0x07) //判断返回值 新版默认|0x04，不需要由0x07修改至0x03
{
/* Test passed. We can trust the gyro data here, so let's push it down
* to the DMP.
*/
for(i = 0; i<3; i++)
{
gyro[i] = (long)(gyro[i] * 32.8f); //convert to +-1000dps
accel[i] *= 2048.f; //convert to +-16G
accel[i] = accel[i] >> 16;
gyro[i] = (long)(gyro[i] >> 16);
}
mpu_set_gyro_bias_reg(gyro);
#if defined (MPU6500) || defined (MPU9250)
mpu_set_accel_bias_6500_reg(accel);
#elif defined (MPU6050) || defined (MPU9150)
mpu_set_accel_bias_6050_reg(accel);
#endif
}
else
{
return -1;
}
return 0;
}2. 读数据函数int MPU6050_DMP_GetData
通过inv_mpu_dmp_motion_driver.c 的 dmp_read_fifo (gyro, accel, quat, timestamp, sensors, more)函数读取四元数数据。相应的参数：
- gyro 为陀螺仪数据，需传入三个 short 型元素的数组
- accel 加速度数据，需传入三个 short 型元素的数组
- quat 四元数数据，需传入四个 long 型元素的数组
- timestamp 毫秒时间戳 usigned long 型
- sensors 标志位，short型
- more unsigned char 型，返回FIFO中剩余的数据包个数
int MPU6050_DMP_GetData(long *quat, double *pitch, double *roll, double *yaw)
{
short gyro[3];
short accel[3];
double q[3] = {0.0f};
unsigned long timestamp;
short sensors;
unsigned char more;
while (dmp_read_fifo(gyro, accel, quat, ×tamp, &sensors, &more));
if (sensors & INV_WXYZ_QUAT)
{
for (short i = 0; i < 4; i++)
q[i] = quat[i] / Q30;
*pitch = asin(-2 * q[1] * q[3] + 2 * q[0] * q[2]) * 57.3;
*roll = atan2(2 * q[2] * q[3] + 2 * q[0] * q[1], -2 * q[1] * q[1] - 2 * q[2] * q[2] + 1) * 57.3;
*yaw = atan2(2 * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]) * 57.3;
}
return 0;
}- 使用dmp_read_fifo函数读取数据，进行判断，其值不为0时，读取成功，失败返回-1。特别注意这里常出现的错误是FIFO溢出，读不到数据。主要是由于使用延迟函数和DMP速率设置等问题导致的。UP主的视频中使用一次 if 判断，我调试时经常会卡在这里。进入dmp_read_fifo 我们可以看到在 mpu_read_fifo_stream 函数中，失败会调用 mpu_reset_fifo() 重置 fifo，所以这里我使用while循环，直到读取成功后向后进行
- 判断sensors & INV_WXYZ_QUAT == 1时 即得到 quat 数据，根据需要，这里可以将四元数转换成欧拉角，公式如程序所列。四元数数据是Q30格式的，使用欧拉角公式时候，需转成浮点型数据。特别注意，UP主及网上大部分的代码,使用的是 flot 型存储欧拉角，但是我的环境下 flot 会发生溢出导致欧拉角数据都是0，这里应使用 double 型
结语
整个调试的过程比较曲折，文中标黄的特别注意都是我踩坑的地方，特写此文记录下，也希望能帮到更多的人。目前还没有解决的问题是使用I2C中断通讯，会导致mpu_init 初始化失败，还在尝试。
扫一扫
261
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
