LSMushui
3c95bb0887
原 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 的注释说明。
45 lines
1.8 KiB
C
45 lines
1.8 KiB
C
/* =============================================================================
|
|
* 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);
|
|
}
|
|
}
|