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,互不重叠。
两个工程同步修改,内容一致。
55 lines
2.1 KiB
C
55 lines
2.1 KiB
C
/* =============================================================================
|
|
* attitude.c - 姿态解算封装(BMI088 + QuaternionEKF 惯导)
|
|
* 2026-08-31 起由 Mahony 互补滤波更换为 Wang Hongxi 的 QuaternionEKF:
|
|
* - 陀螺零偏在线估计,不再需要上电静止校准(1s 阻塞校准已移除)
|
|
* - 卡方检验剔除加速度异常帧,动态响应/抗震动干扰优于互补滤波
|
|
* - 欧拉角映射(与旧 Mahony get_yaw/pitch/roll 语义一一对应,勿改):
|
|
* att_deg_yaw <- QEKF_INS.Yaw (标准偏航;取多圈累计 YawTotalAngle)
|
|
* att_deg_roll <- QEKF_INS.Pitch (绕 x 轴;本云台 Pitch 环反馈用它)
|
|
* att_deg_pitch <- QEKF_INS.Roll (绕 y 轴)
|
|
* 原因:QEKF 内部变量命名与航空惯例互换,见 ins.c 文件头
|
|
* 数据链: bmi088.c(SPI DMA 后台更新) -> ins_update(1kHz) -> att_deg_*
|
|
* 健康位: bmi088_online=0 时姿态数据不可用(云台任务会拒绝输出);
|
|
* 姿态收敛需数秒,上电初期 ConvergeFlag=0 属正常
|
|
* ========================================================================== */
|
|
#include "attitude.h"
|
|
#include "bmi088.h"
|
|
#include "ist8310.h"
|
|
#include "ins.h"
|
|
#include "tim.h"
|
|
|
|
extern TIM_HandleTypeDef htim10;
|
|
|
|
float att_deg_yaw; // 多圈连续偏航角(°)
|
|
float att_deg_pitch; // 绕 y 轴角(°)
|
|
float att_deg_roll; // 绕 x 轴角(°,本云台 Pitch 环反馈)
|
|
uint8_t att_ready = 0; // 1-标定完成且姿态已开始输出(控制任务门控用)
|
|
|
|
void attitude_init(void)
|
|
{
|
|
ist8310_init();
|
|
bmi088_init();
|
|
ins_init();
|
|
}
|
|
|
|
void attitude_update(void)
|
|
{
|
|
/* 先做数据路健康检查:IMU 数据一旦冻结,姿态会安静地"保持不动"甚至
|
|
* 匀速漂移(见 bmi088.c 的 bmi088_health_check 说明) */
|
|
bmi088_health_check();
|
|
|
|
ins_update(0.001f); // 1kHz 固定节拍
|
|
|
|
att_deg_yaw = INS.YawTotalAngle; // 多圈连续角,云台 Yaw 环直接使用
|
|
att_deg_roll = INS.Pitch; // QEKF 命名互换,见文件头
|
|
att_deg_pitch = INS.Roll;
|
|
|
|
if (INS.UpdateCount > 0)
|
|
att_ready = 1;
|
|
}
|
|
|
|
void imu_heater_off(void)
|
|
{
|
|
__HAL_TIM_SET_COMPARE(&htim10, TIM_CHANNEL_1, 0);
|
|
}
|