BetaFlight代码解析（6）— IMU和姿态系统

目的和范围

惯性测量单元（IMU）和姿态系统负责融合惯性传感器数据（陀螺仪、加速度计、磁力计）和GPS航向信息，以估计飞行器在三维空间中的姿态。该系统提供驱动飞行控制回路的基本姿态参考，从而确保飞行器在所有飞行模式下都能稳定飞行。

系统架构

IMU系统作为一个传感器融合引擎，将多个输入源融合起来，生成可靠的姿态估计。核心处理在主飞行控制回路中以高频（通常为1-8kHz）进行。

核心数据结构

IMU系统采用多种姿态数学表示方法，每种方法都针对不同的计算任务进行了优化：

表示	类型	目的	主要功能
四元数	quaternion_t q	内部计算，无奇异性	imuMahonyAHRSupdate()
旋转矩阵	matrix33_t rMat	坐标变换	imuComputeRotationMatrix()
欧拉角	attitudeEulerAngles_t attitude	用户界面、飞行模式	imuUpdateEulerAngles()

Mahony AHRS 算法

核心姿态估计采用 Mahony 姿态航向参考系统 (AHRS) 算法，该算法融合了陀螺仪速率、加速度计和磁力计/GPS 航向修正：

传感器融合与误差校正

IMU系统会根据飞行条件和传感器可用性调整其运行方式：

自适应增益控制

该imuCalcKpGain()功能根据武装状态和飞机运动情况调整校正增益：

GPS航向积分

GPS对地航向可在飞机移动时提供航向修正：

配置和运行时参数

IMU系统通过imuConfig_t参数组和运行时计算进行配置：

范围	默认	目的	单位
imu_dcm_kp	2500	比例增益 × 10000	无量纲
imu_dcm_ki	0	积分增益 × 10000	无量纲
small_angle	25	“水平”检测的角度限制	学位
mag_declination	0	局部磁偏角 × 10	学位

与飞行控制集成

IMU系统向多个飞行控制子系统提供姿态数据：

节气门角度修正

在角度模式和水平模式下，IMU 会计算倾斜姿态的油门补偿：

调试输出和监控

该系统提供实时调试信息，用于调优和分析：

DEBUG_SET(DEBUG_ATTITUDE, 0, attitude.values.roll);
DEBUG_SET(DEBUG_ATTITUDE, 1, attitude.values.pitch);
DEBUG_SET(DEBUG_ATTITUDE, 2, lrintf(groundspeedGain * 100.0f));
DEBUG_SET(DEBUG_ATTITUDE, 3, (headingErrCog * 100));
DEBUG_SET(DEBUG_ATTITUDE, 7, lrintf(dcmKpGain * 100.0f));

测试与仿真

IMU系统包含全面的单元测试和模拟器支持：

单元测试覆盖率

该测试套件验证核心数学运算和算法行为：

• 利用四元数计算旋转矩阵
• 从旋转矩阵中提取欧拉角
• Mahony AHRS 更新算法
• 小角度检测逻辑
• 坐标系变换

模拟器集成

对于软件在环（SITL）测试，IMU系统支持外部姿态注入：

#ifdef SIMULATOR_BUILD
void imuSetAttitudeRPY(float roll, float pitch, float yaw);
void imuSetAttitudeQuat(float w, float x, float y, float z);
#if defined(SIMULATOR_IMU_SYNC)
void imuSetHasNewData(uint32_t dt);
#endif
#endif

在多线程模拟器环境中，通过互斥锁来保证线程安全。