BetaFlight代码解析（8）— 陀螺仪和加速度计处理

本文档介绍了Betaflight中的惯性测量单元（IMU）传感器处理系统，具体包括陀螺仪和加速度计的数据采集、滤波、校准和处理。该系统为姿态估计和飞行控制提供核心传感器输入。

系统架构概述

IMU处理系统可处理多个陀螺仪和加速度计传感器的数据，并将经过滤波和校准的传感器数据提供给飞行控制系统。该系统支持冗余传​​感器、多种滤波模式和全面的校准程序。

IMU处理架构

陀螺仪处理流程

陀螺仪处理系统可同时处理来自多达 10 个GYRO_COUNT传感器的数据，并提供平均、滤波和安全机制。

陀螺仪数据流

陀螺仪更新过程

该gyroUpdate()函数实现了陀螺仪的核心处理逻辑：

1. 多传感器数据采集：基于以下方式读取所有已启用的陀螺仪传感器的数据gyro.gyroEnabledBitmask
2. 每个传感器的处理：应用校准偏移量（gyroZero）、对准校正和缩放。
3. 传感器融合：对来自多个主动式陀螺仪传感器的数据进行平均
4. 下采样：通过简单平均或 LPF2 滤波，累积样本以进行低频处理。

陀螺仪滤波系统

该gyroFiltering()函数通过实现来协调多个过滤阶段gyro_filter_impl.c：

加速度计处理流程

加速度计系统通过幅值和加加速度计算提供姿态参考和碰撞检测。

加速度计处理流程

加速度计计算

加速度计系统计算出几个衍生值：

• 加加速度计算：acc.jerk.v[axis] = (acc.accADC.v[axis] - accAdcPrev.v[axis]) * acc.sampleRateHz
• 震级：acc.accMagnitude = vector3Norm(&acc.accADC) * acc.dev.acc_1G_rec
• 加加速度幅度：acc.jerkMagnitude = vector3Norm(&acc.jerk) * acc.dev.acc_1G_rec

过滤器实施系统

该过滤系统提供多种滤波器类型，具有可配置参数和动态调整功能。

过滤器类型层级

动态低通滤波

动态低通滤波器系统根据油门输入调整滤波器截止频率：

float dynThrottle(float throttle) {
    return throttle * (1 - (throttle * throttle) / 3.0f) * 1.5f;
}

该系统支持不同的滤波器类型（DYN_LPF_PT1，，，）DYN_LPF_BIQUAD，具有油门响应截止频率。DYN_LPF_PT2DYN_LPF_PT3

校准系统

陀螺仪和加速度计系统均实施了全面的校准程序，包括运动检测和质量评估。

陀螺仪校准过程

校准质量控制

陀螺仪校准系统包含运动检测功能，以确保校准质量：

• 运动阈值：gyroMovementCalibrationThreshold参数控制灵敏度
• 标准偏差监测：跟踪校准过程中的数据稳定性
• 自动重启：如果检测到过度移动，则重新启动校准。
• 持续时间控制：gyroCalibrationDuration以 1/100 秒为单位设置校准时间

配置管理

IMU系统使用参数组进行配置管理，并具有默认值和验证功能。

配置结构

范围	类型	目的
gyro_lpf1_static_hz	uint16_t	主低通滤波器截止频率
gyro_lpf2_static_hz	uint16_t	二级低通滤波器截止频率
gyro_lpf1_dyn_min_hz	uint16_t	动态低通滤波器最小频率
gyro_lpf1_dyn_max_hz	uint16_t	动态低通滤波器最大频率
gyroCalibrationDuration	uint16_t	校准时间（单位：1/100秒）
gyroMovementCalibrationThreshold	uint8_t	运动检测灵敏度
checkOverflow	uint8_t	溢出检测模式
yaw_spin_recovery	uint8_t	偏航旋转恢复模式

安全防护系统

IMU系统包含多种安全机制：

• 溢出检测：监控指定轴上的传感器溢出情况
• 偏航旋转恢复：检测并处理可能表明碰撞或失控的过大偏航率。
• 转换速率限制：防止传感器数值突然变化，避免数据损坏。
• 温度监测：跟踪传感器温度以进行热补偿