ELCT/Simba-OmniInfantry
9
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
main
Simba-OmniInfantry/云台/Application/Task/Src/debug_task.c
T
lyf 267a659242 feat(ui): 新增裁判系统自定义UI(0x0301)并打通键鼠→底盘链路 
键鼠控制(此前 WASD 无反应):
- 云台 robot_cmd_task:0x0304 与 USART3 帧内键鼠合并为共享视图 keyboard_merged(0x0304 优先);
  键鼠模式由帧内键鼠动作进入,动云台左摇杆退出
- c_comm_task/c_comm:键鼠在控时底盘摇杆由 WASD+鼠标合成,改读共享视图(此前只认 0x0304)
- dr16/调试输出:新增帧内键鼠原始字节诊断(key_lo/key_hi/mouse_*/s1/frames)

自定义UI(底盘板经裁判系统 USART6 绘制到选手端):
- 新增 底盘/modules/comms/referee_ui.c|h:15 字节图形组包(带编译期断言),
  v5 卡片式 HUD = 俯仰梯(pitch) + 罗盘刻度带(底盘朝向) + 预估底盘功率 + 超级电容
  + 摩擦轮 L/R 转速 + 小陀螺状态,yaw 在刻度带右侧;5Hz 刷新,约 50 图元/8 包
- referee.c 新增 referee_send_frame()(cmd 0x0301 机器人交互数据,CRC8+CRC16,USART6 TX DMA)
- 新增 CToC 0x1B9:云台把 pitch 与摩擦轮转速带给底盘(底盘本地没有这两个量)
- 版式避开官方保留区:顶部比分条/左下小地图/右下战车模块/中部两块官方保留

验证:云台 Code=99540 RO=1764 RW=420 ZI=62572;底盘 Code=99452 RO=1928 RW=416 ZI=62584;均 0 错误 0 警告。
未做实机验证(当前无板/无图传);UI 协议常量(UI_RECEIVER_ID/UI_SEND_HZ)与版式坐标集中在 referee_ui.h 宏区,便于按协议 PDF 一处校正。
2026-10-16 04:24:32 +08:00

214 lines
11 KiB
C
Raw Permalink Blame History

/* =============================================================================
* 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 <stdio.h>
#include <math.h>
#include <string.h>
/* ===== 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);
}
}
Reference in New Issue View Git Blame Copy Permalink