LSMushui
9fc1575615
现场数据(yaw 以恒定角速度自漂,陀螺却不响应板子移动):
yaw=-213.09 -> -223.10,2.60s 内 -10.01° => -3.8500 °/s = -0.06720 rad/s
同窗口 gn = 0.017 rad/s = 0.9740 °/s 比值 3.95
an=9.80 恒定、pitch/roll 基本不动、cvg=1、imu=1
## 根因一:gyro_z_offset 被运动样本污染(本次主因)
gn 打印的是 bmi088_gyro[] 原始模长(未减零偏),而实际进滤波器的是
gyro_z = gz - gyro_z_offset。漂移率 0.0672 rad/s 比原始 |gyro| 0.017
还大近 4 倍,只可能是 gyro_z_offset 本身是个约 +0.067 rad/s 的假零偏。
ins_init() 原来直接对前 1000 个样本取均值:上电瞬间 BMI088 的 SPI DMA
路刚起步,且车体常正被摆弄,运动样本一旦混进窗口,零偏就被标歪 ——
之后 yaw 便以该常值匀速自漂,与陀螺真实输出无关。
修: ins.c 的 z 标定改为
- 先等采样路稳定(INS_Z_CAL_SETTLE_MS=50ms)
- 单样本 |gyro| > 0.35rad/s(20°/s)视为无效,静止时不该出现
- 一轮有效样本占比 <90% 视为"上电时车体在动",整轮作废重测,最多 3 轮
- 单轮取 0.5s:LED_Task 的 IWDG 上电宽限期只有 5s,而 IMU_Task 在
attitude_init() 期间不刷心跳,整个初始化必须留在 5s 内,可靠性靠
"整轮重测"而非拉长单轮(3 轮最坏 1.55s,仅比原来 1s 略长)
- 多轮都不静则用最后一轮均值并置 ins_z_cal_ok=0
## 根因二(顺带修):EXTI 触发沿与传感器/文档不符
bmi088.c 文件头与 bmi088.h 都写"数据就绪下降沿触发",legacy 用的也是
EXTI_Trigger_Falling,但 CubeMX 生成的 gpio.c 配的是 GPIO_MODE_IT_RISING
—— 会落在数据就绪电平的恢复沿上,采样时序整体错位。已改为
GPIO_MODE_IT_FALLING,并在 .ioc 里补 GPIO_ModeDefaultEXTI=GPIO_MODE_IT_FALLING
(否则重新生成工程又会被改回 RISING)。
## 根因三(隐患):数据路可静默冻结
start_dma_read 里 read_phase 先置位、片选先拉低,之后若
HAL_SPI_TransmitReceive_DMA 启动失败就直接返回,完成回调永不到来,
read_phase 永久卡在非 NONE:此后每个数据就绪中断都被
if (read_phase == NONE) 挡掉,数据冻结在最后一个样本,而 bmi088_online
仍是 1、an/gn 看着还挺正常。修:失败则释放片选并回滚 read_phase;
另在 EXTI 回调加 20ms 超时兜底(一次 SPI 读远小于 1ms),回调丢失时
强行复位,保证能自恢复。
## 遥测:让这类问题一眼可见
bmi088.c 增加 bmi088_accel_cnt / bmi088_gyro_cnt(解析成功后自增),
ins.c 暴露 ins_gyro_z_offset / ins_z_cal_ok,debug_task 输出扩展为
yaw= pitch= roll= cvg= rdy= imu= an= gn= gx= gy= gz= zo= ck= ca= cg=
其中 ca/cg 是否增长是判断"数据是否还在更新"的唯一可靠依据(数值冻结时
它们不涨);zo 与 gz 量级对比可直接看出零偏是否被标歪。新增
DEBUG_ATT_DIAG 开关(置 0 去掉这些排障字段)。
两个工程同步修改,内容一致。
87 lines
4.1 KiB
C
87 lines
4.1 KiB
C
/* =============================================================================
|
|
* 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 加速度/陀螺采样计数:停止增长即数据路已死(数值冻结时它们不涨)
|
|
*
|
|
* 开关: DEBUG_ATT_HEALTH=0 只输出三个欧拉角;DEBUG_ATT_DIAG=0 去掉诊断位。
|
|
* ========================================================================== */
|
|
#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 <stdio.h>
|
|
#include <math.h>
|
|
|
|
/* 1-输出健康位(cvg/rdy/imu/an/gn); 0-只输出三个欧拉角 */
|
|
#define DEBUG_ATT_HEALTH 1
|
|
/* 1-再附加原始陀螺/零偏/采样计数(排障用); 0-只留上面的通用健康位 */
|
|
#define DEBUG_ATT_DIAG 1
|
|
|
|
/* 输出周期(ms)。姿态观测建议 100ms(10Hz) */
|
|
#define DEBUG_OUT_PERIOD_MS 100
|
|
|
|
void debug_task(void const * argument)
|
|
{
|
|
char line[192];
|
|
char tail[112];
|
|
|
|
for (;;)
|
|
{
|
|
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",
|
|
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);
|
|
#endif
|
|
|
|
#if DEBUG_ATT_HEALTH
|
|
int len = snprintf(line, sizeof(line),
|
|
"yaw=%.2f pitch=%.2f roll=%.2f cvg=%u rdy=%u imu=%u an=%.2f gn=%.3f%s\r\n",
|
|
att_deg_yaw, att_deg_pitch, att_deg_roll,
|
|
(unsigned)INS.ConvergeFlag, (unsigned)att_ready,
|
|
(unsigned)bmi088_online, an, gn, tail);
|
|
#else
|
|
int 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);
|
|
|
|
osDelay(DEBUG_OUT_PERIOD_MS);
|
|
}
|
|
}
|