当前位置：首页 > news >正文

【免费开源】STM32F103C8T6移植DMP解算MPU9250 - 少年

news 2026/3/26 22:48:55

项目说明

本文详细介绍了基于HAL库移植MPU9250 DMP（数字运动处理器）的完整流程，包括DMP库下载、关键文件修改、I2C接口适配及STM32平台移植步骤，并提供了姿态解算代码与常见问题调试方法。

文章相关工程文件的开源文件获取链接见文章末尾。

文献参考以及说明

移植MPU9250的DMP，参考下面的文章，以及正点原子的移植项目工程文件。但是移植到最后使用还是有部分问题，问题内容将在最后进行说明。

https://blog.csdn.net/weixin_45682654/article/details/136244101

https://blog.csdn.net/Rare_Hunter/article/details/134200468

使用的是HAL库，关于I2C初始化一类的处理，这里不做讲解，需要保证你有以下接口的IIC函数。且能正常使用

int mpu9250_write_bytes(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *data);
int mpu9250_read_bytes(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *data);

简单描述一下我这两个函数的封装逻辑。首先封装了一个封装的HAL库。

int mpu9250_write_bytes(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *data)
{u_i2c1_write_bytes(addr, reg, data ,len);return 0;
}int mpu9250_read_bytes(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *data)
{u_i2c1_read_bytes(addr,reg,data,len);return 0;
}

然后每个函数内部的实现函数如下：

void u_i2c1_write_bytes(unsigned char add,unsigned char reg,unsigned char *data,unsigned char len)
{	HAL_I2C_Mem_Write(&hi2c1, (add<<1), reg, I2C_MEMADD_SIZE_8BIT, data,len,HAL_MAX_DELAY);
}void u_i2c1_read_bytes(unsigned char add,unsigned char reg,unsigned char *data,unsigned char len)
{		HAL_I2C_Mem_Read(&hi2c1, (add<<1), reg,I2C_MEMADD_SIZE_8BIT, data, len, HAL_MAX_DELAY);  
}

具体的实现方法可以去我的开源代码里面查看，开源代码中不仅包含DMP库的解算，还有使用卡尔曼滤波，互补滤波，Mahony姿态解算以及Madgwick的解算方式。

下载DMP库

进入官网：https://invensense.tdk.com/

先注册，注册完成后需要邮箱里面点击他发给你的链接进行一个验证。注册密码需要有特殊字符、大小写、数字。登录完成后点击下图。

点进去后滑动到最底下，找到MPU栏目。点击压缩包下载。

DMP移植

在下载的项目包中，找到和DMP解算相关的代码。将以下内容中全部文件都加入在项目中去，处理好的文件内容可以直接下载我的开源文件查看，以下步骤只是为了方便解释内容。

加载的时候，在mpl文件夹中有一个libmpllib.a文件，该文件需要用实际的库替换，

如图所示，找到对应的lib文件，去替换，我这里使用的是STM32F103，所以应该使用M3的库文件，解压后将其替换刚刚的libmpllib.a文件。

所有文件加载进去后，目录如下，其中inv_mpu_stm32port.c文件暂时不用管，该部分内容只是为了提供一个计算的接口，后面会做说明。

接下来就是代码处理，首先是inv_mpu.c，我会使用注释表示要添加的内容。在开头的部分，我们将I2C处理的头文件包含进来，我这里I2C处理部分封装在了#include "mpu6050.h"，STM32_MPU6050，是为了定义我们自己的操作函数。

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "inv_mpu.h"/*********************用户处理部分 start**********************/
#include "mpu9250.h"
#include "usart.h"#define STM32_MPU9250  //开启自定义的函数宏定义
#define MPU9250        //使用MPU9250相关的处理函数
/*********************用户处理部分 end  **********************/

然后紧接着添加I2C处理部分的代码。下面多余的部分只是为了方便定位说明，在40几行左右。这部分的处理，需要注意i2c_write和i2c_read的函数格式，格式内容在源文件注释里面有。

/* The following functions must be defined for this platform:* i2c_write(unsigned char slave_addr, unsigned char reg_addr,              ###   格式*      unsigned char length, unsigned char const *data)* i2c_read(unsigned char slave_addr, unsigned char reg_addr,               ###   格式*      unsigned char length, unsigned char *data)* delay_ms(unsigned long num_ms)* get_ms(unsigned long *count)* reg_int_cb(void (*cb)(void), unsigned char port, unsigned char pin)* labs(long x)* fabsf(float x)* min(int a, int b)*/
/*********************用户处理部分 修改函数宏定义 start**********************/
#if defined STM32_MPU9250
#include "log.h"
unsigned char *mpl_key = (unsigned char*)"eMPL 5.1";
#define i2c_write   mpu9250_write_bytes
#define i2c_read    mpu9250_read_bytes
#define delay_ms    HAL_Delay
#define get_ms      mget_ms#define log_i       printf
#define log_e       printf
/* 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)static inline int reg_int_cb(struct int_param_s *int_param)
{return NULL;
}//空函数,未用到.
static void mget_ms(unsigned long *time)
{}
/*********************用户处理部分 修改函数宏定义 end**********************/
#elif defined EMPL_TARGET_STM32F4

然后在inv_mpu.h中，给int_param_s添加一个指针变量。

struct int_param_s {
#if defined EMPL_TARGET_MSP430 || defined MOTION_DRIVER_TARGET_MSP430void (*cb)(void);unsigned short pin;unsigned char lp_exit;unsigned char active_low;
#elif defined EMPL_TARGET_UC3L0unsigned long pin;void (*cb)(volatile void*);void *arg;
#elif defined EMPL_TARGET_STM32F4void (*cb)(void);
/***********  添加   *************/
#else  void (*cb)(void);
/*********************************/
#endif
};

同样在inv_mpu_dmp_motion_driver.c文件中也修改为自己的操作函数，并将一个__no_operation()函数注释。

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "dmpKey.h"
#include "dmpmap.h"/*********************用户处理部分 start**********************/
#include "mpu9250.h"#define STM32_MPU9250  //开启自定义的函数宏定义
#define MPU9250        //使用MPU9250相关的处理函数
/*********************用户处理部分 end  **********************//* The following functions must be defined for this platform:* i2c_write(unsigned char slave_addr, unsigned char reg_addr,*      unsigned char length, unsigned char const *data)* i2c_read(unsigned char slave_addr, unsigned char reg_addr,*      unsigned char length, unsigned char *data)* delay_ms(unsigned long num_ms)* get_ms(unsigned long *count)*/
/*********************用户处理部分 修改函数宏定义 start**********************/
#if defined STM32_MPU9250#define delay_ms       HAL_Delay
#define get_ms         mget_ms
#define log_i          printf
#define log_e          printfstatic void mget_ms(unsigned long *time)
{}
/*********************用户处理部分 修改函数宏定义 end**********************/
#elif defined EMPL_TARGET_STM32F4

然后修改log_stm32.c文件，首先，在开头处，将他包含的stm32f4xx.h去除，并修改成包含了stm32f1处理库的main.h，如果你是标准库，就是其他的文件。

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>#include "packet.h"
#include "log.h"
+ #include "main.h"
+ #include "usart.h"

然后修改下面函数中的三个函数的fputs的代码。

int _MLPrintLog (int priority, const char* tag, const char* fmt, ...)
{
+	FILE * noUse;/***************///  原代码部分  ///***************/for (ii = 0; ii < length; ii += (PACKET_LENGTH-5)) {
#define min(a,b) ((a < b) ? a : b)this_length = min(length-ii, PACKET_LENGTH-5);memset(out+3, 0, 18);memcpy(out+3, buf+ii, this_length);for (i=0; i<PACKET_LENGTH; i++) {
/*************** 修改处 ****************/
+		  fputc(out[i],noUse);
/**************************************/}}va_end(args);return 0;
}void eMPL_send_quat(long *quat)
{
+	FILE * noUse;/***************///  原代码部分  ///***************/for (i=0; i<PACKET_LENGTH; i++) {
/*************** 修改处 ****************/
+      fputc(out[i],noUse);
/**************************************/}
}void eMPL_send_data(unsigned char type, long *data)
{
+	FILE * noUse;/***************///  原代码部分  ///***************/for (i=0; i<PACKET_LENGTH; i++) {
/*************** 修改处 ****************/
+      fputc(out[i],noUse);
/**************************************/}
}

接着需要在项目配置的宏定义处，加上EMPL。

DMP使用

然后新建一个方便其他文件使用的接口文件inv_mpu_stm32port.c。

#include "inv_mpu_stm32port.h"#include <math.h>
#include "inv_mpu.h"
#include "mpl.h"
#include "invensense.h"
#include "invensense_adv.h"
#include "data_builder.h"
#include "eMPL_outputs.h"
#include "mltypes.h"
#include "mpu.h"
#include "log.h"
#include "packet.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "stdio.h"#define DEFAULT_MPU_HZ  (100)
#define COMPASS_READ_MS (100)
#define Q30  1073741824.0f
#define Q16  65536.0f/* 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.*/
/* (使用Ai简易翻译了一下原注释)* 传感器可以以任何方向安装到板上。* 下面的安装矩阵告诉MPL如何从驱动程序旋转原始数据。* TODO: 下面的矩阵指的是Invensense内部测试板上的配置。* 如果需要，请修改矩阵以匹配您特定设置的芯片到本体矩阵。
*/
static signed char gyro_orientation[9] = {-1, 0, 0,0,-1, 0,0, 0, 1};
//磁力计方向设置
static signed char comp_orientation[9] = { 0, 1, 0,1, 0, 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.*/
/* (使用Ai简易翻译了一下原注释)* 以下这两个函数将方向矩阵（参见gyro_orientation）转换为标量表示，以供DMP使用。* 注释：这些函数是从Invensense的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;elseb = 7;      // errorreturn b;
}static unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx)
{unsigned short scalar;/*XYZ  010_001_000 Identity MatrixXZY  001_010_000YXZ  010_000_001YZX  000_010_001ZXY  001_000_010ZYX  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;
}/*** @brief   自检测试* @param    * @retval  void**/
static int run_self_test(void)
{int result;long gyro[3], accel[3];result = mpu_run_self_test(gyro, accel);//result = mpu_run_6500_self_test(gyro, accel,0);if (result == 0x7) {/* 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;
}/*** @brief   初始化MPU6050的DMP相关配置* @param    * @retval  void**/
int mpu_dmp_init(void)
{uint8_t res=0;struct int_param_s int_param;unsigned char accel_fsr;unsigned short gyro_rate, gyro_fsr;unsigned short compass_fsr;if(mpu_init(&int_param)==0)	//初始化MPU9250{	 res=inv_init_mpl();     //初始化MPLif(res)return 1;inv_enable_quaternion();inv_enable_9x_sensor_fusion();inv_enable_fast_nomot();inv_enable_gyro_tc();inv_enable_vector_compass_cal();inv_enable_magnetic_disturbance();inv_enable_eMPL_outputs();res=inv_start_mpl();    //开启MPLif(res)return 1;res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL|INV_XYZ_COMPASS);//设置所需要的传感器if(res)return 2; res=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);   //设置FIFOif(res)return 3; res=mpu_set_sample_rate(DEFAULT_MPU_HZ);	            //设置采样率if(res)return 4; res=mpu_set_compass_sample_rate(1000/COMPASS_READ_MS);  //设置磁力计采样率if(res)return 5;mpu_get_sample_rate(&gyro_rate);mpu_get_gyro_fsr(&gyro_fsr);mpu_get_accel_fsr(&accel_fsr);mpu_get_compass_fsr(&compass_fsr);inv_set_gyro_sample_rate(1000000L/gyro_rate);inv_set_accel_sample_rate(1000000L/gyro_rate);inv_set_compass_sample_rate(COMPASS_READ_MS*1000L);inv_set_gyro_orientation_and_scale(inv_orientation_matrix_to_scalar(gyro_orientation),(long)gyro_fsr<<15);inv_set_accel_orientation_and_scale(inv_orientation_matrix_to_scalar(gyro_orientation),(long)accel_fsr<<15);inv_set_compass_orientation_and_scale(inv_orientation_matrix_to_scalar(comp_orientation),(long)compass_fsr<<15);res=dmp_load_motion_driver_firmware();		             //加载dmp固件if(res)return 6; res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向if(res)return 7; 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 8; res=dmp_set_fifo_rate(DEFAULT_MPU_HZ);	//设置DMP输出速率(最大不超过200Hz)if(res)return 9;   res=run_self_test();		//自检if(res)return 10;    res=mpu_set_dmp_state(1);	//使能DMPif(res)return 11;     }return 0;
}/*** @brief   读取四元数值并计算得到实际的角度值* @param    * @retval  void**/
int mpu_dmp_get_data(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, &timestamp, &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;
}int mpu_mpl_get_data(float *pitch,float *roll,float *yaw)
{unsigned long sensor_timestamp;short gyro[3], accel_short[3],compass_short[3],sensors;unsigned char more;long compass[3],accel[3],quat[4],temperature; long data[9];inv_time_t timestamp;int8_t accuracy;if(dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors,&more))return 1;	 if(sensors&INV_XYZ_GYRO){inv_build_gyro(gyro,sensor_timestamp);          //把新数据发送给MPLmpu_get_temperature(&temperature,&sensor_timestamp);inv_build_temp(temperature,sensor_timestamp);   //把温度值发给MPL，只有陀螺仪需要温度值}if(sensors&INV_XYZ_ACCEL){accel[0] = (long)accel_short[0];accel[1] = (long)accel_short[1];accel[2] = (long)accel_short[2];inv_build_accel(accel,0,sensor_timestamp);      //把加速度值发给MPL}if (!mpu_get_compass_reg(compass_short, &sensor_timestamp)) {compass[0]=(long)compass_short[0];compass[1]=(long)compass_short[1];compass[2]=(long)compass_short[2];inv_build_compass(compass,0,sensor_timestamp); //把磁力计值发给MPL}inv_execute_on_data();inv_get_sensor_type_euler(data,&accuracy,&timestamp);*roll  = (data[0]/Q16);*pitch = -(data[1]/Q16);*yaw   = -data[2] / Q16;return 0;
}

并且在inv_mpu_stm32port.h中进行声明。

#ifndef _INV_MPU_STM32PORT_H
#define _INV_MPU_STM32PORT_H#include "main.h"int mpu_dmp_init(void);
int mpu_dmp_get_data(float *pitch, float *roll, float *yaw);
int mpu_mpl_get_data(float *pitch,float *roll,float *yaw);#endif

然后就可以直接使用了

static void cal_with_dmp()
{int count = 0;unsigned long timestamp;mpu_dmp_init();while(1){HAL_Delay(10);if(mpu_dmp_get_data(&mpu9250_data.anglePitch,&mpu9250_data.angleRoll,&mpu9250_data.angleYaw)==0){ mpu9250_get_gyro(&mpu9250_data.gyro[0],&mpu9250_data.gyro[1],&mpu9250_data.gyro[2]);mpu9250_get_acc (&mpu9250_data.acc[0] ,&mpu9250_data.acc[1] ,&mpu9250_data.acc[2]);//mpu9250_get_mag(&mpu9250_data.mag[0],&mpu9250_data.mag[1],&mpu9250_data.mag[2]);mpu_get_compass_reg(mpu9250_data.mag, &timestamp);//做单位解算 量程转换mpu9250_data.gyroxReal = mpu9250_data.gyro[0] * MPU9250_GYRO_2000_SEN;mpu9250_data.gyroyReal = mpu9250_data.gyro[1] * MPU9250_GYRO_2000_SEN;mpu9250_data.gyrozReal = mpu9250_data.gyro[2] * MPU9250_GYRO_2000_SEN;mpu9250_data.accxReal  = mpu9250_data.acc[0]  * MPU9250_ACCEL_2G_SEN;mpu9250_data.accyReal  = mpu9250_data.acc[1]  * MPU9250_ACCEL_2G_SEN;mpu9250_data.acczReal  = mpu9250_data.acc[2]  * MPU9250_ACCEL_2G_SEN;count ++;if(count % 100 == 0){printf("%f, %f, %f\r\n",mpu9250_data.anglePitch,mpu9250_data.angleRoll,mpu9250_data.angleYaw);}}}
}