/* =============================================================================
 * debug_task.c - USB 虚拟串口(CDC)欧拉角输出
 * 每 DEBUG_OUT_PERIOD_MS 输出一行 QuaternionEKF(见 modules/ins)解算出的
 * 欧拉角,单位度。板子插上电脑出现虚拟 COM 口,串口助手打开即可(波特率随意)。
 *
 * 输出格式: yaw=<> pitch=<> roll=<> cvg=<>
 *   yaw   多圈连续偏航角 (att_deg_yaw   = INS.YawTotalAngle,云台 Yaw 环反馈)
 *   pitch 绕 y 轴角       (att_deg_pitch = QEKF_INS.Roll )
 *   roll  绕 x 轴角       (att_deg_roll  = QEKF_INS.Pitch,云台 Pitch 环反馈)
 *   以上为工程侧语义(att_deg_*);QEKF 内部 Pitch/Roll 命名与航空惯例互换,
 *   映射关系与原因见 modules/attitude/attitude.c 文件头。
 *   cvg   1-EKF 已有有效姿态输出(att_ready),0-未收敛,此时角度不可信
 *
 * 说明: 云台板与底盘板共用本文件,两者 IMU_Task 均运行 attitude_update(),
 *       故 att_deg_* 在两种板上都有效,无需按 ROBOT_TYPE 分支。
 * ========================================================================== */
#include "debug_task.h"
#include "cmsis_os.h"
#include "FreeRTOS.h"
#include "task.h"
#include "usbd_cdc_if.h"
#include "attitude.h"
#include <stdio.h>

/* 输出周期(ms)。姿态观测建议 100ms(10Hz);如需接近原诊断节拍可改 500 */
#define DEBUG_OUT_PERIOD_MS 100

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

    for (;;)
    {
        int len = snprintf(line, sizeof(line),
                           "yaw=%.2f pitch=%.2f roll=%.2f cvg=%u\r\n",
                           att_deg_yaw, att_deg_pitch, att_deg_roll,
                           (unsigned)att_ready);

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

        osDelay(DEBUG_OUT_PERIOD_MS);
    }
}