/* ============================================================================= * 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 时追加;用于排查 控制源→cmd_robot→执行机构) ---- * online 1=**遥控器(DR16)在线** —— 本板这一位只反映 DR16,键鼠不置位! * 没有遥控器时整段控制分支都不执行(见 robot_cmd_task.c 的 online 门控), * 所以"键鼠测试"也必须有遥控器在线,否则这里恒为 0。 * src 0=遥控器 1=键鼠(判据:0x0304 在 1s 内到过 keyboard_data.last_tick) * spin 小陀螺使能(云台侧检测,随 0x189 下发给底盘) * yaw_d/pitch_d 本周期云台角度增量(°/2ms 任务周期,×500 即 °/s) * fric/fire/eject 发射机构开关 reset 云台角度复位请求 * s2/ctrl 遥控器 S2 档位与 Ctrl 按下沿 —— 底盘就是靠这两个量推出 * chassis_mode,所以本板不输出模式本身(本板该字段恒为 OFF) * wheel 遥控拨轮原始值(1000~1050 为中立) * --- 以下为 DR16 帧内容诊断(判断"键鼠数据到底有没有到机器人") --- * s1 左侧拨杆(1上 2下 3中)。注意官方 RoboRTS 的 dbus.c 与本工程一致: * sw2=bit44-45→s2, sw1=bit46-47→s1;若与实物相反,用本列与 s2 对照即可判定 * mouse_rl/mouse_du/mouse_wheel 帧里 data[6..11] 解析出的鼠标三轴 * mouse_l/mouse_r 帧里 data[12]/data[13](鼠标左右键) * key_lo/key_hi 帧里 data[14]/data[15] **原始字节**(不做位拆分) * ★ 按下 W 时 key_lo 应变 0x01;若恒为 0,说明帧里根本没有键鼠数据 * frames dr16_frame_count;持续增长说明 USART3 一直在收帧 * 无健康位(DEBUG_ATT_HEALTH=0)时输出为纯数字 CSV,列序即上面顺序, * 可被 Lissio FireWater 直接解析成通道绘图。 * * 开关: 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 "gimbal.h" #include "dr16.h" #include "robot_typedef.h" #include #include #include /* ===== 2026-10-15 台架调参临时遥测(已停用,保留备查) ===== * 启用时把 #if 0 改 #if 1:本任务改为输出 5 列 CSV * setpoint_deg, actual_deg, out_current, ecd_deg, gyro_x_dps * (逗号分隔+换行,可被 Lissio FireWater 自动识别为 5 通道),采样率提到 200Hz, * 用于定量观测阶跃响应与判断"轴是否真的在转动"。 */ #if 0 #include "robot_param.h" #define DEBUG_PITCH_TUNE 1 #if DEBUG_PITCH_TUNE #define DEBUG_OUT_PERIOD_MS 5 /* 200Hz:2ms 控制环的阶跃响应需 ≥100Hz 采样才量得准 */ #endif #endif /* 1-输出健康位(cvg/rdy/imu/an/gn); 0-只输出三个欧拉角 */ #define DEBUG_ATT_HEALTH 0 /* 1-再附加原始陀螺/零偏/采样计数(排障用); 0-只留上面的通用健康位 */ #define DEBUG_ATT_DIAG 0 /* 1-仅输出 Pitch 调参行(便于用 VOFA+/串口示波器绘图); 0-同时输出姿态行与 Pitch 调参行 */ #define DEBUG_PITCH_ONLY 0 /* 1-追加控制量字段(见文件头); 0-保持原姿态输出不变 */ #define DEBUG_CMD_OUT 1 /* 输出周期(ms)。姿态观测建议 100ms(10Hz) */ #define DEBUG_OUT_PERIOD_MS 100 void debug_task(void const * argument) { char line[384]; char tail[112]; char pitch_line[128]; for (;;) { #if 0 /* ===== 2026-10-15 台架 Pitch 调参遥测(已停用,保留备查) ===== * 5 列 CSV → Lissio FireWater 直接成 5 通道: * setpoint(位置环设定值含台架偏置), actual(-att_deg_roll), * out(最终电流), ecd_deg(编码器机械角), gyro_x_dps(陀螺 x 轴角速度) */ const float tune_ecd_deg = ((float)motor_pitch_gimbal.fdb.raw_angle - (float)PITCH_GM6020_ENCODER_ZERO) * (360.0f / 8192.0f); const float tune_gyro_dps = bmi088_gyro[0] * 57.29578f; int len = snprintf(line, sizeof(line), "%.3f,%.3f,%.1f,%.3f,%.2f\n", gimbal_pitch_setpoint_deg(), -att_deg_roll, pitch_current, tune_ecd_deg, tune_gyro_dps); #endif #if DEBUG_PITCH_ONLY /* 仅输出 Pitch 调参数据(编码值、roll、PID最后输出值、遥控pitch杆量) */ int len = snprintf(line, sizeof(line), "p_ecd=%u roll=%.2f p_out=%.1f rc_pitch=%u\r\n", (unsigned)motor_pitch_gimbal.fdb.raw_angle, att_deg_roll, pitch_current, (unsigned)dr16.rc_l_UD); #else 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_ATT_HEALTH #if DEBUG_CMD_OUT /* 健康位分支是文本格式,控制量就按 key=value 追加,便于人读 */ char ctl[160]; snprintf(ctl, sizeof(ctl), " online=%u src=%u spin=%u yaw_d=%.4f pitch_d=%.4f fric=%u fire=%u" " eject=%u reset=%u s2=%u ctrl=%u wheel=%d", (unsigned)cmd_robot.online, (unsigned)cmd_robot.source, (unsigned)cmd_robot.spin_flag, cmd_robot.gimbal_yaw_delta, cmd_robot.gimbal_pitch_delta, (unsigned)cmd_robot.friction_on, (unsigned)cmd_robot.fire, (unsigned)cmd_robot.eject, (unsigned)cmd_robot.gimbal_reset, (unsigned)dr16.s2, (unsigned)dr16.key_push_ctrl, (int)cmd_robot.thumb_wheel); #else const char * ctl = ""; #endif int len = snprintf(line, sizeof(line), "ecd=%u yaw=%.2f pitch=%.2f roll=%.2f cvg=%u rdy=%u imu=%u an=%.2f gn=%.3f%s%s\r\n", (unsigned)motor_yaw_gimbal.fdb.raw_angle, att_deg_yaw, att_deg_pitch, att_deg_roll, (unsigned)INS.ConvergeFlag, (unsigned)att_ready, (unsigned)bmi088_online, an, gn, tail, ctl); #else #if DEBUG_CMD_OUT /* 纯数字 CSV。前 3 列保持原样(pitch_ecd, pitch_current, yaw_ecd),其余: * online,src,spin, yaw_d,pitch_d, fric,fire,eject,reset, s2,ctrl,wheel, * s1, mouse_rl,mouse_du,mouse_wheel,mouse_l,mouse_r, key_lo,key_hi, * dr16_frame_count * 末尾这 9 列是 DR16 帧内容诊断:key_lo/key_hi 就是帧里的 data[14]/data[15]。 * 按住 W 时若它们仍为 0,说明帧里本就没有键鼠数据(不是解析问题)。 */ int len = snprintf(line, sizeof(line), "%u,%.1f,%u,%u,%u,%u,%.4f,%.4f,%u,%u,%u,%u,%u,%u,%d,%u,%d,%d,%d,%u,%u,%u,%u,%lu\r\n", (unsigned)motor_pitch_gimbal.fdb.raw_angle, pitch_current, (unsigned)motor_yaw_gimbal.fdb.raw_angle, (unsigned)cmd_robot.online, (unsigned)cmd_robot.source, (unsigned)cmd_robot.spin_flag, cmd_robot.gimbal_yaw_delta, cmd_robot.gimbal_pitch_delta, (unsigned)cmd_robot.friction_on, (unsigned)cmd_robot.fire, (unsigned)cmd_robot.eject, (unsigned)cmd_robot.gimbal_reset, (unsigned)dr16.s2, (unsigned)dr16.key_push_ctrl, (int)cmd_robot.thumb_wheel, (unsigned)dr16.s1, (int)dr16.mouse_rl, (int)dr16.mouse_du, (int)dr16.mouse_wheel, (unsigned)dr16.mouse_key_l, (unsigned)dr16.mouse_key_r, (unsigned)dr16.key_code_lo, (unsigned)dr16.key_code_hi, (unsigned long)dr16_frame_count); #else int len = snprintf(line, sizeof(line), "%u,%.1f,%u\r\n", (unsigned)motor_pitch_gimbal.fdb.raw_angle, pitch_current,(unsigned)motor_yaw_gimbal.fdb.raw_angle); #endif #endif #endif if (len > 0) CDC_Transmit_FS((uint8_t *)line, (uint16_t)len); osDelay(DEBUG_OUT_PERIOD_MS); } }