LSMushui
3e5e5e3f95
现场遥测(zo/ck 字段生效,说明新固件在跑):
yaw -18.13 -> -18.61 / 0.7s , pitch/roll/an 全部纹丝不动
ca=832 cg=1032 —— **8 行完全一致,采样计数停止增长**
zo=-0.0009 ck=1 (零偏标定正常) , gx/gy/gz 也恒定
## 判定
ca/cg 不增长 = SPI 完成回调不再产生样本,IMU 数据已冻结在最后一个样本上。
此时 yaw 的"匀速自漂"只是 EKF 在忠实积分冻结值:
gz - zo = -0.0117 - (-0.0009) = -0.0108 rad/s ≈ 观测 -0.0119 rad/s
所以姿态算法本身是对的,问题全在数据路。上一轮加的 z 标定健壮性生效了
(zo 从 0.067 降到 0.0009,ck=1),但数据路仍会停摆。
## 为什么必须放在任务上下文
停摆只有两类原因,而**中断回调里的兜底两类都救不了**:
a) hspi1 卡在 BUSY(某次 SPI/DMA 出错后 HAL 没回到 READY):
start_dma_read() 的 HAL_SPI_GetState 检查让每次调用都直接返回;
b) BMI088 的 DRDY 是电平型信号:某次失败/被挡掉的读没把传感器的数据
寄存器读走,INT 电平就保持住,之后既无下降沿也无上升沿 ——
EXTI 永不触发,写在 EXTI 回调里的任何兜底都不会执行。
故新增 bmi088_health_check(),由 attitude_update() 每 1ms 调用:采样计数
超过 BMI088_STALL_MS(5ms)没增长即判停摆,执行
关 EXTI -> 复位 read_phase/片选 -> SPI 卡住则 abort,仍不 READY 就
DeInit+Init(hspi1.Init 里仍是模式3/16分频,不会被改回)
-> 清 EXTI 挂起标志 -> 阻塞读一次加速度+陀螺(顺带清传感器 DRDY 电平)
-> 重新使能 EXTI
若中断路始终修不好,则退化为每 5ms 补读一次(约 200Hz)的轮询保底。因为
dt 固定 1ms、1kHz 调用,1 秒内积分出的总转角仍然正确(保持值×0.001×1000),
只是运动解析粒度变粗 —— 用于排障足够,正式使用应查清停摆根源。
## 附带
- 实现 HAL_SPI_ErrorCallback(HAL 里是 __weak):此前完全没实现,SPI/DMA
出错后相位与片选无人复位;现在复位相位/片选并累加 bmi088_spi_errors。
- 新增 bmi088_spi_state() 暴露 HAL_SPI_GetState,debug_task 追加
se=<SPI错误数> st=<SPI状态,1=READY>。若 st 长期非 1 即命中原因 (a);
若 st 恒为 1 而 ca/cg 不涨,即命中原因 (b)。
- 已静态核对 DMA 流分配无冲突:SPI1 RX/TX = DMA2_Stream2/3 ch3,
USART1 = Stream5/7, USART6 = Stream1/6, USART3_RX = DMA1_S1,
I2C2_TX = DMA1_S7,互不重叠。
两个工程同步修改,内容一致。
90 lines
4.3 KiB
C
90 lines
4.3 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 加速度/陀螺采样计数:停止增长即数据路已死(数值冻结时它们不涨)
|
|
* se bmi088_spi_errors:SPI/DMA 出错次数;st=bmi088_spi_state()
|
|
* (1=READY,非 1 说明 SPI 被卡住,数据路会因此停摆)
|
|
*
|
|
* 开关: 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 se=%lu st=%u",
|
|
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,
|
|
(unsigned long)bmi088_spi_errors, (unsigned)bmi088_spi_state());
|
|
#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);
|
|
}
|
|
}
|