/* =============================================================================
 * debug_task.c - USB 虚拟串口(CDC)姿态输出(欧拉角 + 姿态链路健康位)
 * 每 DEBUG_OUT_PERIOD_MS 输出一行。单位: 角度 ° / 加速度 m/s² / 角速度 rad/s
 *
 * ---- 欧拉角(QuaternionEKF 解算结果,见 modules/ins) ----
 *   yaw   多圈连续偏航角 (att_deg_yaw   = INS.YawTotalAngle,云台 Yaw 环反馈)
 *   pitch 绕 y 轴角       (att_deg_pitch = QEKF_INS.Roll )
 *   roll  绕 x 轴角       (att_deg_roll  = QEKF_INS.Pitch,云台 Pitch 环反馈)
 *   QEKF 内部 Pitch/Roll 命名与航空惯例互换,映射与原因见 attitude.c 文件头。
 *   注意 yaw 没有绝对基准(纯陀螺积分),上电静止时恒为该次上电的相对 0 点,
 *   因此"yaw=0.00"本身不是故障特征;判断故障要看下面的健康位。
 *
 * ---- 健康位(排查"角度不动"时必须与角度一起看) ----
 *   cvg  1-INS.ConvergeFlag:EKF 卡方检验通过、姿态可信(原诊断输出的 cvg
 *        就是它,不要改成别的量,否则会丢掉"是否真的收敛"这一唯一判据);
 *        0-未收敛/发散,此时角度不可信
 *   rdy  1-att_ready:ins_update() 至少执行过一次,只说明"在跑",不代表可信
 *   imu  1-bmi088_online:IMU 初始化成功;0=ID 检测失败,姿态必然全 0
 *   an   |bmi088_accel|:静止时≈9.8;≈0 说明 IMU 根本没给出数据
 *   gn   |bmi088_gyro|:静止时≈0;转动时明显变大说明陀螺通路正常
 *
 * 不需要健康位时把 DEBUG_ATT_HEALTH 置 0,即只输出三个欧拉角。
 * ========================================================================== */
#include "debug_task.h"
#include "cmsis_os.h"
#include "FreeRTOS.h"
#include "task.h"
#include "usbd_cdc_if.h"
#include "attitude.h"
#include "ins.h"
#include "bmi088.h"
#include <stdio.h>
#include <math.h>

/* 1-输出欧拉角 + 姿态链路健康位; 0-只输出欧拉角 */
#define DEBUG_ATT_HEALTH 1

/* 输出周期(ms)。姿态观测建议 100ms(10Hz) */
#define DEBUG_OUT_PERIOD_MS 100

void debug_task(void const * argument)
{
    char line[128];

    for (;;)
    {
        const float an = sqrtf(bmi088_accel[0] * bmi088_accel[0] +
                               bmi088_accel[1] * bmi088_accel[1] +
                               bmi088_accel[2] * bmi088_accel[2]);
        const float gn = sqrtf(bmi088_gyro[0] * bmi088_gyro[0] +
                               bmi088_gyro[1] * bmi088_gyro[1] +
                               bmi088_gyro[2] * bmi088_gyro[2]);

        int len;
#if DEBUG_ATT_HEALTH
        len = snprintf(line, sizeof(line),
                       "yaw=%.2f pitch=%.2f roll=%.2f cvg=%u rdy=%u imu=%u an=%.2f gn=%.3f\r\n",
                       att_deg_yaw, att_deg_pitch, att_deg_roll,
                       (unsigned)INS.ConvergeFlag, (unsigned)att_ready,
                       (unsigned)bmi088_online, an, gn);
#else
        len = snprintf(line, sizeof(line), "yaw=%.2f pitch=%.2f roll=%.2f\r\n",
                       att_deg_yaw, att_deg_pitch, att_deg_roll);
#endif

        if (len > 0)
            CDC_Transmit_FS((uint8_t *)line, (uint16_t)len);

        osDelay(DEBUG_OUT_PERIOD_MS);
    }
}