From 3c95bb0887f585d0eb3376367923b881235ab36a Mon Sep 17 00:00:00 2001 From: LSMushui Date: Sat, 12 Sep 2026 10:33:29 +0800 Subject: [PATCH] =?UTF-8?q?feat(debug):=20USB=20CDC=20=E8=BE=93=E5=87=BA?= =?UTF-8?q?=E6=94=B9=E4=B8=BA=20QuaternionEKF=20=E6=AC=A7=E6=8B=89?= =?UTF-8?q?=E8=A7=92?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 原 debug_task 每 500ms 经 CDC 打印一行混装诊断信息(芯片 ID/帧计数/空闲中断 计数/链路状态/电机掉线/跟随误差等 18 个字段),稳定后已无持续观察价值。 现改为直接输出姿态解算结果,便于上位机/串口助手直接看欧拉角与验证 EKF 收敛。 输出格式(单位: 度): yaw=<...> pitch=<...> roll=<...> cvg=<0|1> yaw = att_deg_yaw (多圈连续偏航角,= INS.YawTotalAngle,云台 Yaw 环反馈) pitch = att_deg_pitch (= QEKF_INS.Roll, 绕 y 轴角) roll = att_deg_roll (= QEKF_INS.Pitch,绕 x 轴角,云台 Pitch 环反馈) cvg = att_ready (0 表示 EKF 未收敛,此时角度不可信) 字段命名沿用工程侧语义,QEKF 内部 Pitch/Roll 与航空惯例互换的映射见 modules/attitude/attitude.c 文件头。 其他调整: - 云台/底盘共用本文件,因两板 IMU_Task 都跑 attitude_update(),att_deg_* 均有效,故移除原 ROBOT_TYPE_GIMBAL / ROBOT_TYPE_CHASSIS 分支; - 输出周期 500ms -> 100ms(10Hz,宏 DEBUG_OUT_PERIOD_MS 可调),2Hz 下无法 观察姿态动态响应; - 清理因此不再需要的 include 与 dbg_* 外部声明(bmi088/ist8310/dr16/referee/ c_comm/bsp_can/bsp_usart/usart.h/dbg_imu_loop/dbg_can_loop); - 同步更新两份 debug_task.h 的注释说明。 --- 云台/Application/Task/Inc/debug_task.h | 5 +- 云台/Application/Task/Src/debug_task.c | 86 +++++++------------------- 底盘/Application/Task/Inc/debug_task.h | 5 +- 底盘/Application/Task/Src/debug_task.c | 86 +++++++------------------- 4 files changed, 54 insertions(+), 128 deletions(-) diff --git a/云台/Application/Task/Inc/debug_task.h b/云台/Application/Task/Inc/debug_task.h index d419830..58115d3 100644 --- a/云台/Application/Task/Inc/debug_task.h +++ b/云台/Application/Task/Inc/debug_task.h @@ -1,8 +1,9 @@ -/* debug_task.h - USB 虚拟串口诊断任务声明(仅诊断用) */ +/* debug_task.h - USB 虚拟串口(CDC)欧拉角输出任务声明 */ #ifndef DEBUG_TASK_H #define DEBUG_TASK_H -/* debug_task:USB 虚拟串口(CDC)诊断输出(仅诊断用,稳定后可整组移除) */ +/* debug_task:USB 虚拟串口(CDC)输出 QuaternionEKF 欧拉角(yaw/pitch/roll,单位度) + * 输出格式与字段含义见 debug_task.c 文件头 */ extern void debug_task(void const * argument); #endif diff --git a/云台/Application/Task/Src/debug_task.c b/云台/Application/Task/Src/debug_task.c index 4a61d00..7b2a792 100644 --- a/云台/Application/Task/Src/debug_task.c +++ b/云台/Application/Task/Src/debug_task.c @@ -1,82 +1,44 @@ /* ============================================================================= - * debug_task.c - USB 虚拟串口(CDC)诊断输出(仅诊断用,稳定后可整组移除) - * 每 500ms 一行状态:各芯片实测 ID/DR16 帧计数/USART 空闲中断计数/任务循环 - * 计数等;板子插电脑出现虚拟 COM 口,串口助手打开即可(波特率随意)。 - * 字段含义见重构说明与编译指南.md 的诊断章节 + * 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 "app_tasks.h" #include "cmsis_os.h" #include "FreeRTOS.h" #include "task.h" -#include "usb_device.h" #include "usbd_cdc_if.h" -#include "usart.h" -#include "bsp_can.h" -#include "bsp_usart.h" -#include -#include "dr16.h" -#include "referee.h" -#include "c_comm.h" -#if defined(ROBOT_TYPE_GIMBAL) -#include "bmi088.h" -#include "ist8310.h" -#include "ins.h" #include "attitude.h" -#include "gimbal.h" -#elif defined(ROBOT_TYPE_CHASSIS) -#include "chassis.h" -#include "bmi088.h" -#include "ins.h" -#include "robot_typedef.h" -#endif +#include -/* debug_task:USB 虚拟串口(CDC)诊断输出,每 500ms 一行 - * 板子插上电脑会出现虚拟 COM 口,用串口助手打开即可看到(波特率随意) - * 输出为纯整数,用于远程定位传感器/通讯初始化问题 */ - -/* 各模块导出的诊断变量(定义在 imu_task.c / can_task.c) */ -extern volatile uint32_t dbg_imu_loop; -extern volatile uint32_t dbg_can_loop; +/* 输出周期(ms)。姿态观测建议 100ms(10Hz);如需接近原诊断节拍可改 500 */ +#define DEBUG_OUT_PERIOD_MS 100 void debug_task(void const * argument) { - char line[128]; + char line[96]; for (;;) { - int len = 0; -#if defined(ROBOT_TYPE_GIMBAL) - len = snprintf(line, sizeof(line), - "app=%u can=%u imu=%u acc=%02X gyro=%02X bmi=%u ist=%u who=%02X " - "cvg=%u ys=%d yaw=%.1f u3g=%02X i3=%u l3=%u d16=%u ref=%u/%u mot=%u\r\n", - app_ready, dbg_can_loop, dbg_imu_loop, - bmi088_last_acc_id, bmi088_last_gyro_id, bmi088_online, - ist8310_online, ist8310_last_who, - INS.ConvergeFlag, - (int)gimbal_yaw_sign(), att_deg_yaw, - (unsigned)(huart3.gState), dbg_usart3_idle, dbg_usart3_last_len, - dr16_frame_count, - referee.link_status, (unsigned)(HAL_GetTick() - referee.last_update_tick), - gimbal_motor_offline()); -#elif defined(ROBOT_TYPE_CHASSIS) - len = snprintf(line, sizeof(line), - "app=%u can=%u imu=%u acc=%02X gyro=%02X cvg=%u " - "ctoc=%u/%u att=%u/%d mot=%u md=%u err=%.1f fw=%d spin=%u lim=%u\r\n", - app_ready, dbg_can_loop, dbg_imu_loop, - bmi088_last_acc_id, bmi088_last_gyro_id, - INS.ConvergeFlag, - ctoc_remote.status, - (unsigned)(ctoc_remote.last_tick ? HAL_GetTick() - ctoc_remote.last_tick : 0), - ctoc_att_fresh(), (int)ctoc_gimbal_yaw, - chassis_motor_offline(), - cmd_robot.chassis_mode, - dbg_follow_err_deg, (int)dbg_follow_out, - ctoc_remote.spin_flag, referee.chassis_power_limit); -#endif + 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(500); + osDelay(DEBUG_OUT_PERIOD_MS); } } diff --git a/底盘/Application/Task/Inc/debug_task.h b/底盘/Application/Task/Inc/debug_task.h index d419830..58115d3 100644 --- a/底盘/Application/Task/Inc/debug_task.h +++ b/底盘/Application/Task/Inc/debug_task.h @@ -1,8 +1,9 @@ -/* debug_task.h - USB 虚拟串口诊断任务声明(仅诊断用) */ +/* debug_task.h - USB 虚拟串口(CDC)欧拉角输出任务声明 */ #ifndef DEBUG_TASK_H #define DEBUG_TASK_H -/* debug_task:USB 虚拟串口(CDC)诊断输出(仅诊断用,稳定后可整组移除) */ +/* debug_task:USB 虚拟串口(CDC)输出 QuaternionEKF 欧拉角(yaw/pitch/roll,单位度) + * 输出格式与字段含义见 debug_task.c 文件头 */ extern void debug_task(void const * argument); #endif diff --git a/底盘/Application/Task/Src/debug_task.c b/底盘/Application/Task/Src/debug_task.c index 4a61d00..7b2a792 100644 --- a/底盘/Application/Task/Src/debug_task.c +++ b/底盘/Application/Task/Src/debug_task.c @@ -1,82 +1,44 @@ /* ============================================================================= - * debug_task.c - USB 虚拟串口(CDC)诊断输出(仅诊断用,稳定后可整组移除) - * 每 500ms 一行状态:各芯片实测 ID/DR16 帧计数/USART 空闲中断计数/任务循环 - * 计数等;板子插电脑出现虚拟 COM 口,串口助手打开即可(波特率随意)。 - * 字段含义见重构说明与编译指南.md 的诊断章节 + * 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 "app_tasks.h" #include "cmsis_os.h" #include "FreeRTOS.h" #include "task.h" -#include "usb_device.h" #include "usbd_cdc_if.h" -#include "usart.h" -#include "bsp_can.h" -#include "bsp_usart.h" -#include -#include "dr16.h" -#include "referee.h" -#include "c_comm.h" -#if defined(ROBOT_TYPE_GIMBAL) -#include "bmi088.h" -#include "ist8310.h" -#include "ins.h" #include "attitude.h" -#include "gimbal.h" -#elif defined(ROBOT_TYPE_CHASSIS) -#include "chassis.h" -#include "bmi088.h" -#include "ins.h" -#include "robot_typedef.h" -#endif +#include -/* debug_task:USB 虚拟串口(CDC)诊断输出,每 500ms 一行 - * 板子插上电脑会出现虚拟 COM 口,用串口助手打开即可看到(波特率随意) - * 输出为纯整数,用于远程定位传感器/通讯初始化问题 */ - -/* 各模块导出的诊断变量(定义在 imu_task.c / can_task.c) */ -extern volatile uint32_t dbg_imu_loop; -extern volatile uint32_t dbg_can_loop; +/* 输出周期(ms)。姿态观测建议 100ms(10Hz);如需接近原诊断节拍可改 500 */ +#define DEBUG_OUT_PERIOD_MS 100 void debug_task(void const * argument) { - char line[128]; + char line[96]; for (;;) { - int len = 0; -#if defined(ROBOT_TYPE_GIMBAL) - len = snprintf(line, sizeof(line), - "app=%u can=%u imu=%u acc=%02X gyro=%02X bmi=%u ist=%u who=%02X " - "cvg=%u ys=%d yaw=%.1f u3g=%02X i3=%u l3=%u d16=%u ref=%u/%u mot=%u\r\n", - app_ready, dbg_can_loop, dbg_imu_loop, - bmi088_last_acc_id, bmi088_last_gyro_id, bmi088_online, - ist8310_online, ist8310_last_who, - INS.ConvergeFlag, - (int)gimbal_yaw_sign(), att_deg_yaw, - (unsigned)(huart3.gState), dbg_usart3_idle, dbg_usart3_last_len, - dr16_frame_count, - referee.link_status, (unsigned)(HAL_GetTick() - referee.last_update_tick), - gimbal_motor_offline()); -#elif defined(ROBOT_TYPE_CHASSIS) - len = snprintf(line, sizeof(line), - "app=%u can=%u imu=%u acc=%02X gyro=%02X cvg=%u " - "ctoc=%u/%u att=%u/%d mot=%u md=%u err=%.1f fw=%d spin=%u lim=%u\r\n", - app_ready, dbg_can_loop, dbg_imu_loop, - bmi088_last_acc_id, bmi088_last_gyro_id, - INS.ConvergeFlag, - ctoc_remote.status, - (unsigned)(ctoc_remote.last_tick ? HAL_GetTick() - ctoc_remote.last_tick : 0), - ctoc_att_fresh(), (int)ctoc_gimbal_yaw, - chassis_motor_offline(), - cmd_robot.chassis_mode, - dbg_follow_err_deg, (int)dbg_follow_out, - ctoc_remote.spin_flag, referee.chassis_power_limit); -#endif + 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(500); + osDelay(DEBUG_OUT_PERIOD_MS); } }