/* =============================================================================
 * ins.h - 惯导姿态解算封装(移植自 RoboMaster-C-Board-INS-Example, Wang Hongxi)
 * 核心:  QuaternionEKF 四元数 EKF——陀螺积分预测 + 加速度重力方向观测,
 *        在线估计 x/y 陀螺零偏,卡方检验剔除加速度异常(撞击/运动)帧
 * 对比旧 Mahony 方案:  收敛更快、零偏在线估计(无需上电静止校准)、
 *        动态加速度干扰抑制更好(卡方检验)
 * 数据源: 本工程 BMI088 驱动的 bmi088_accel/gyro(SPI DMA 后台更新)
 * 调用:  ins_init() 一次,ins_update(dt) 每 1ms;dt 用固定 0.001
 *        (IMU_Task 为 vTaskDelayUntil 精确 1kHz 节拍,无需 DWT 计时)
 * ========================================================================== */
#ifndef INS_H
#define INS_H

#include <stdint.h>

typedef struct
{
    uint8_t Initialized;

    float Gyro[3];     // rad/s(零偏修正后)
    float Accel[3];    // m/s^2
    float q[4];        // 四元数 w x y z

    float Yaw;           // -180~180(°)
    float Pitch;         // QEKF 命名(实际为绕 x 轴角,本工程映射见 attitude.c)
    float Roll;          // 同上
    float YawTotalAngle; // 多圈连续 Yaw(°)

    float GyroBias[3];       // EKF 在线估计的陀螺零偏(rad/s)
    float MotionAccel_b[3];  // 运动加速度(机体系,去除重力,低通)
    uint8_t StableFlag;      // 1-载体静止且加速度正常(可用于 ZUPT 等扩展)
    uint8_t ConvergeFlag;    // 1-卡方检验收敛(姿态可信)
    float ChiSquare;         // 最近一次卡方检验值(诊断用)
    uint32_t UpdateCount;    // 更新计数(诊断用)
} INS_t;

extern INS_t INS;

/* 初始化:先做 z 轴静止零偏标定(约 1s 阻塞,车体必须静止——QEKF 的
 * 卡方观测无法感知 z 偏置,GyroBias[2] 恒为 0,不标定会导致 Yaw 持续漂移),
 * 再初始化 EKF(参数为 INS 例程 demo 值,调参见 QuaternionEKF.c 头注释) */
extern void ins_init(void);

/* 姿态更新(1kHz 调用,dt 单位秒;内部读 bmi088_accel/gyro 全局量)
 * 标定完成前直接返回,不更新 */
extern void ins_update(float dt);

/* 1-标定完成且已有有效姿态输出(控制任务以此为门控) */
extern uint8_t ins_ready(void);

/* 坐标系变换工具(机体系 <-> 导航系) */
extern void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q);
extern void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q);

#endif