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 去掉这些排障字段)。
两个工程同步修改,内容一致。
63 lines
2.9 KiB
C
63 lines
2.9 KiB
C
/* =============================================================================
|
|
* ins.h - 惯导姿态解算封装(移植自 RoboMaster-C-Board-INS-Example, Wang Hongxi)
|
|
* 核心: QuaternionEKF 四元数 EKF——陀螺积分预测 + 加速度重力方向观测,
|
|
* 在线估计 x/y 陀螺零偏,卡方检验剔除加速度异常(撞击/运动)帧
|
|
* 对比旧 Mahony 方案: 收敛更快、零偏在线估计(无需上电静止校准)、
|
|
* 动态加速度干扰抑制更好(卡方检验)
|
|
* 数据源: 本工程 BMI088 驱动的 bmi088_accel/gyro(SPI DMA 后台更新)
|
|
* 调用: ins_init() 一次,ins_update(dt) 每 1ms;dt 用固定 0.001
|
|
* (IMU_Task 为 vTaskDelayUntil 精确 1kHz 节拍,无需 DWT 计时)
|
|
* ========================================================================== */
|
|
#ifndef INS_H
|
|
#define INS_H
|
|
|
|
#include <stdint.h>
|
|
|
|
typedef struct
|
|
{
|
|
uint8_t Initialized;
|
|
|
|
float Gyro[3]; // rad/s(零偏修正后)
|
|
float Accel[3]; // m/s^2
|
|
float q[4]; // 四元数 w x y z
|
|
|
|
float Yaw; // -180~180(°)
|
|
float Pitch; // QEKF 命名(实际为绕 x 轴角,本工程映射见 attitude.c)
|
|
float Roll; // 同上
|
|
float YawTotalAngle; // 多圈连续 Yaw(°)
|
|
|
|
float GyroBias[3]; // EKF 在线估计的陀螺零偏(rad/s)
|
|
float MotionAccel_b[3]; // 运动加速度(机体系,去除重力,低通)
|
|
uint8_t StableFlag; // 1-载体静止且加速度正常(可用于 ZUPT 等扩展)
|
|
uint8_t ConvergeFlag; // 1-卡方检验收敛(姿态可信)
|
|
float ChiSquare; // 最近一次卡方检验值(诊断用)
|
|
uint32_t UpdateCount; // 更新计数(诊断用)
|
|
} INS_t;
|
|
|
|
extern INS_t INS;
|
|
|
|
/* 初始化:先做 z 轴静止零偏标定(约 1s 阻塞,车体必须静止——QEKF 的
|
|
* 卡方观测无法感知 z 偏置,GyroBias[2] 恒为 0,不标定会导致 Yaw 持续漂移),
|
|
* 再初始化 EKF(参数为 INS 例程 demo 值,调参见 QuaternionEKF.c 头注释) */
|
|
extern void ins_init(void);
|
|
|
|
/* 姿态更新(1kHz 调用,dt 单位秒;内部读 bmi088_accel/gyro 全局量)
|
|
* 标定完成前直接返回,不更新 */
|
|
extern void ins_update(float dt);
|
|
|
|
/* 1-标定完成且已有有效姿态输出(控制任务以此为门控) */
|
|
extern uint8_t ins_ready(void);
|
|
|
|
/* ---- z 轴零偏标定的诊断量(见 ins.c 文件头) ----
|
|
* 姿态异常时先看这两个量和 debug_task 的 gz/cvg 是否自洽:
|
|
* 若 gyro_z_offset 明显偏离静止时的真实零偏,yaw 会以 -(offset) 的恒定
|
|
* 角速度自漂,与陀螺原始输出无关 */
|
|
extern float ins_gyro_z_offset; // 实际采用的 z 零偏(rad/s)
|
|
extern uint8_t ins_z_cal_ok; // 1-标定时车体静止,零偏可信;0-始终在动
|
|
|
|
/* 坐标系变换工具(机体系 <-> 导航系) */
|
|
extern void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q);
|
|
extern void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q);
|
|
|
|
#endif
|