MPU6050 项目经常同时出现寄存器驱动、DMP 固件和自定义滤波代码。新生最容易把它们误认为同一条数据链。本文先分清三层职责，再沿实际调用关系判断姿态角到底从哪里产生。
姿态驱动由哪些层组成
| 层次 | 代表文件 | 作用 |
|---|---|---|
| 板级适配 | board.c/.h | MSPM0 DriverLib 与 I²C 适配 |
| 寄存器驱动 | bsp_mpu6050.c/.h | 配置、收发、原始六轴读取 |
| DMP 上游 | inv_mpu.c/.h | MPU 配置、FIFO 与传感器接口 |
| DMP Motion Driver | inv_mpu_dmp_motion_driver.c/.h | 固件、特征、四元数接口 |
| 应用融合 | IMU.c/.h | 读取六轴并生成 Roll 等变量 |
源文件出现在工程中，不代表它已经参与构建或运行。判断功能是否生效，要从初始化入口和周期调用一路追到最终输出。
MPU6050 底层初始化
bsp_mpu6050.c 通过 I²C 写寄存器完成复位、时钟源、量程、低通和采样相关配置，并提供寄存器读写与多字节读取。
初始化流程还应核对 WHO_AM_I 和每一步返回值。当前返回值注释与实现语义存在冲突，调用者可能把失败当成功。
代码引用的 bsp_system.h 没有对应实现。移植时需要明确补齐时间、延时或系统接口，并统一其单位和错误语义，不能临时编造同名函数。
I²C 状态机与超时
底层通过 MSPM0 DriverLib 和中断状态维护收发过程，但多个等待循环没有超时。
若设备缺失、地址错误、总线被拉低或中断未到达，函数可能永远不返回。放进协作式调度器后，一次 I²C 故障会冻结所有周期任务。
工程化接口至少应返回 OK / TIMEOUT / NACK / BUS_ERROR，并在截止时间后停止传输、恢复总线状态。上层收到错误后必须使姿态数据失效，而不是继续使用旧值。
DMP 文件是否真正启用
工程中可以找到 dmp_load_motion_driver_firmware()、FIFO 和四元数相关接口，也有 DMP 映射表。
但当前 IMU_getEuleranAngles() 没有调用 dmp_read_fifo()，也没有从 DMP FIFO 获取四元数。可达路径读取原始加速度和角速度，再计算自定义 Roll。
因此，DMP 相关源码虽然存在，当前 IMU.c 的主输出仍来自原始六轴与互补滤波。要启用 DMP，必须补齐固件加载、FIFO 读取和四元数输出的可达调用链。
当前 Roll 输出链
IMU.c 由加速度向量计算横滚角，再用 X 轴陀螺仪积分，最后通过互补滤波融合：
加速度 XYZ → 几何 Roll ─┐陀螺仪 X → 积分 Roll ───┼→ 互补滤波 → 最终 Roll这条路径轻量，但车辆存在明显线加速度时，加速度计不再只测重力，几何角会受到干扰。滤波权重和调用周期必须用动态日志验证。
fast_sqrt() 的数学错误
函数先用经典位级近似得到约等于 1/sqrt(x) 的值，最后却返回：
return 1.0f / conv.f * x;若 conv.f ≈ 1/sqrt(x)，该式约等于 x·sqrt(x)，不是 sqrt(x)。它会破坏向量或四元数归一化。
修复前应先用 x=1、4、9 等已知输入建立测试，再选择标准 sqrtf() 或经过验证的快速平方根实现。不能只看姿态曲线“似乎还能动”。
角速度单位问题
代码把陀螺仪换算成度每秒，又把值送入四元数微分相关计算。常见四元数微分公式要求弧度每秒；若不转换，积分尺度会错误。
更关键的是，这部分四元数结果没有进入最终 Roll 返回链。它目前属于实验代码，不是主输出的依据。
推荐移植步骤
- 先补齐缺失依赖并让底层驱动可独立构建。
- 为每个 I²C 等待加入超时和错误返回。
- 读取
WHO_AM_I，记录原始六轴和时间戳。 - 修复并测试平方根、单位和归一化。
- 在“原始六轴互补滤波”和“DMP FIFO”两条路径中明确选择一条。
- 若启用 DMP，增加 FIFO 溢出、包长度和新鲜度处理。
接入真实姿态链前还要确认什么
| 检查项 | 当前代码表现 | 接入要求 |
|---|---|---|
| DMP 路径 | 有上游源码，但 IMU.c 未调用 dmp_read_fifo() | 若启用 DMP，应建立固件加载、FIFO 和溢出处理的完整链 |
| 当前 Roll | 来自原始六轴互补滤波 | 用已知姿态验证单位、轴向和动态响应 |
| I²C 错误 | 等待循环没有截止条件 | 增加超时、NACK、总线错误和恢复状态 |
| 数学实现 | fast_sqrt() 与角速度单位存在问题 | 用已知输入单测后再进入姿态计算 |
| 工程依赖 | 缺少 bsp_system.h 等接口 | 明确补齐时基与延时服务后再构建 |
本篇总结
- 底层驱动、DMP 上游和自定义融合属于不同层次，不能混为一条已启用链路。
- 当前可确认的 Roll 输出来自原始六轴互补滤波，不是 DMP FIFO。
fast_sqrt()、角速度单位和初始化返回语义存在明确风险。- 无超时 I²C 会阻塞整个非抢占式系统。
- 移植时应先建立可靠底层和错误路径，再决定使用 DMP 还是自定义融合。