简介：面向STM32 HAL库开发者与嵌入式姿态解算学习者，这份源码包围绕STM32F405平台实现MPU6050的DMP姿态解算，配套CubeMX配置与CubeIDE工程，可直接用于代码移植与原理理解。包内共446个文件，以78个h头文件、39个c源文件为核心，辅以72个d依赖文件、71个cyclo与71个su编译中间文件、16个mk构建脚本及ioc工程配置、ld链接脚本、elf与map产物等，压缩包约9.68MB，工程结构完整。功能上覆盖蓝牙透传、OLED屏显示、串口监视器实时输出DMP解算过程与陀螺仪姿态，并在OLED上同步呈现加速度、陀螺仪原始值与DMP解算结果，注释较完整，便于对照阅读inv_mpu与DMP驱动实现。已有6235人学习下载，适合需要快速搭建姿态检测原型、移植DMP驱动或排查HAL层I2C与定时器配置的开发者参考。
1. 拿到一块 MPU6050 和一块 F405，怎么让姿态角自己“冒”出来
很多人第一次用 MPU6050 的姿态解算，都是先啃原始加速度计和陀螺仪数据，自己写互补滤波，调参数调到怀疑人生。其实这颗芯片内部自带 DMP（数字运动处理器），只要把固件正确加载进去，四元数、欧拉角、重力向量都能直接从 FIFO 里读出来，主控几乎不用做数学运算。这份源码就是干这件事的：面向 STM32F405，基于 STM32Cube HAL 库，把 MPU6050 的 DMP 固件加载、FIFO 读取、姿态解算整条链路打通，适合正在做飞控、云台、平衡车、体感设备，又不想在姿态算法上反复翻车的人。下面按“它是什么、怎么跑、怎么改”的顺序，把这份工程拆开讲。
2. 工程结构与技术选型：为什么是 HAL + DMP 固件这套组合
2.1 目录骨架与各文件职责
这份源码的组织方式很典型，是 CubeMX 生成工程 + 手写驱动层的混合结构。核心目录大致是这样：
MPU6050_DMP_F405/ ├── Core/ │ ├── Inc/ │ │ ├── main.h │ │ ├── mpu6050.h // 对外接口：初始化、读姿态 │ │ ├── inv_mpu.h // DMP 驱动核心接口 │ │ ├── inv_mpu_dmp_motion_driver.h │ │ └── i2c_soft.h // 软件 I2C 或硬件 I2C 封装 │ └── Src/ │ ├── main.c │ ├── mpu6050.c // 初始化流程、姿态读取封装 │ ├── inv_mpu.c // DMP 加载、寄存器配置 │ ├── inv_mpu_dmp_motion_driver.c │ └── i2c_soft.c ├── Drivers/ │ └── STM32F4xx_HAL_Driver/ └── MDK-ARM/ 或 STM32CubeIDE/inv_mpu.c和inv_mpu_dmp_motion_driver.c是 InvenSense 官方驱动，基本不用动，负责把 DMP 固件按块写进芯片、配置采样率、开哪些传感器。mpu6050.c是二次封装层，把“初始化 + 读四元数 + 转欧拉角”包成几个好调用的函数。i2c_soft.c是通信层，用 GPIO 模拟 I2C 还是走硬件 I2C，取决于你的板子布线。
提示：官方驱动里有一大段 DMP 固件数组（
dmp_memory），体积不小，别以为是乱码删掉，那是必须原样烧进芯片的。
2.2 为什么选 HAL 而不是标准库
STM32F405 现在新开的项目基本都在 CubeMX 里点出来，HAL 库的 I2C 句柄、延时函数、时钟配置都是现成的。这份源码把 I2C 读写抽象成两个函数指针，挂在inv_mpu.c里的mpu_write/mpu_read上，底层用 HAL 的HAL_I2C_Mem_Write/HAL_I2C_Mem_Read实现。好处是换 MCU 型号时只改底层，DMP 驱动层一行不动。
常见做法是把 I2C 速率设到 400kHz，MPU6050 支持 Fast Mode，DMP 输出频率 100Hz 时 FIFO 压力不大。如果你用软件 I2C，延时函数要给够，否则 DMP 固件加载阶段容易校验失败。
2.3 DMP 解算链路：从寄存器到欧拉角
整条链路是：主控通过 I2C 把 DMP 固件写入 MPU6050 的内部 RAM → 配置 DMP 输出哪些数据（四元数、加速度、陀螺仪）→ 使能 DMP → DMP 在芯片内部以固定频率跑融合 → 结果塞进 FIFO → 主控读 FIFO 拿到四元数 → 主控做四元数转欧拉角。
关键点在于：融合运算在 MPU6050 内部完成，F405 只负责搬运和一次三角函数换算。这就是为什么很多低成本方案愿意用这颗芯片——主控负载极低。
3. 跑起来：从零到串口打出姿态角
3.1 硬件连接与 CubeMX 配置
接线就四根：VCC、GND、SCL、PB8/PB9 之类的 I2C 引脚（按你板子来）。MPU6050 的 AD0 接地则地址是0x68，接高则0x69，源码里默认0x68。
CubeMX 里要做的事：
- 开 I2C1，模式选 I2C，速度 400kHz。
- 开 USART1，115200，用来打印姿态角。
- 系统时钟配到 168MHz（F405 的常见主频）。
- 生成工程时选 HAL 库，工具链按你用的 IDE 选。
生成后把源码里的mpu6050.c、inv_mpu.c、inv_mpu_dmp_motion_driver.c、i2c_soft.c拷进Core/Src，头文件拷进Core/Inc，再在main.c里包含mpu6050.h。
3.2 初始化代码逐段讲
初始化分两步：先让 MPU6050 正常通信并复位，再加载 DMP 固件。核心调用长这样：
#include "mpu6050.h" int main(void) { HAL_Init(); SystemClock_Config(); MX_GPIO_Init(); MX_I2C1_Init(); MX_USART1_UART_Init(); // 第一步：MPU6050 基础初始化，失败就停在这里方便排查 if (mpu6050_init() != 0) { printf("mpu6050 init failed\r\n"); while (1); } // 第二步：加载 DMP 固件并使能 if (mpu6050_dmp_init() != 0) { printf("dmp init failed\r\n"); while (1); } while (1) { float pitch, roll, yaw; // 读一次姿态，内部会处理 FIFO if (mpu6050_get_euler(&pitch, &roll, &yaw) == 0) { printf("pitch=%.2f roll=%.2f yaw=%.2f\r\n", pitch, roll, yaw); } HAL_Delay(10); } }mpu6050_init()内部做的是：写PWR_MGMT_1唤醒芯片、选时钟源、设量程（陀螺仪 ±2000dps、加速度 ±2g 是 DMP 常用配置）、设采样率。mpu6050_dmp_init()则调用dmp_load_motion_driver_firmware()把固件写进去，再dmp_enable_feature()打开四元数和姿态输出，最后dmp_set_fifo_rate(100)设 100Hz。
参数说明：陀螺仪量程改大能测更快旋转，但分辨率下降；FIFO 速率设太高，主控读不过来会溢出，100Hz 对大多数场景够用。
3.3 读 FIFO 与四元数转欧拉角
mpu6050_get_euler()里最关键的是读 FIFO 和换算：
int mpu6050_get_euler(float *pitch, float *roll, float *yaw) { short gyro[3], accel[3], sensors; unsigned char more; long quat[4]; float q0, q1, q2, q3; // 从 FIFO 取一包数据，dmp_read_fifo 内部会解析四元数 if (dmp_read_fifo(gyro, accel, quat, &sensors, &more) != 0) return -1; if (!(sensors & INV_WXYZ_QUAT)) return -1; // 四元数定点数转浮点，2^30 是驱动里的缩放因子 q0 = quat[0] / 1073741824.0f; q1 = quat[1] / 1073741824.0f; q2 = quat[2] / 1073741824.0f; q3 = quat[3] / 1073741824.0f; // 四元数转欧拉角，注意 asin 参数要限幅，否则浮点异常 *pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3f; *roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3f; *yaw = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3f; return 0; }dmp_read_fifo是官方驱动函数，quat返回的是 Q30 定点四元数，除以 2^30 得到浮点。57.3f是弧度转角度。asin的参数理论上在 [-1,1]，但浮点误差可能越界，稳妥做法是加一句限幅，这是很多人第一次跑时偶发 NaN 的来源。
注意：yaw 角没有磁力计校正，会随时间缓慢漂移，这是 MPU6050 的物理限制，不是代码 bug。需要绝对航向就得外挂磁力计做融合。
4. 避坑与排查：DMP 初始化失败的五种典型现场
4.1 现象：dmp_load_motion_driver_firmware返回非 0
原因通常是 I2C 通信不稳或芯片没复位干净。先确认mpu_init()里读WHO_AM_I寄存器返回0x68，读不到就是接线或地址问题。读到了但加载固件失败，多半是 I2C 速率太高或软件 I2C 延时不够，把速率降到 100kHz 试一次。还有一种情况是芯片处于 sleep 模式没唤醒，检查PWR_MGMT_1是否写了0x00再写0x01选 PLL。
4.2 现象：能初始化，但读出来全是 0 或不变
先看dmp_read_fifo的返回值，如果一直非 0，说明 FIFO 里没数据。检查dmp_set_fifo_rate是否在dmp_enable_feature之后调用，顺序反了 DMP 不输出。另外确认INT引脚有没有接——有些例程靠中断触发读 FIFO，这份源码是轮询方式，不接也能跑，但如果你改成了中断方式却没接引脚，就会一直等不到。
4.3 现象：姿态角抖动厉害或跳变
常见原因是 FIFO 溢出。主控读得太慢，DMP 还在往里写，旧数据被覆盖。把HAL_Delay(10)改小，或者把 FIFO 速率从 100Hz 降到 50Hz。另一个原因是电源噪声，MPU6050 对电源敏感，VCC 上并一个 0.1uF 电容能明显改善。
4.4 现象：yaw 一直往一个方向漂
这是没有磁力计的正常表现，不是故障。DMP 的六轴融合只用陀螺仪积分航向，零偏随温度变化。缓解办法是上电后静止几秒做零偏校准，或者在应用层对 yaw 做缓慢回中。要彻底解决只能加 HMC5883 之类的磁力计。
4.5 现象：换到 F103 或其他型号编译报错
官方驱动里用了不少 F4 特有的类型和宏，换型号时重点改i2c_soft.c的底层读写和inv_mpu.c里的延时函数。HAL 库的 I2C 接口在不同系列基本一致，但HAL_Delay的时基要确认已配置，否则 DMP 加载阶段的等待会失效。
5. 二次开发：把姿态数据接进你自己的控制环
5.1 改造点一：把轮询改成中断驱动
轮询方式简单但占 CPU，实际产品里一般用 MPU6050 的 INT 引脚。配置INT_ENABLE寄存器让 DMP 数据就绪时拉低/拉高，然后在 EXTI 回调里置标志位，主循环检测标志再读 FIFO。这样主控可以去做别的事，姿态读取变成事件驱动。
// EXTI 回调里只置标志，不做耗时操作 void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { if (GPIO_Pin == MPU_INT_Pin) { dmp_data_ready = 1; } } // 主循环 if (dmp_data_ready) { dmp_data_ready = 0; mpu6050_get_euler(&pitch, &roll, &yaw); // 送进控制环 }5.2 改造点二：输出格式与坐标系对齐
DMP 输出的欧拉角坐标系和你的机械安装方向不一定一致。常见做法是在mpu6050_get_euler之后加一层坐标变换，把 pitch/roll/yaw 映射到你机构定义的轴。比如云台 pitch 轴装反了，就在输出前取负。这一步建议做成宏或配置表，别散落在业务代码里。
5.3 改造点三：加一层数据滤波
DMP 输出已经比较干净，但如果你发现某个轴在特定频率下仍有抖动，可以在应用层加一个一阶低通：
// alpha 越小越平滑，但延迟越大，0.1~0.3 是常用范围 static float pitch_f = 0; pitch_f = 0.2f * pitch + 0.8f * pitch_f;注意别在 DMP 输出上再叠加重滤波，否则控制环响应会变慢，平衡车这类场景容易振荡。
5.4 验证方法：静止 + 单轴旋转
改完之后怎么确认没改坏？两个测试：一是静止放置，三个角应该在零点附近小幅波动，漂移速率肉眼可见地慢；二是绕单一轴匀速转 90 度，对应角应该单调变化到接近 90，另外两轴基本不动。如果单轴旋转时其他轴跟着大幅变化，多半是四元数转欧拉角的公式或坐标系映射写错了。
我自己的习惯是每次动完驱动层，都先跑一遍这两个测试再往业务里接，血泪经验是跳过这步直接联调，最后分不清是姿态错了还是控制环错了。希望帮到你。
本文还有配套的精品资源，点击获取
