SkyLi
106 lines
4.1 KiB
C
106 lines
4.1 KiB
C
#ifndef __IMU_H
|
||
#define __IMU_H
|
||
|
||
#include "ICM_42688.h"
|
||
|
||
//==================================================================================================
|
||
// IMU 姿态解算层
|
||
// 负责：传感器数据滤波 → Mahony AHRS 姿态解算 → 欧拉角输出
|
||
// 所有数据基于传感器坐标系（无机体坐标转换）
|
||
//==================================================================================================
|
||
|
||
#define ANG2RAD 0.01745329252f // 角度制 → 弧度制
|
||
#define RAD2ANG 57.295779513f // 弧度制 → 角度制
|
||
|
||
//==================================================================================================
|
||
// 双二阶滤波器 (Biquad Filter)
|
||
//==================================================================================================
|
||
|
||
/** @brief 滤波器类型 */
|
||
typedef enum {
|
||
BIQUAD_LOWPASS, // 低通
|
||
BIQUAD_HIGHPASS, // 高通
|
||
BIQUAD_BANDPASS_PEAK, // 带通（峰值）
|
||
BIQUAD_BANDSTOP_NOTCH, // 带阻（陷波）
|
||
} biquad_type_t;
|
||
|
||
/** @brief 滤波器状态（系数 + 历史值）
|
||
* 典型 Q 值: 0.5773 (Thomson-Bessel, 相位线性度好)
|
||
* 0.7071 (Butterworth, 最平坦)
|
||
* 0.9565 (Chebyshev 1dB, 阻带衰减快)
|
||
*/
|
||
typedef struct {
|
||
float a0, a1, a2, a3, a4; // 传递函数系数
|
||
float x1, x2, y1, y2; // 历史输入/输出
|
||
} biquad_state_t;
|
||
|
||
//==================================================================================================
|
||
// 姿态输出
|
||
//==================================================================================================
|
||
|
||
/** @brief 姿态数据（传感器坐标系，volatile 因为 ISR 写入、主循环读取） */
|
||
typedef struct {
|
||
float pitch, roll, yaw; // 欧拉角 (°)
|
||
float gyro_x, gyro_y, gyro_z; // 滤波后角速度 (°/s)
|
||
} imu_attitude_t;
|
||
|
||
extern volatile imu_attitude_t imu_angle; // 当前姿态
|
||
extern volatile int yaw_round_count;
|
||
|
||
//==================================================================================================
|
||
// 用户接口
|
||
//==================================================================================================
|
||
|
||
/** @brief 初始化 IMU（传感器 + 滤波器 + Mahony）
|
||
* @param sample_freq_hz 采样频率（Hz），需与定时器中断频率一致，典型值 200
|
||
* 用户只需调用此函数，内部自动调用 icm42688_init()
|
||
*/
|
||
void imu_init(int sample_freq_hz);
|
||
|
||
/** @brief 更新姿态（在定时器 ISR 中以固定频率调用）
|
||
* 读取传感器 → 零偏修正 → 双二阶滤波 → Mahony 解算 → 写入 imu_angle
|
||
* 执行时间约 100~150us（80MHz Cortex-M0+ 估算），需保证在中断周期内完成
|
||
*/
|
||
void imu_update(void);
|
||
|
||
/** @brief 陀螺仪零偏校准（静止状态下调用）
|
||
* 采集 3 秒静止数据，计算各轴零偏，自动应用于后续 imu_update()
|
||
* 调用时设备必须完全静止
|
||
*/
|
||
void imu_calibrate_gyro(void);
|
||
|
||
//==================================================================================================
|
||
// 算法接口（也可被外部直接调用）
|
||
//==================================================================================================
|
||
|
||
/** @brief 双二阶滤波器初始化
|
||
* @param state 滤波器状态指针
|
||
* @param type 滤波器类型
|
||
* @param fs 采样频率 (Hz)
|
||
* @param fc 截止频率 (Hz)
|
||
* @param q 品质因数
|
||
*/
|
||
void biquad_init(biquad_state_t *state, biquad_type_t type, int fs, float fc, float q);
|
||
|
||
/** @brief 双二阶滤波器单步计算
|
||
* @param state 滤波器状态指针
|
||
* @param data 当前输入值
|
||
* @return 滤波后输出值
|
||
*/
|
||
float biquad_apply(biquad_state_t *state, float data);
|
||
|
||
/** @brief Mahony AHRS 姿态解算（6轴 IMU 模式）
|
||
* @param gx/gy/gz 陀螺仪角速度 (°/s, 传感器坐标系)
|
||
* @param ax/ay/az 加速度计测量值 (g, 传感器坐标系)
|
||
* 输出写入全局变量 imu_angle.pitch/roll/yaw
|
||
*/
|
||
void mahony_ahrs_update(float gx, float gy, float gz, float ax, float ay, float az);
|
||
|
||
/** @brief 快速平方根倒数（Quake III 算法）
|
||
* @param x 输入值
|
||
* @return 1/sqrt(x) 的近似值
|
||
*/
|
||
float inv_sqrt(float x);
|
||
|
||
#endif
|