/* =============================================================================
 * attitude.c - 姿态解算封装(BMI088 + QuaternionEKF 惯导)
 * 2026-08-31 起由 Mahony 互补滤波更换为 Wang Hongxi 的 QuaternionEKF:
 *   - 陀螺零偏在线估计,不再需要上电静止校准(1s 阻塞校准已移除)
 *   - 卡方检验剔除加速度异常帧,动态响应/抗震动干扰优于互补滤波
 *   - 欧拉角映射(与旧 Mahony get_yaw/pitch/roll 语义一一对应,勿改):
 *       att_deg_yaw   <- QEKF_INS.Yaw   (标准偏航;取多圈累计 YawTotalAngle)
 *       att_deg_roll  <- QEKF_INS.Pitch (绕 x 轴;本云台 Pitch 环反馈用它)
 *       att_deg_pitch <- QEKF_INS.Roll  (绕 y 轴)
 *     原因:QEKF 内部变量命名与航空惯例互换,见 ins.c 文件头
 * 数据链: bmi088.c(SPI DMA 后台更新) -> ins_update(1kHz) -> att_deg_*
 * 健康位: bmi088_online=0 时姿态数据不可用(云台任务会拒绝输出);
 *         姿态收敛需数秒,上电初期 ConvergeFlag=0 属正常
 * ========================================================================== */
#include "attitude.h"
#include "bmi088.h"
#include "ist8310.h"
#include "ins.h"
#include "tim.h"

extern TIM_HandleTypeDef htim10;

float att_deg_yaw;    // 多圈连续偏航角(°)
float att_deg_pitch;  // 绕 y 轴角(°)
float att_deg_roll;   // 绕 x 轴角(°,本云台 Pitch 环反馈)
uint8_t att_ready = 0;  // 1-标定完成且姿态已开始输出(控制任务门控用)

void attitude_init(void)
{
    ist8310_init();
    bmi088_init();
    ins_init();
}

void attitude_update(void)
{
    /* 先做数据路健康检查:IMU 数据一旦冻结,姿态会安静地"保持不动"甚至
     * 匀速漂移(见 bmi088.c 的 bmi088_health_check 说明) */
    bmi088_health_check();

    ins_update(0.001f);  // 1kHz 固定节拍

    att_deg_yaw = INS.YawTotalAngle;  // 多圈连续角,云台 Yaw 环直接使用
    att_deg_roll = INS.Pitch;         // QEKF 命名互换,见文件头
    att_deg_pitch = INS.Roll;

    if (INS.UpdateCount > 0)
        att_ready = 1;
}

void imu_heater_off(void)
{
    __HAL_TIM_SET_COMPARE(&htim10, TIM_CHANNEL_1, 0);
}