ELCT/Simba-OmniInfantry
9
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
9fc15756159be46bd16d83bc0b9e887dafd865dd
Simba-OmniInfantry/云台/modules/BMI088/bmi088.c
T
LSMushui 9fc1575615 fix(ins/bmi088): 修复 yaw 匀速自漂(零偏标定被污染)与 IMU 数据冻结隐患 
现场数据(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 去掉这些排障字段)。

两个工程同步修改,内容一致。
2026-09-12 11:27:42 +08:00

340 lines
12 KiB
C
Raw Blame History

/* =============================================================================
* bmi088.c - C 板板载六轴 IMU BMI088 驱动(HAL 版,架构与 legacy 一致)
* 初始化(bmi088_init,任务上下文):
* 1. 先关 EXTI4/EXTI9_5 NVIC(数据就绪中断若在初始化期间触发 DMA 读,
* 会把 SPI 永久占在 BUSY,导致读 ID 全部失败--疯转/绿灯闪的根因)
* 2. 重配 SPI1 为模式3(CPOL=1,CPHA=2边沿)/16 分频(5.25MHz,
* CubeMX 生成的是模式0/2分频 42MHz,BMI088 不支持)
* 3. 有界重试读 ID(acc=0x1E, gyro=0x0F),成功后按 legacy 序列配置寄存器
* 4. 清 EXTI 挂起标志后放行中断
* 运行: EXTI 下降沿 -> start_dma_read() SPI DMA 读取 -> TxRx 完成回调解析
* (加速度 8 字节 -> 链式读温度 4 字节;陀螺 7 字节),量程 ±3g/±2000dps
* ========================================================================== */
#include "bmi088.h"
#include "main.h"
#include "spi.h"
#include "led.h"
/* ---- 寄存器(与 legacy 一致) ---- */
#define BMI088_ACC_CHIP_ID 0x00
#define BMI088_ACC_PWR_CTRL 0x7D
#define BMI088_ACC_PWR_CONF 0x7C
#define BMI088_ACC_CONF 0x40
#define BMI088_ACC_RANGE 0x41
#define BMI088_INT1_IO_CONF 0x53
#define BMI088_INT1_INT2_MAP_DATA 0x58
#define BMI088_ACC_SOFTRESET 0x7E
#define BMI088_ACC_DATA_START 0x12
#define BMI088_ACC_TEMP_MSB 0x22
#define BMI088_GYRO_CHIP_ID 0x00
#define BMI088_GYRO_RANGE 0x0F
#define BMI088_GYRO_BANDWIDTH 0x10
#define BMI088_GYRO_LPM1 0x11
#define BMI088_GYRO_INT_CTRL 0x15
#define BMI088_INT3_INT4_IO_CONF 0x16
#define BMI088_INT3_INT4_IO_MAP 0x18
#define BMI088_GYRO_SOFTRESET 0x14
#define BMI088_GYRO_DATA_START 0x02
#define BMI088_ACC_ID_VALUE 0x1E
#define BMI088_GYRO_ID_VALUE 0x0F
#define BMI088_CS_ACC(x) HAL_GPIO_WritePin(CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, (x) ? GPIO_PIN_SET : GPIO_PIN_RESET)
#define BMI088_CS_GYRO(x) HAL_GPIO_WritePin(CS1_GYRO_GPIO_Port, CS1_GYRO_Pin, (x) ? GPIO_PIN_SET : GPIO_PIN_RESET)
float bmi088_accel[3];
float bmi088_gyro[3];
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];
/* 读数阶段(当前 SPI DMA 正在搬运的数据类型) */
typedef enum
{
BMI088_PHASE_NONE = 0,
BMI088_PHASE_ACCEL,
BMI088_PHASE_TEMP,
BMI088_PHASE_GYRO,
} 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;
/* ---- 阻塞式寄存器读写 ---- */
static void accel_write_reg(uint8_t reg, uint8_t data)
{
uint8_t tx[2] = {(uint8_t)(reg & 0x7F), data};
BMI088_CS_ACC(0);
HAL_SPI_TransmitReceive(&hspi1, tx, spi_rx_buf, 2, 10);
BMI088_CS_ACC(1);
}
static uint8_t accel_read_reg(uint8_t reg)
{
uint8_t tx[3] = {(uint8_t)(reg | 0x80), 0x00, 0x00}; // 加速度计首字节回读为乱码
uint8_t rx[3];
BMI088_CS_ACC(0);
HAL_SPI_TransmitReceive(&hspi1, tx, rx, 3, 10);
BMI088_CS_ACC(1);
return rx[2];
}
static void gyro_write_reg(uint8_t reg, uint8_t data)
{
uint8_t tx[2] = {(uint8_t)(reg & 0x7F), data};
BMI088_CS_GYRO(0);
HAL_SPI_TransmitReceive(&hspi1, tx, spi_rx_buf, 2, 10);
BMI088_CS_GYRO(1);
}
static uint8_t gyro_read_reg(uint8_t reg)
{
uint8_t tx[2] = {(uint8_t)(reg | 0x80), 0x00};
uint8_t rx[2];
BMI088_CS_GYRO(0);
HAL_SPI_TransmitReceive(&hspi1, tx, rx, 2, 10);
BMI088_CS_GYRO(1);
return rx[1];
}
/* ---- 数据解析(换算系数与 legacy 一致) ---- */
static void process_accel(void)
{
int16_t raw[3];
raw[0] = (int16_t)(((uint16_t)spi_rx_buf[3] << 8) | spi_rx_buf[2]);
raw[1] = (int16_t)(((uint16_t)spi_rx_buf[5] << 8) | spi_rx_buf[4]);
raw[2] = (int16_t)(((uint16_t)spi_rx_buf[7] << 8) | spi_rx_buf[6]);
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)
{
uint16_t raw = ((uint16_t)spi_rx_buf[2] << 3) | (spi_rx_buf[3] >> 5); // 11bits
int16_t temp = (raw > 1023) ? (int16_t)(raw - 2048) : (int16_t)raw;
bmi088_temperature = temp * 0.125f + 23.0f;
}
static void process_gyro(void)
{
int16_t raw[3];
raw[0] = (int16_t)(((uint16_t)spi_rx_buf[2] << 8) | spi_rx_buf[1]);
raw[1] = (int16_t)(((uint16_t)spi_rx_buf[4] << 8) | spi_rx_buf[3]);
raw[2] = (int16_t)(((uint16_t)spi_rx_buf[6] << 8) | spi_rx_buf[5]);
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 忙则丢弃本次,等下一个数据就绪沿) */
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;
if (phase == BMI088_PHASE_ACCEL || phase == BMI088_PHASE_TEMP)
BMI088_CS_ACC(0);
else
BMI088_CS_GYRO(0);
/* 启动失败必须回滚:此时 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;
}
}
/* ---- 初始化 ---- */
static void error_blink(void)
{
/* BMI088 连接异常报警:绿灯连闪两下(经 TIM5 PWM,引脚在复用模式下
* 直接写 GPIO 寄存器无效,必须走 LED 模块) */
static uint8_t counter = 0;
counter++;
if (counter == 1)
led_set_green(1);
else if (counter == 5)
led_set_green(0);
else if (counter == 13)
led_set_green(1);
else if (counter == 17)
led_set_green(0);
else if (counter == 40)
counter = 0;
HAL_Delay(25);
}
void bmi088_init(void)
{
/* 关键:数据就绪中断(EXTI)在芯片上电后就会持续触发,若不屏蔽,
* EXTI 回调里的 SPI DMA 读会把 SPI 永远占在 BUSY,初始化的阻塞式
* 读 ID 全部失败(legacy 靠初始化期间关闭同款 NVIC 规避,此处一致) */
HAL_NVIC_DisableIRQ(EXTI4_IRQn);
HAL_NVIC_DisableIRQ(EXTI9_5_IRQn);
/* CubeMX 生成的 SPI1 为模式0/2分频(42MHz),BMI088 需要模式3 且 <=10MHz,
* 此处重新配置为模式3/16分频(5.25MHz,与 legacy 相同) */
HAL_SPI_DeInit(&hspi1);
hspi1.Init.CLKPolarity = SPI_POLARITY_HIGH;
hspi1.Init.CLKPhase = SPI_PHASE_2EDGE;
hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_16;
if (HAL_SPI_Init(&hspi1) != HAL_OK)
return; // 放弃本次初始化,保持系统可运行
BMI088_CS_ACC(1);
BMI088_CS_GYRO(1);
/* 检测 ID 并配置寄存器(与 legacy 序列一致)
* 有界重试:最多 40 次(约 1s),失败则放弃并保持系统可运行 */
for (uint8_t retry = 0; retry < 40; retry++)
{
bmi088_last_acc_id = accel_read_reg(BMI088_ACC_CHIP_ID);
bmi088_last_gyro_id = gyro_read_reg(BMI088_GYRO_CHIP_ID);
if (bmi088_last_acc_id == BMI088_ACC_ID_VALUE &&
bmi088_last_gyro_id == BMI088_GYRO_ID_VALUE)
{
bmi088_online = 1;
accel_write_reg(BMI088_ACC_SOFTRESET, 0xB6);
HAL_Delay(1); // 加速度计软重启 1ms
gyro_write_reg(BMI088_GYRO_SOFTRESET, 0xB6);
HAL_Delay(30); // 陀螺仪软重启 30ms
accel_read_reg(BMI088_ACC_CHIP_ID); // 软重启后首笔读写有 bug,丢弃
gyro_read_reg(BMI088_GYRO_CHIP_ID);
accel_write_reg(BMI088_ACC_PWR_CTRL, 0x04);
HAL_Delay(1); // 上电等待(legacy 300us,放宽到 1ms)
accel_write_reg(BMI088_ACC_PWR_CONF, 0x00);
HAL_Delay(1);
accel_write_reg(BMI088_ACC_CONF, 0xAB); // 800Hz ODR,正常低通
HAL_Delay(1);
accel_write_reg(BMI088_ACC_RANGE, 0x00); // ±3g
HAL_Delay(1);
accel_write_reg(BMI088_INT1_IO_CONF, 0x08); // INT1 推挽,低有效
HAL_Delay(1);
accel_write_reg(BMI088_INT1_INT2_MAP_DATA, 0x04); // INT1=数据就绪
HAL_Delay(1);
gyro_write_reg(BMI088_GYRO_RANGE, 0x00); // ±2000°/s
HAL_Delay(1);
gyro_write_reg(BMI088_GYRO_BANDWIDTH, 0x82); // 116Hz 带宽,1kHz ODR
HAL_Delay(1);
gyro_write_reg(BMI088_GYRO_LPM1, 0x00); // 正常模式
HAL_Delay(1);
gyro_write_reg(BMI088_GYRO_INT_CTRL, 0x80); // 使能数据就绪中断
HAL_Delay(1);
gyro_write_reg(BMI088_INT3_INT4_IO_CONF, 0x00); // INT3 推挽,低有效
HAL_Delay(1);
gyro_write_reg(BMI088_INT3_INT4_IO_MAP, 0x01); // INT3=数据就绪
HAL_Delay(1);
break;
}
error_blink();
}
/* 配置成功后清除挂起的数据就绪标志,再放行 EXTI 中断 */
if (bmi088_online)
{
__HAL_GPIO_EXTI_CLEAR_IT(INT1_ACCEL_Pin);
__HAL_GPIO_EXTI_CLEAR_IT(INT1_GYRO_Pin);
HAL_NVIC_EnableIRQ(EXTI4_IRQn);
HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
}
}
/* ---- 中断回调 ---- */
/* EXTI4(加速度计数据就绪)/ EXTI5(陀螺仪数据就绪) */
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)
start_dma_read(BMI088_PHASE_ACCEL, BMI088_ACC_DATA_START, 8);
}
else if (GPIO_Pin == INT1_GYRO_Pin)
{
if (read_phase == BMI088_PHASE_NONE)
start_dma_read(BMI088_PHASE_GYRO, BMI088_GYRO_DATA_START, 7);
}
}
/* SPI 收发完成 */
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef * hspi)
{
if (hspi->Instance != SPI1)
return;
switch (read_phase)
{
case BMI088_PHASE_ACCEL:
BMI088_CS_ACC(1);
process_accel();
read_phase = BMI088_PHASE_NONE;
start_dma_read(BMI088_PHASE_TEMP, BMI088_ACC_TEMP_MSB, 4); // 链式读取温度
break;
case BMI088_PHASE_TEMP:
BMI088_CS_ACC(1);
process_temperature();
read_phase = BMI088_PHASE_NONE;
break;
case BMI088_PHASE_GYRO:
BMI088_CS_GYRO(1);
process_gyro();
read_phase = BMI088_PHASE_NONE;
break;
default:
break;
}
}
Reference in New Issue View Git Blame Copy Permalink