 LSMushui
|
4f18ada2f7
|
fix(ins): 修复 QEKF 矩阵内存分配失败(堆 512B -> 8KB),并恢复姿态诊断位
## 根因: 堆太小导致卡尔曼滤波静默失效
modules/ins/kalman_filter.h 的分配宏是
#ifdef _CMSIS_OS_H
#define user_malloc pvPortMalloc
#else
#define user_malloc malloc
#endif
而 kalman_filter.c 整个编译单元只 include 了 kalman_filter.h,从未引入
cmsis_os.h,因此 _CMSIS_OS_H 未定义 -> user_malloc 展开为 libc malloc,
FreeRTOS 那 32KB(configTOTAL_HEAP_SIZE=32768)堆完全用不上。
libc malloc 的堆来自 MDK-ARM/startup_stm32f407xx.s 的 Heap_Size,CubeMX
默认值是 0x200 = 512 字节。而 Kalman_Filter_Init(6, 0, 3) 会发起 25 次
分配、合计约 1602 字节(P/Pminus/F/FT/Q/S/temp_matrix/temp_matrix1 各
144B,H/HT/K 各 72B,R 36B,Q 系列 24B 等),且全程没有一处 NULL 检查,
分配失败后紧接着 memset(NULL, 0, n) 并把 NULL 直接交给 arm_mat_* 使用:
- 前半段小分配成功、后半段 144B 级分配返回 NULL
- arm_mat_* 对 pData=NULL 只读写地址 0 附近,函数本身不报错、只是不改
动输出 -> 滤波器不再更新状态,但 QEKF_INS.UpdateCount 照常自增
- 表现为: att_ready=1(看起来"在跑")、欧拉角却恒定不动
对照原始例程(能正常工作的那版)RoboMaster-C-Board-INS-Example-master,
其 MDK-ARM/startup_stm32f407xx.s 的 Heap_Size 为 0x2000 = 8KB,是本工程
的 16 倍。本次将两个工程的 Heap_Size 一并提到 0x2000,与该例程一致。
注意: startup_stm32f407xx.s 由 CubeMX 生成,若日后重新生成工程,这行会
被改回 0x200,需再次修改(或改走 pvPortMalloc 路线)。注释用纯 ASCII,
避免 armasm 处理非 ASCII 源文件。
## 恢复姿态诊断位
上一版 debug_task.c 把 cvg 换成了 att_ready,丢掉了唯一的"是否真的收敛"
判据 —— 于是"IMU 没数据"和"滤波器没跑"都表现为 cvg=1 + 三个 0.00,无法
区分。本次:
- cvg 恢复为 INS.ConvergeFlag(与原诊断输出同义)
- 新增 rdy(=att_ready)、imu(=bmi088_online)、an(=|accel|)、
gn(=|gyro|) 四个姿态链路健康位
- 保留 DEBUG_ATT_HEALTH 开关,置 0 即只输出三个欧拉角
- 文件头补注: yaw 无绝对基准,上电静止时恒为 0,不能作为故障判据
|
2026-09-12 11:15:16 +08:00 |
|
|
LSMushui
|
3c95bb0887
|
feat(debug): USB CDC 输出改为 QuaternionEKF 欧拉角
原 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 的注释说明。
|
2026-09-12 10:33:29 +08:00 |
|