/* =============================================================================
 * 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 点。
 *
 * ---- 健康位 / 诊断位(排查"角度不动或自漂"时必须一起看) ----
 *   cvg  1-INS.ConvergeFlag:EKF 卡方检验通过、姿态可信(与原诊断输出同义,
 *        不要改成别的量,否则会丢掉"是否真的收敛"这一唯一判据)
 *   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
 *   gx/gy/gz 原始陀螺三分量(rad/s) —— 与 gn 一起可判断数据是否冻结
 *   zo   ins_gyro_z_offset:上电静止标定得到的 z 零偏(rad/s)。**关键**:
 *        实际进入滤波器的 gyro_z = gz - zo。若 zo 与 gz 量级相近甚至更大,
 *        而 yaw 又匀速自漂,即标定窗口内车体在动把零偏标歪了
 *   ck   ins_z_cal_ok:1-标定时全程静止,zo 可信;0-三轮都在动,zo 存疑
 *   ca/cg 加速度/陀螺采样计数:停止增长即数据路已死(数值冻结时它们不涨)
 *   se   bmi088_spi_errors:SPI/DMA 出错次数;st=bmi088_spi_state()
 *        (1=READY,非 1 说明 SPI 被卡住,数据路会因此停摆)
 *
 * ---- 控制量 / 控制模式(DEBUG_CMD_OUT=1 时追加) ----
 *   mode    底盘模式 0=OFF 1=MANUAL 2=FOLLOW 3=SPIN_SLOW 4=SPIN_FAST
 *           (由本板依据收到的 s2/spin 推出;模式逻辑见 robot_cmd_task.c)
 *   online  1=板间通讯(CToC)在线 —— 本板唯一的"控制源在线"判据
 *   vx/vy   底盘平移速度指令(世界系映射值)   wz [已废弃,恒 0]
 *   spin    **收到的**小陀螺使能(0x189 bit2,云台侧做 Ctrl 沿检测)
 *   s2      **收到的**遥控 S2 档位(小陀螺为真时它被覆盖)
 *   reset   跟随朝向重锁请求(跟随模式重连时置位)
 *   注意:本板拿不到"遥控器还是键鼠"这一信息——0x149/0x189 不带该位,
 *   robot_cmd_task 里 cmd_robot.source 恒被置 0,故这里不输出假字段。
 *   与姿态同源(cmd_robot/ctoc_remote),一起看才能判断"模式对不对、量给没给"。
 *
 * 开关: DEBUG_ATT_HEALTH=0 只输出三个欧拉角;DEBUG_ATT_DIAG=0 去掉诊断位;
 *       DEBUG_CMD_OUT=1 追加控制量/控制模式。
 * ========================================================================== */
#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 "robot_typedef.h"
#include "c_comm.h"
#include <stdio.h>
#include <math.h>

/* 1-输出健康位(cvg/rdy/imu/an/gn); 0-只输出三个欧拉角 */
#define DEBUG_ATT_HEALTH 1
/* 1-再附加原始陀螺/零偏/采样计数(排障用); 0-只留上面的通用健康位 */
#define DEBUG_ATT_DIAG 1
/* 1-追加控制量/控制模式(见文件头); 0-保持原姿态输出不变 */
#define DEBUG_CMD_OUT 1

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

#if DEBUG_CMD_OUT
/* 底盘模式名(便于人读;数值定义见 robot_typedef.h 的 chassis_mode_e) */
static const char * cmd_mode_name(chassis_mode_e m)
{
    switch (m)
    {
        case CHASSIS_MODE_OFF:       return "OFF";
        case CHASSIS_MODE_MANUAL:    return "MANUAL";
        case CHASSIS_MODE_FOLLOW:    return "FOLLOW";
        case CHASSIS_MODE_SPIN_SLOW: return "SPIN_SLOW";
        case CHASSIS_MODE_SPIN_FAST: return "SPIN_FAST";
        default:                     return "?";
    }
}
#endif

void debug_task(void const * argument)
{
    char line[448];
    char tail[112];

    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]);

        tail[0] = '\0';
#if DEBUG_ATT_HEALTH && DEBUG_ATT_DIAG
        snprintf(tail, sizeof(tail),
                 " gx=%.4f gy=%.4f gz=%.4f zo=%.4f ck=%u ca=%lu cg=%lu se=%lu st=%u",
                 bmi088_gyro[0], bmi088_gyro[1], bmi088_gyro[2],
                 ins_gyro_z_offset, (unsigned)ins_z_cal_ok,
                 (unsigned long)bmi088_accel_cnt, (unsigned long)bmi088_gyro_cnt,
                 (unsigned long)bmi088_spi_errors, (unsigned)bmi088_spi_state());
#endif

#if DEBUG_CMD_OUT
        char ctl[192];
        snprintf(ctl, sizeof(ctl),
                 " mode=%s online=%u vx=%.3f vy=%.3f wz=%.3f spin=%u s2=%u reset=%u",
                 cmd_mode_name(cmd_robot.chassis_mode),
                 (unsigned)cmd_robot.online,
                 cmd_robot.vx, cmd_robot.vy, cmd_robot.wz_manual,
                 (unsigned)ctoc_remote.spin_flag,
                 (unsigned)ctoc_remote.s2,
                 (unsigned)cmd_robot.gimbal_reset);
#else
        const char * ctl = "";
#endif

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

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

        osDelay(DEBUG_OUT_PERIOD_MS);
    }
}