ELCT/Simba-OmniInfantry
9
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix(ins/bmi088): 修复 yaw 匀速自漂(零偏标定被污染)与 IMU 数据冻结隐患

Browse Source 
现场数据(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 去掉这些排障字段)。

两个工程同步修改,内容一致。
This commit is contained in:
LSMushui
2026-09-12 11:27:42 +08:00
parent 72bf925f47
commit 9fc1575615
14 changed files with 302 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files
底盘/modules/BMI088/bmi088.c
+35 -1
View File
@@ -49,6 +49,8 @@ float bmi088_temperature;
uint8_t bmi088_online = 0;
uint8_t bmi088_last_acc_id = 0;
uint8_t bmi088_last_gyro_id = 0;
volatile uint32_t bmi088_accel_cnt = 0;
volatile uint32_t bmi088_gyro_cnt = 0;
static uint8_t spi_tx_buf[8];
static uint8_t spi_rx_buf[8];
@@ -63,6 +65,17 @@ typedef enum
} bmi088_phase_e;
static volatile bmi088_phase_e read_phase = BMI088_PHASE_NONE;
/* 当前读的启动时刻,用于完成回调丢失时的超时兜底(见 EXTI 回调) */
static volatile uint32_t read_phase_tick = 0;
/* 释放当前读对应的片选 */
static void cs_release(bmi088_phase_e phase)
{
if (phase == BMI088_PHASE_ACCEL || phase == BMI088_PHASE_TEMP)
BMI088_CS_ACC(1);
else
BMI088_CS_GYRO(1);
}
extern SPI_HandleTypeDef hspi1;
@@ -115,6 +128,7 @@ static void process_accel(void)
bmi088_accel[0] = raw[0] * 0.0008974358974f; // (raw/32768*3)*9.8
bmi088_accel[1] = raw[1] * 0.0008974358974f;
bmi088_accel[2] = raw[2] * 0.0008974358974f;
bmi088_accel_cnt++;
}
static void process_temperature(void)
@@ -133,6 +147,7 @@ static void process_gyro(void)
bmi088_gyro[0] = raw[0] * 0.0010652644360316952f; // (raw/32768*2000)/180*π
bmi088_gyro[1] = raw[1] * 0.0010652644360316952f;
bmi088_gyro[2] = raw[2] * 0.0010652644360316952f;
bmi088_gyro_cnt++;
}
/* 启动一次 SPI DMA 读(若 SPI 忙则丢弃本次,等下一个数据就绪沿) */
@@ -140,7 +155,9 @@ static void start_dma_read(bmi088_phase_e phase, uint8_t reg, uint8_t len)
{
if (HAL_SPI_GetState(&hspi1) != HAL_SPI_STATE_READY)
return;
read_phase = phase;
read_phase_tick = HAL_GetTick();
spi_tx_buf[0] = (uint8_t)(reg | 0x80);
for (uint8_t i = 1; i < len; i++)
spi_tx_buf[i] = 0x00;
@@ -150,7 +167,15 @@ static void start_dma_read(bmi088_phase_e phase, uint8_t reg, uint8_t len)
else
BMI088_CS_GYRO(0);
HAL_SPI_TransmitReceive_DMA(&hspi1, spi_tx_buf, spi_rx_buf, len);
/* 启动失败必须回滚:此时 read_phase 已置位、片选已拉低,若直接返回,
* 完成回调永远不会到来,read_phase 会永久停在非 NONE 状态,之后每个
* 数据就绪中断都被 if (read_phase == NONE) 挡掉 —— 表现为 IMU 数据
* 冻结在最后一个样本,而 bmi088_online 仍是 1(2026-09-12 排障) */
if (HAL_SPI_TransmitReceive_DMA(&hspi1, spi_tx_buf, spi_rx_buf, len) != HAL_OK)
{
cs_release(phase);
read_phase = BMI088_PHASE_NONE;
}
}
/* ---- 初始化 ---- */
@@ -260,6 +285,15 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
if (!bmi088_online)
return; // 初始化完成前不启动 DMA 读,避免占用 SPI
/* 超时兜底:一次 SPI 读远小于 1ms,若超过 20ms 还没等到完成回调,
* 说明回调丢了(SPI/DMA 异常),强行复位状态让数据路能自恢复,
* 否则会永久冻结 —— 与上面 start_dma_read 的失败回滚同一考虑 */
if (read_phase != BMI088_PHASE_NONE && (HAL_GetTick() - read_phase_tick) > 20U)
{
cs_release(read_phase);
read_phase = BMI088_PHASE_NONE;
}
if (GPIO_Pin == INT1_ACCEL_Pin)
{
if (read_phase == BMI088_PHASE_NONE)
底盘/modules/BMI088/bmi088.h
+6
View File
@@ -15,6 +15,12 @@ extern uint8_t bmi088_online; // 1-初始化成功 0-检测失败(有界重
extern uint8_t bmi088_last_acc_id; // 最近一次读到的加速度计 ID(诊断用,正常 0x1E)
extern uint8_t bmi088_last_gyro_id; // 最近一次读到的陀螺仪 ID(诊断用,正常 0x0F)
/* 采样计数(诊断用):在 SPI 完成回调解析成功后自增。
* **判断"IMU 数据是否还在更新"只看这两个计数是否持续增长** ——
* 数值变化与否不足以说明问题(数据可能冻结在最后一个样本上) */
extern volatile uint32_t bmi088_accel_cnt;
extern volatile uint32_t bmi088_gyro_cnt;
/* 初始化(任务上下文调用,含阻塞式寄存器配置与 ID 检测循环;失败时绿灯报警并重试) */
extern void bmi088_init(void);
底盘/modules/ins/ins.c
+55 -9
View File
@@ -16,11 +16,33 @@
#include "main.h"
#include <string.h>
/* z 轴静止零偏标定参数:QEKF 无法观测 z 偏置(见 ins.h),上电采 1s */
#define INS_Z_CAL_SAMPLES 1000
/* ---- z 轴静止零偏标定 ----
* QEKF 无法观测 z 偏置(见 ins.h),只能上电静止标定。
* 2026-09-12 加健壮性:原来直接对前 1000 个样本取均值,而 BMI088 刚初始化
* 完时 SPI DMA 还没稳定,且上电瞬间车体往往正被摆弄 —— 一旦标定窗口里混入
* 运动样本,gyro_z_offset 就被污染成一个偏大的常值,表现为**上电后 yaw 以
* 恒定角速度自漂**(实测 -3.85°/s,而原始 |gyro| 仅 0.017rad/s,量级对不上
* 才发现是零偏而非陀螺本身)。现在:
* 1. 先等采样路稳定(INS_Z_CAL_SETTLE_MS)
* 2. 单样本 |gyro| 超过 INS_Z_CAL_MAX_G 视为无效(静止时不该出现)
* 3. 一轮内有效样本占比不足 INS_Z_CAL_MIN_QUIET 视为"上电时车体在动",
* 整轮作废重测,最多 INS_Z_CAL_ATTEMPTS 轮
* 4. 多轮都不静则用最后一轮的均值并置 ins_z_cal_ok=0(有补偿总好过没有,
* 但是否可信由该标志告诉调用方/调试者)
* 单轮只取 0.5s:LED_Task 的 IWDG 上电宽限期是 5s,而 IMU_Task 在
* attitude_init() 期间不刷新心跳,所以整个初始化必须留在 5s 内 —— 可靠
* 性靠"整轮作废重测"而不是靠拉长单轮,3 轮最坏 1.55s,仅比原来的 1s 略长 */
#define INS_Z_CAL_SETTLE_MS 50
#define INS_Z_CAL_SAMPLES 500
#define INS_Z_CAL_MAX_G 0.35f /* rad/s,约 20°/s */
#define INS_Z_CAL_MIN_QUIET 0.9f
#define INS_Z_CAL_ATTEMPTS 3
static float gyro_z_offset = 0;
static uint8_t z_cal_done = 0;
float ins_gyro_z_offset = 0; /* 诊断用,见 ins.h */
uint8_t ins_z_cal_ok = 0; /* 诊断用,见 ins.h */
INS_t INS;
static const float xb[3] = {1, 0, 0};
@@ -31,15 +53,39 @@ void ins_init(void)
{
memset(&INS, 0, sizeof(INS));
/* z 轴静止零偏标定:上电静止采 1s 取平均(x/y 偏置由 EKF 在线估计,
* 无需校准;此期间 ins_update() 不输出,控制任务由 ins_ready() 门控) */
float sum = 0;
for (uint16_t i = 0; i < INS_Z_CAL_SAMPLES; i++)
/* 等 SPI 采样路稳定,避免把未就绪/残留的样本算进零偏 */
HAL_Delay(INS_Z_CAL_SETTLE_MS);
/* z 轴静止零偏标定(见文件头;x/y 偏置由 EKF 在线估计,无需校准)。
* 标定期间 ins_update() 不输出,控制任务由 ins_ready() 门控 */
for (uint8_t attempt = 0; attempt < INS_Z_CAL_ATTEMPTS; attempt++)
{
sum += bmi088_gyro[2];
HAL_Delay(1);
float sum = 0;
uint16_t valid = 0;
for (uint16_t i = 0; i < INS_Z_CAL_SAMPLES; i++)
{
const float gx = bmi088_gyro[0];
const float gy = bmi088_gyro[1];
const float gz = bmi088_gyro[2];
if (gx * gx + gy * gy + gz * gz <= INS_Z_CAL_MAX_G * INS_Z_CAL_MAX_G)
{
sum += gz;
valid++;
}
HAL_Delay(1);
}
if (valid > 0)
gyro_z_offset = sum / (float)valid;
if ((float)valid >= INS_Z_CAL_MIN_QUIET * (float)INS_Z_CAL_SAMPLES)
{
ins_z_cal_ok = 1; /* 整轮都静止,零偏可信 */
break;
}
/* 否则认为上电时车体在动,整轮作废、再来一轮 */
}
gyro_z_offset = sum / INS_Z_CAL_SAMPLES;
ins_gyro_z_offset = gyro_z_offset;
z_cal_done = 1;
INS.Initialized = 1;
底盘/modules/ins/ins.h
+7
View File
@@ -48,6 +48,13 @@ 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);