在无人机飞控、两轮自平衡小车、机械臂云台或者 VR 头显这类项目里，姿态角是否准确、是否平滑，直接决定了系统的稳定性和体验感。很多初学者拿到 ICM42688 和 MMC5983 之后，第一反应是“能读到数据就行”，但真正开始做九轴姿态解算时，就会发现：加速度计噪声大、陀螺仪积分漂移、磁力计容易被环境干扰，三个传感器各有各的“脾性”。如果只把数据打印出来，不做融合，姿态角根本没法用。
本文围绕 ICM42688 + MMC5983 这套九轴方案，从传感器选型、硬件接线、驱动读取，到 Mahony 互补滤波落地，给出一套可以照着跑通的完整流程。如果你正准备做一个基于 STM32 的 IMU 姿态解算项目，或者已经读到了原始数据但融合效果不理想，这篇文章会比较适合你。文章里会包含完整代码、配置说明、常见报错排查思路，以及我在实际调试中认为最重要的几个工程细节。
1. 项目背景与核心概念
1.1 这套九轴方案是谁：ICM42688 + MMC5983
先介绍一下这套方案里的两个传感器。
ICM42688 是 TDK InvenSense 推出的六轴惯性传感器，内部集成了三轴加速度计和三轴陀螺仪。它支持 I2C 和 SPI 两种通信接口，相比老一代的 MPU6050，ICM42688 的噪声更低、功耗更小，而且内部带了 2KB FIFO，可以显著减少 MCU 的读取频率。对于需要高频采集姿态数据的项目来说，这是一个很实用的特性。
MMC5983MA 是 MEMSIC 推出的三轴磁力计，用来测量地磁场方向，可以把它理解成一个三维罗盘。它同样支持 I2C 和 SPI，输出分辨率比较高，能提供 20bit 的原始数据，整体体积也小，适合和六轴 IMU 组合使用。
所谓“九轴”，指的就是六轴 IMU（三轴加速度 + 三轴陀螺仪）加上三轴磁力计。ICM42688 提供加速度和角速度，MMC5983 提供磁场方向，三者一起通过算法融合，就能得到比较可靠的 Roll（横滚）、Pitch（俯仰）、Yaw（航向）三个姿态角。
1.2 为什么六轴不够，非要加磁力计
很多刚接触 IMU 的同学会问：六轴传感器已经能测加速度和角速度了，为什么还要单独加一个磁力计？
答案是：六轴方案无法解决航向角漂移问题。
陀螺仪积分短时间内非常准，但时间一长就会因为零偏、温度漂移和积分误差慢慢偏出去。加速度计可以修正 Roll 和 Pitch，因为重力方向始终是固定的参考；但它无法修正 Yaw，因为绕重力轴旋转时，重力向量并没有变化。没有磁力计参与时，Yaw 会像“开环积分”一样越漂越远，无法收敛。
磁力计测量的是地磁场方向，地磁北极在地理上可以作为航向的绝对参考。把磁力计数据加入融合算法后，Yaw 就有了“锚点”，不会无限制地漂移。这就是为什么云台、无人机、AGV 小车这类对航向有要求的项目，普遍会采用九轴方案。
1.3 姿态解算算法怎么选：Mahony 互补滤波的优势
姿态解算的常见算法有欧拉角微分方程、Mahony 互补滤波、Madgwick 滤波、扩展卡尔曼滤波（EKF）等。
欧拉角微分方程实现简单，但存在万向锁问题，而且直接在欧拉角上做积分不适合全姿态解算。
EKF 精度上限最高，但需要做矩阵运算，状态向量和协方差矩阵的推导都比较复杂，在 STM32F103 这类低主频芯片上跑起来压力偏大。
Mahony 互补滤波是工程中最常用的折中方案。它本质上是一种带 PI 校正的互补滤波：陀螺仪提供高频姿态变化，加速度计和磁力计提供低频参考，二者通过比例积分环节融合。相比 EKF，Mahony 不需要解算矩阵，计算量小，参数只有两个（比例系数和积分系数），对嵌入式开发非常友好。
所以本文选择 Mahony 互补滤波作为九轴姿态解算的核心算法，既能把项目跑通，又能给后续学习 EKF 和更复杂的滤波算法打好基础。
2. 系统架构与解算原理
2.1 从传感器原始数据到姿态角的完整链路
整个系统的数据链路可以拆成下面几个阶段，理解这条链路比直接抄代码更重要。
ICM42688（加速度 + 陀螺仪） │ ├── I2C/SPI 读取原始寄存器 │ MMC5983（磁力计） │ ├── I2C/SPI 读取原始寄存器 │ ▼ 预处理：陀螺仪零偏扣除、量程换算、加速度/磁力计归一化 │ ▼ Mahony 互补滤波：四元数更新 │ ▼ 四元数转欧拉角：Roll / Pitch / Yaw │ ▼ 串口输出 / 控制环 / 上位机显示在这个链路里，MCU 负责传感器数据读取和算法计算，常用的通信方式是 I2C 或 SPI。I2C 接线少，适合大多数应用；SPI 速率更高，适合对实时性要求更高的项目。本文示例以 I2C 为主讲解，SPI 只是接线和寄存器读取方式不同，算法部分完全一样。
2.2 坐标系约定：机体系、世界系与欧拉角
姿态解算绕不开坐标系的定义，这里先约定一下：
- 机体系（body frame）：固定在传感器模块上，三轴分别为 X、Y、Z，通常按“前右下”或“右前上”定义，不同传感器厂商的定义可能略有差异。
- 世界系（world frame）：固定在地面上，可以选“东北天”或“北东地”，主要用于描述传感器模块相对地面的姿态。
欧拉角是把姿态分解为绕 Z 轴旋转的 Yaw、绕 Y 轴旋转的 Pitch、绕 X 轴旋转的 Roll。它的优点是直观，缺点是当 Pitch 接近 ±90° 时会发生万向锁，导致 Yaw 和 Roll 无法唯一确定。
四元数则没有万向锁问题，计算过程也更适合程序实现。因此 Mahony 算法内部使用四元数保存姿态，只在输出给用户或者控制环时才转换为欧拉角。
2.3 Mahony 互补滤波的核心思想
先看一张简化理解图（不是真正的框图，只表示信息流向）：
陀螺仪角速度 ──► 四元数积分 ──► 预测姿态 ▲ │ 误差反馈 加速度计/磁力计 ──► 参考方向 ──► 误差计算 ──► PI 校正Mahony 算法的几个关键点：
- 陀螺仪积分负责“短时间准确”的快速姿态变化。
- 加速度计在静止或低速运动时，测得的合向量方向就是重力方向，可以用来修正 Roll 和 Pitch。
- 磁力计在水平面内的分量可以给出地磁北方向，用来修正 Yaw。
- 加速度计估计的重力方向和磁力计估计的磁场方向，与当前四元数预测出的方向做叉积，得到误差。
- 误差通过 PI 控制器反馈到陀螺仪角速度上，再更新四元数。
比例系数 twoKp 越大，修正越快，但容易引入高频噪声；积分系数 twoKi 用于消除稳态误差，比如陀螺仪零偏导致的持续漂移。
2.4 磁力计如何参与九轴修正
磁力计参与融合时，不能直接把原始磁场值当成“参考向量”使用，因为地磁场在小范围内并不是水平指向北方的，它还有一个垂直向下的分量，也就是磁倾角。
在 Mahony 更新中，一般先把磁力计向量通过当前四元数旋转到世界系，得到磁场在世界系下的表示，然后取水平方向的分量作为北向参考，垂直方向的磁场分量结合重力方向一起估计。这样处理可以避免磁倾角对 Roll 和 Pitch 的引入额外的误差。
所以你会看到 Mahony 代码里有那么一段“计算参考磁场 HX、HY、BZ”的逻辑，它本质上是完成磁场方向与重力方向的对齐。这也是九轴 Mahony 和六轴 Mahony 最大的区别。
3. 环境准备与硬件接线
3.1 硬件清单与接线表
本文示例使用的硬件如下，供参考：
| 硬件 | 型号 | 说明 |
|---|---|---|
| 主控板 | STM32F103ZET6 核心板 | 也可以用其他 STM32，代码需微调 |
| 六轴 IMU | ICM42688 模块 | I2C/SPI 均可 |
| 磁力计 | MMC5983MA 模块 | I2C/SPI 均可 |
| 通信工具 | USB 转 TTL 串口模块 | 用于打印姿态数据 |
| 供电 | 3.3V/5V 电源 | 传感器统一使用 3.3V 供电 |
两个传感器可以挂在同一条 I2C 总线上，因为它们地址不同。实际接线建议如下：
| ICM42688 | STM32 | MMC5983 | STM32 |
|---|---|---|---|
| VCC | 3.3V | VCC | 3.3V |
| GND | GND | GND | GND |
| SCL | PB8（I2C1_SCL） | SCL | PB8（I2C1_SCL） |
| SDA | PB9（I2C1_SDA） | SDA | PB9（I2C1_SDA） |
| AD0 | 可悬空或接 GND | 无 | 无 |
接线时要注意两个点：一是传感器模块的 VCC 必须是 3.3V，不要接 5V，否则可能烧坏模块；二是 I2C 的 SCL 和 SDA 需要上拉电阻，大部分模块上已经带了，如果你的模块没有，要在总线上外接 4.7kΩ 上拉到 3.3V。
3.2 软件环境
本文代码基于以下环境编写，版本不一定要求完全一致，但思路一致：
- STM32CubeMX 6.x
- Keil MDK5
- 固件包 STM32CubeF1
- HAL 库
不同芯片型号、不同 HAL 库版本之间的 I2C 句柄名可能不同，代码里用到hi2c1的地方需要替换成你自己工程生成的句柄。
3.3 STM32CubeMX 基础配置
在 STM32CubeMX 中需要做如下配置：
- 选择 MCU 型号，例如 STM32F103ZET6。
- 将 PB8 和 PB9 配置为 I2C1 的 SCL 和 SDA。
- 将 I2C 速率设置为 400kHz，并在高级参数中确认外设被使能。
- 将 USART1 配置为异步模式，波特率 115200，用于打印姿态数据。
- 在 Project Manager 中生成 Keil MDK 工程。
生成工程后，I2C 初始化代码已经在i2c.c中自动生成，我们只需要在业务代码里调用HAL_I2C_Mem_Read和HAL_I2C_Mem_Write来进行寄存器读写。
4. STM32 读取 ICM42688 数据
4.1 I2C 通信驱动封装
为了代码整洁，建议把传感器驱动封装成独立文件，不要全部堆在main.c里。下面先封装 I2C
