diff --git a/云台/Application/Task/Src/debug_task.c b/云台/Application/Task/Src/debug_task.c index 7b2a792..0d04872 100644 --- a/云台/Application/Task/Src/debug_task.c +++ b/云台/Application/Task/Src/debug_task.c @@ -1,18 +1,25 @@ /* ============================================================================= - * debug_task.c - USB 虚拟串口(CDC)欧拉角输出 - * 每 DEBUG_OUT_PERIOD_MS 输出一行 QuaternionEKF(见 modules/ins)解算出的 - * 欧拉角,单位度。板子插上电脑出现虚拟 COM 口,串口助手打开即可(波特率随意)。 + * debug_task.c - USB 虚拟串口(CDC)姿态输出(欧拉角 + 姿态链路健康位) + * 每 DEBUG_OUT_PERIOD_MS 输出一行。单位: 角度 ° / 加速度 m/s² / 角速度 rad/s * - * 输出格式: yaw=<> pitch=<> roll=<> cvg=<> + * ---- 欧拉角(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 环反馈) - * 以上为工程侧语义(att_deg_*);QEKF 内部 Pitch/Roll 命名与航空惯例互换, - * 映射关系与原因见 modules/attitude/attitude.c 文件头。 - * cvg 1-EKF 已有有效姿态输出(att_ready),0-未收敛,此时角度不可信 + * QEKF 内部 Pitch/Roll 命名与航空惯例互换,映射与原因见 attitude.c 文件头。 + * 注意 yaw 没有绝对基准(纯陀螺积分),上电静止时恒为该次上电的相对 0 点, + * 因此"yaw=0.00"本身不是故障特征;判断故障要看下面的健康位。 * - * 说明: 云台板与底盘板共用本文件,两者 IMU_Task 均运行 attitude_update(), - * 故 att_deg_* 在两种板上都有效,无需按 ROBOT_TYPE 分支。 + * ---- 健康位(排查"角度不动"时必须与角度一起看) ---- + * cvg 1-INS.ConvergeFlag:EKF 卡方检验通过、姿态可信(原诊断输出的 cvg + * 就是它,不要改成别的量,否则会丢掉"是否真的收敛"这一唯一判据); + * 0-未收敛/发散,此时角度不可信 + * 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;转动时明显变大说明陀螺通路正常 + * + * 不需要健康位时把 DEBUG_ATT_HEALTH 置 0,即只输出三个欧拉角。 * ========================================================================== */ #include "debug_task.h" #include "cmsis_os.h" @@ -20,21 +27,41 @@ #include "task.h" #include "usbd_cdc_if.h" #include "attitude.h" +#include "ins.h" +#include "bmi088.h" #include +#include -/* 输出周期(ms)。姿态观测建议 100ms(10Hz);如需接近原诊断节拍可改 500 */ +/* 1-输出欧拉角 + 姿态链路健康位; 0-只输出欧拉角 */ +#define DEBUG_ATT_HEALTH 1 + +/* 输出周期(ms)。姿态观测建议 100ms(10Hz) */ #define DEBUG_OUT_PERIOD_MS 100 void debug_task(void const * argument) { - char line[96]; + char line[128]; 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); + 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]); + + int len; +#if DEBUG_ATT_HEALTH + len = snprintf(line, sizeof(line), + "yaw=%.2f pitch=%.2f roll=%.2f cvg=%u rdy=%u imu=%u an=%.2f gn=%.3f\r\n", + att_deg_yaw, att_deg_pitch, att_deg_roll, + (unsigned)INS.ConvergeFlag, (unsigned)att_ready, + (unsigned)bmi088_online, an, gn); +#else + len = snprintf(line, sizeof(line), "yaw=%.2f pitch=%.2f roll=%.2f\r\n", + att_deg_yaw, att_deg_pitch, att_deg_roll); +#endif if (len > 0) CDC_Transmit_FS((uint8_t *)line, (uint16_t)len); diff --git a/云台/MDK-ARM/startup_stm32f407xx.s b/云台/MDK-ARM/startup_stm32f407xx.s index ef8311f..6281c75 100644 --- a/云台/MDK-ARM/startup_stm32f407xx.s +++ b/云台/MDK-ARM/startup_stm32f407xx.s @@ -40,7 +40,14 @@ __initial_sp ; Heap Size (in Bytes) <0x0-0xFFFFFFFF:8> ; -Heap_Size EQU 0x200 +; Heap raised from 0x200 (512B) to 0x2000 (8KB): +; modules/ins/kalman_filter.c allocates all KF matrix storage via libc +; malloc (it does not include cmsis_os.h, so user_malloc expands to +; malloc, not pvPortMalloc). Kalman_Filter_Init(6,0,3) issues 25 +; allocations totalling about 1.6KB with no NULL checks; a 512B heap +; made the later ones return NULL and silently broke the filter. +; 8KB matches the original RoboMaster-C-Board-INS-Example value. +Heap_Size EQU 0x2000 AREA HEAP, NOINIT, READWRITE, ALIGN=3 __heap_base diff --git a/底盘/Application/Task/Src/debug_task.c b/底盘/Application/Task/Src/debug_task.c index 7b2a792..0d04872 100644 --- a/底盘/Application/Task/Src/debug_task.c +++ b/底盘/Application/Task/Src/debug_task.c @@ -1,18 +1,25 @@ /* ============================================================================= - * debug_task.c - USB 虚拟串口(CDC)欧拉角输出 - * 每 DEBUG_OUT_PERIOD_MS 输出一行 QuaternionEKF(见 modules/ins)解算出的 - * 欧拉角,单位度。板子插上电脑出现虚拟 COM 口,串口助手打开即可(波特率随意)。 + * debug_task.c - USB 虚拟串口(CDC)姿态输出(欧拉角 + 姿态链路健康位) + * 每 DEBUG_OUT_PERIOD_MS 输出一行。单位: 角度 ° / 加速度 m/s² / 角速度 rad/s * - * 输出格式: yaw=<> pitch=<> roll=<> cvg=<> + * ---- 欧拉角(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 环反馈) - * 以上为工程侧语义(att_deg_*);QEKF 内部 Pitch/Roll 命名与航空惯例互换, - * 映射关系与原因见 modules/attitude/attitude.c 文件头。 - * cvg 1-EKF 已有有效姿态输出(att_ready),0-未收敛,此时角度不可信 + * QEKF 内部 Pitch/Roll 命名与航空惯例互换,映射与原因见 attitude.c 文件头。 + * 注意 yaw 没有绝对基准(纯陀螺积分),上电静止时恒为该次上电的相对 0 点, + * 因此"yaw=0.00"本身不是故障特征;判断故障要看下面的健康位。 * - * 说明: 云台板与底盘板共用本文件,两者 IMU_Task 均运行 attitude_update(), - * 故 att_deg_* 在两种板上都有效,无需按 ROBOT_TYPE 分支。 + * ---- 健康位(排查"角度不动"时必须与角度一起看) ---- + * cvg 1-INS.ConvergeFlag:EKF 卡方检验通过、姿态可信(原诊断输出的 cvg + * 就是它,不要改成别的量,否则会丢掉"是否真的收敛"这一唯一判据); + * 0-未收敛/发散,此时角度不可信 + * 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;转动时明显变大说明陀螺通路正常 + * + * 不需要健康位时把 DEBUG_ATT_HEALTH 置 0,即只输出三个欧拉角。 * ========================================================================== */ #include "debug_task.h" #include "cmsis_os.h" @@ -20,21 +27,41 @@ #include "task.h" #include "usbd_cdc_if.h" #include "attitude.h" +#include "ins.h" +#include "bmi088.h" #include +#include -/* 输出周期(ms)。姿态观测建议 100ms(10Hz);如需接近原诊断节拍可改 500 */ +/* 1-输出欧拉角 + 姿态链路健康位; 0-只输出欧拉角 */ +#define DEBUG_ATT_HEALTH 1 + +/* 输出周期(ms)。姿态观测建议 100ms(10Hz) */ #define DEBUG_OUT_PERIOD_MS 100 void debug_task(void const * argument) { - char line[96]; + char line[128]; 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); + 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]); + + int len; +#if DEBUG_ATT_HEALTH + len = snprintf(line, sizeof(line), + "yaw=%.2f pitch=%.2f roll=%.2f cvg=%u rdy=%u imu=%u an=%.2f gn=%.3f\r\n", + att_deg_yaw, att_deg_pitch, att_deg_roll, + (unsigned)INS.ConvergeFlag, (unsigned)att_ready, + (unsigned)bmi088_online, an, gn); +#else + len = snprintf(line, sizeof(line), "yaw=%.2f pitch=%.2f roll=%.2f\r\n", + att_deg_yaw, att_deg_pitch, att_deg_roll); +#endif if (len > 0) CDC_Transmit_FS((uint8_t *)line, (uint16_t)len); diff --git a/底盘/MDK-ARM/startup_stm32f407xx.s b/底盘/MDK-ARM/startup_stm32f407xx.s index ef8311f..6281c75 100644 --- a/底盘/MDK-ARM/startup_stm32f407xx.s +++ b/底盘/MDK-ARM/startup_stm32f407xx.s @@ -40,7 +40,14 @@ __initial_sp ; Heap Size (in Bytes) <0x0-0xFFFFFFFF:8> ; -Heap_Size EQU 0x200 +; Heap raised from 0x200 (512B) to 0x2000 (8KB): +; modules/ins/kalman_filter.c allocates all KF matrix storage via libc +; malloc (it does not include cmsis_os.h, so user_malloc expands to +; malloc, not pvPortMalloc). Kalman_Filter_Init(6,0,3) issues 25 +; allocations totalling about 1.6KB with no NULL checks; a 512B heap +; made the later ones return NULL and silently broke the filter. +; 8KB matches the original RoboMaster-C-Board-INS-Example value. +Heap_Size EQU 0x2000 AREA HEAP, NOINIT, READWRITE, ALIGN=3 __heap_base