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,不能作为故障判据
This commit is contained in:
@@ -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 <stdio.h>
|
||||
#include <math.h>
|
||||
|
||||
/* 输出周期(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",
|
||||
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)att_ready);
|
||||
(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);
|
||||
|
||||
@@ -40,7 +40,14 @@ __initial_sp
|
||||
; <o> Heap Size (in Bytes) <0x0-0xFFFFFFFF:8>
|
||||
; </h>
|
||||
|
||||
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
|
||||
|
||||
@@ -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 <stdio.h>
|
||||
#include <math.h>
|
||||
|
||||
/* 输出周期(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",
|
||||
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)att_ready);
|
||||
(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);
|
||||
|
||||
@@ -40,7 +40,14 @@ __initial_sp
|
||||
; <o> Heap Size (in Bytes) <0x0-0xFFFFFFFF:8>
|
||||
; </h>
|
||||
|
||||
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
|
||||
|
||||
Reference in New Issue
Block a user