目录
1. 硬件与 DMP 简介
MPU-9250 特性
DMP 作用
2. 软件整体架构
3. 代码实现（STM32 HAL 库）
3.1 I²C 底层驱动
3.2 MPU-9250 与 DMP 初始化
3.3 读取 DMP 姿态数据
3.4 主函数与串口输出
4. DMP 库移植说明
5. 关键优化与注意事项
6. 进阶应用
MPU-9250 9 轴传感器的融合解算代码，包含DMP（Digital Motion Processor）库的移植和姿态角（Pitch/Roll/Yaw）输出，适用于 STM32/CH32 平台，使用 I²C 通信。
1. 硬件与 DMP 简介
MPU-9250 特性
- 内部集成：
- 3 轴加速度计
- 3 轴陀螺仪
- 3 轴磁力计（AK8963）
- 内置DMP（数字运动处理器），可直接输出四元数、欧拉角等融合数据
- 通信接口：I²C / SPI
DMP 作用
- 减轻 MCU 负担，在传感器内部完成：
- 加速度计 + 陀螺仪 + 磁力计 数据融合
- 姿态解算（四元数、欧拉角）
- 自动校准与误差补偿
- 输出频率最高可达 200Hz
2. 软件整体架构
┌─────────────────┐ │ 硬件层：I2C驱动 │ ← 与MPU-9250、AK8963通信 └────────┬────────┘ │ ┌────────▼────────┐ │ 驱动层：MPU-9250初始化 │ ← 配置采样率、量程、DMP └────────┬────────┘ │ ┌────────▼────────┐ │ DMP层：固件加载与启动 │ ← 官方DMP库移植 └────────┬────────┘ │ ┌────────▼────────┐ │ 数据层：姿态数据读取 │ ← 四元数 / 欧拉角 └────────┬────────┘ │ ┌────────▼────────┐ │ 应用层：串口输出 │ ← Pitch/Roll/Yaw └─────────────────┘3. 代码实现（STM32 HAL 库）
这里使用 STM32F103 + HAL 库，I²C1 通信，串口 1 输出。
3.1 I²C 底层驱动
#include "stm32f1xx_hal.h" extern I2C_HandleTypeDef hi2c1; #define MPU9250_I2C &hi2c1 // I2C写寄存器 void I2C_WriteReg(uint8_t addr, uint8_t reg, uint8_t data) { HAL_I2C_Mem_Write(MPU9250_I2C, addr<<1, reg, I2C_MEMADD_SIZE_8BIT, &data, 1, HAL_MAX_DELAY); } // I2C读寄存器 uint8_t I2C_ReadReg(uint8_t addr, uint8_t reg) { uint8_t data; HAL_I2C_Mem_Read(MPU9250_I2C, addr<<1, reg, I2C_MEMADD_SIZE_8BIT, &data, 1, HAL_MAX_DELAY); return data; } // 多字节读取 void I2C_ReadRegs(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf) { HAL_I2C_Mem_Read(MPU9250_I2C, addr<<1, reg, I2C_MEMADD_SIZE_8BIT, buf, len, HAL_MAX_DELAY); }3.2 MPU-9250 与 DMP 初始化
#include "mpu9250.h" #include "inv_mpu.h" #include "inv_mpu_dmp_motion_driver.h" #define MPU9250_ADDR 0x68 // AD0接地 uint8_t mpu_init(void) { int result; // 初始化MPU9250 if (mpu_init(NULL) != 0) return 1; // 配置陀螺仪和加速度计 mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); mpu_set_sample_rate(100); // 100Hz // 加载DMP固件 if (dmp_load_motion_driver_firmware() != 0) return 2; // 启用DMP dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_GYRO_CAL | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO); dmp_set_fifo_rate(100); // 与采样率一致 // 启动DMP if (mpu_set_dmp_state(1) != 0) return 3; return 0; }3.3 读取 DMP 姿态数据
#include <math.h> #define RAD_TO_DEG 57.29578f typedef struct { float pitch; float roll; float yaw; } EulerAngles; EulerAngles euler; void read_dmp_data(void) { long sensor_timestamp; short gyro[3], accel[3], sensors; unsigned char more; long quat[4]; // 读取DMP数据 if (dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more)) return; // 转换四元数到欧拉角 float q0 = quat[0] / 16384.0f; float q1 = quat[1] / 16384.0f; float q2 = quat[2] / 16384.0f; float q3 = quat[3] / 16384.0f; euler.pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * RAD_TO_DEG; euler.roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * RAD_TO_DEG; euler.yaw = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * RAD_TO_DEG; }3.4 主函数与串口输出
#include "stdio.h" extern UART_HandleTypeDef huart1; int fputc(int ch, FILE *f) { HAL_UART_Transmit(&huart1, (uint8_t*)&ch, 1, HAL_MAX_DELAY); return ch; } int main(void) { HAL_Init(); SystemClock_Config(); MX_GPIO_Init(); MX_I2C1_Init(); MX_USART1_UART_Init(); if (mpu_init() != 0) { printf("MPU9250 init failed!\r\n"); while (1); } while (1) { read_dmp_data(); printf("Pitch: %.1f\tRoll: %.1f\tYaw: %.1f\r\n", euler.pitch, euler.roll, euler.yaw); HAL_Delay(10); // 100Hz输出 } }4. DMP 库移植说明
获取 DMP 库
- 从 InvenSense 官方 GitHub 或 ST 提供的 MPU-9250 驱动包获取：
inv_mpu.hinv_mpu_dmp_motion_driver.h- 及其
.c文件
- 这些文件实现了 DMP 固件加载、FIFO 读取、四元数计算等功能。
- 从 InvenSense 官方 GitHub 或 ST 提供的 MPU-9250 驱动包获取：
适配 I²C
- 在
inv_mpu.h中修改 I²C 读写函数，指向你实现的I2C_WriteReg/I2C_ReadRegs。
- 在
配置编译选项
- 添加宏
MPU9250以启用磁力计支持。 - 在编译器中添加浮点支持（
-mfpu=fpv4-sp-d16 -mfloat-abi=hard针对 Cortex-M4）。
- 添加宏
5. 关键优化与注意事项
- 磁力计校准
- DMP 内置磁力计校准，但首次使用建议在无磁干扰环境下做一次八位置校准。
- 数据滤波
- 对输出的欧拉角可加滑动平均滤波，减少抖动。
- 动态性能
- DMP 融合在动态环境下比纯加速度计 + 磁力计稳定，但高速运动仍需结合外部算法（如扩展卡尔曼滤波）。
- 电源与布线
- MPU-9250 对电源噪声敏感，需加去耦电容，磁力计远离电机 / 强磁场。
6. 进阶应用
- 无人机姿态控制：将欧拉角输入 PID 控制器，控制电机转速。
- VR/AR 头戴设备：高刷新率姿态输出，实时显示头部方向。
- 机器人平衡：结合陀螺仪和加速度计，实现自平衡。