init:全链路闭环控制调通

project/code/IMU.h

@@ -0,0 +1,105 @@
+#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