fix(bmi088): 数据路停摆的自恢复(任务上下文健康检查)
现场遥测(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,互不重叠。
两个工程同步修改,内容一致。
This commit is contained in:
@@ -22,6 +22,8 @@
|
||||
* 而 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 去掉诊断位。
|
||||
* ========================================================================== */
|
||||
@@ -61,10 +63,11 @@ void debug_task(void const * argument)
|
||||
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",
|
||||
" 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_accel_cnt, (unsigned long)bmi088_gyro_cnt,
|
||||
(unsigned long)bmi088_spi_errors, (unsigned)bmi088_spi_state());
|
||||
#endif
|
||||
|
||||
#if DEBUG_ATT_HEALTH
|
||||
|
||||
@@ -337,3 +337,136 @@ void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef * hspi)
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* ==========================================================================
|
||||
* 数据路健康检查与自恢复(任务上下文,由 attitude_update() 每 1ms 调用)
|
||||
* --------------------------------------------------------------------------
|
||||
* 背景(2026-09-12 现场):USB 遥测里 ca/cg 采样计数在开机约 1s 后彻底停止
|
||||
* 增长,而 imu=1、an/gn 看起来都正常 —— 数据冻结在最后一个样本上,姿态
|
||||
* 于是表现出"yaw 以恒定角速度自漂、pitch/roll 不动、陀螺不随板子移动"。
|
||||
*
|
||||
* 为什么必须有这个函数: 中断上下文里的兜底救不了这两类停摆——
|
||||
* a) hspi1 被卡在 BUSY(某次 SPI/DMA 出错后 HAL 没回到 READY),
|
||||
* start_dma_read() 的 HAL_SPI_GetState 检查会让每次都直接返回;
|
||||
* b) BMI088 的 DRDY 是电平型信号,一旦某次失败/被挡掉的读取没把
|
||||
* 传感器的数据寄存器读走,INT 电平就保持住,之后既没有下降沿也没有
|
||||
* 上升沿 —— EXTI 再也不触发,写在 EXTI 回调里的任何兜底都不会执行。
|
||||
* 所以检查点放在任务里:停摆时先重启中断路(必要时 abort/重建 SPI),再用
|
||||
* 阻塞读保证本拍拿到真实数据。若中断路始终修不好,就退化成"每
|
||||
* BMI088_STALL_MS 补读一次"的轮询保底(约 200Hz),姿态不至于因为数据
|
||||
* 冻结而彻底失效;此时积分总量仍正确,只是运动解析粒度变粗。
|
||||
* 注意: dt 固定 1ms、1kHz 调用,所以即使采样只有 200Hz,一秒内积分出来
|
||||
* 的总转角依然是对的(保持值 × 0.001 × 1000 次 = 保持值)。
|
||||
* ========================================================================== */
|
||||
#define BMI088_STALL_MS 5U /* 陀螺 1kHz 输出,5ms 没新样本即可判停摆 */
|
||||
|
||||
volatile uint32_t bmi088_spi_errors = 0;
|
||||
|
||||
/* SPI 当前状态(HAL_SPI_StateTypeDef):1=READY,非 1 即被卡住,诊断用 */
|
||||
uint8_t bmi088_spi_state(void)
|
||||
{
|
||||
return (uint8_t)HAL_SPI_GetState(&hspi1);
|
||||
}
|
||||
|
||||
/* 阻塞式读一次加速度计与陀螺仪数据(仅恢复路径使用:此时 EXTI 已关,不会
|
||||
* 与 DMA 抢 spi_rx_buf)。解析逻辑复用 process_accel/process_gyro */
|
||||
static void bmi088_read_blocking(void)
|
||||
{
|
||||
uint8_t tx[8];
|
||||
uint8_t rx[8];
|
||||
|
||||
/* 加速度计:8 字节,首字节乱码,数据在 buf[2..7] */
|
||||
tx[0] = (uint8_t)(BMI088_ACC_DATA_START | 0x80);
|
||||
for (uint8_t i = 1; i < 8; i++)
|
||||
tx[i] = 0x00;
|
||||
BMI088_CS_ACC(0);
|
||||
HAL_SPI_TransmitReceive(&hspi1, tx, rx, 8, 10);
|
||||
BMI088_CS_ACC(1);
|
||||
for (uint8_t i = 0; i < 8; i++)
|
||||
spi_rx_buf[i] = rx[i];
|
||||
process_accel();
|
||||
|
||||
/* 陀螺仪:7 字节,数据在 buf[1..6] */
|
||||
tx[0] = (uint8_t)(BMI088_GYRO_DATA_START | 0x80);
|
||||
for (uint8_t i = 1; i < 7; i++)
|
||||
tx[i] = 0x00;
|
||||
BMI088_CS_GYRO(0);
|
||||
HAL_SPI_TransmitReceive(&hspi1, tx, rx, 7, 10);
|
||||
BMI088_CS_GYRO(1);
|
||||
for (uint8_t i = 0; i < 7; i++)
|
||||
spi_rx_buf[i] = rx[i];
|
||||
process_gyro();
|
||||
}
|
||||
|
||||
/* 重启中断路:复位相位/片选 -> 必要时重建 SPI -> 清 EXTI 挂起 -> 阻塞读一次 */
|
||||
static void bmi088_recover(void)
|
||||
{
|
||||
HAL_NVIC_DisableIRQ(EXTI4_IRQn);
|
||||
HAL_NVIC_DisableIRQ(EXTI9_5_IRQn);
|
||||
|
||||
cs_release(read_phase);
|
||||
read_phase = BMI088_PHASE_NONE;
|
||||
|
||||
/* SPI 可能卡在 BUSY:先 abort,仍不 READY 就重建(注意 hspi1.Init 里
|
||||
* 仍是 bmi088_init 设好的模式3/16 分频,重建不会把时序改回去) */
|
||||
if (HAL_SPI_GetState(&hspi1) != HAL_SPI_STATE_READY)
|
||||
(void)HAL_SPI_Abort(&hspi1);
|
||||
if (HAL_SPI_GetState(&hspi1) != HAL_SPI_STATE_READY)
|
||||
{
|
||||
HAL_SPI_DeInit(&hspi1);
|
||||
(void)HAL_SPI_Init(&hspi1);
|
||||
}
|
||||
|
||||
/* 清掉可能挂起的 EXTI 标志,否则旧电平会把下一次中断吞掉 */
|
||||
__HAL_GPIO_EXTI_CLEAR_IT(INT1_ACCEL_Pin);
|
||||
__HAL_GPIO_EXTI_CLEAR_IT(INT1_GYRO_Pin);
|
||||
|
||||
bmi088_read_blocking(); /* 保证本拍有真实数据,同时清传感器 DRDY 电平 */
|
||||
|
||||
HAL_NVIC_EnableIRQ(EXTI4_IRQn);
|
||||
HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
|
||||
}
|
||||
|
||||
/* 每 1ms 调用:采样计数没涨就说明中断路停了,恢复之 */
|
||||
void bmi088_health_check(void)
|
||||
{
|
||||
static uint32_t last_cnt = 0;
|
||||
static uint32_t stall_since = 0;
|
||||
const uint32_t now = HAL_GetTick();
|
||||
|
||||
if (!bmi088_online)
|
||||
return;
|
||||
|
||||
if (bmi088_gyro_cnt != last_cnt)
|
||||
{
|
||||
/* 正常:中断路在出新样本 */
|
||||
last_cnt = bmi088_gyro_cnt;
|
||||
stall_since = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
if (stall_since == 0)
|
||||
{
|
||||
stall_since = now;
|
||||
return;
|
||||
}
|
||||
if (now - stall_since < BMI088_STALL_MS)
|
||||
return;
|
||||
|
||||
/* 停摆:重启中断路 + 阻塞读补一拍 */
|
||||
bmi088_recover();
|
||||
last_cnt = bmi088_gyro_cnt;
|
||||
stall_since = now;
|
||||
}
|
||||
|
||||
/* SPI/DMA 出错回调:此前完全没有实现,出错后 SPI 状态可能停在 BUSY 而无人
|
||||
* 清理,数据路就此永久静止。这里至少把相位与片选复位,并计数以便遥测观察 */
|
||||
void HAL_SPI_ErrorCallback(SPI_HandleTypeDef * hspi)
|
||||
{
|
||||
if (hspi->Instance != SPI1)
|
||||
return;
|
||||
|
||||
cs_release(read_phase);
|
||||
read_phase = BMI088_PHASE_NONE;
|
||||
bmi088_spi_errors++;
|
||||
}
|
||||
|
||||
@@ -21,6 +21,16 @@ extern uint8_t bmi088_last_gyro_id; // 最近一次读到的陀螺仪 ID(诊断
|
||||
extern volatile uint32_t bmi088_accel_cnt;
|
||||
extern volatile uint32_t bmi088_gyro_cnt;
|
||||
|
||||
/* SPI 错误计数 / 当前 SPI 状态(诊断用):状态非 1 即 SPI 被卡住 */
|
||||
extern volatile uint32_t bmi088_spi_errors;
|
||||
extern uint8_t bmi088_spi_state(void);
|
||||
|
||||
/* 数据路健康检查(任务上下文):由 attitude_update() 每 1ms 调用,采样计数
|
||||
* 超过 20ms 没增长就判定停摆,重启中断路并阻塞读一次补数据。**必须在任务
|
||||
* 里调用**——停摆的两类原因(SPI 卡 BUSY、DRDY 电平锁死导致 EXTI 再不触发)
|
||||
* 都会让写在中断回调里的兜底压根不执行 */
|
||||
extern void bmi088_health_check(void);
|
||||
|
||||
/* 初始化(任务上下文调用,含阻塞式寄存器配置与 ID 检测循环;失败时绿灯报警并重试) */
|
||||
extern void bmi088_init(void);
|
||||
|
||||
|
||||
@@ -34,6 +34,10 @@ void attitude_init(void)
|
||||
|
||||
void attitude_update(void)
|
||||
{
|
||||
/* 先做数据路健康检查:IMU 数据一旦冻结,姿态会安静地"保持不动"甚至
|
||||
* 匀速漂移(见 bmi088.c 的 bmi088_health_check 说明) */
|
||||
bmi088_health_check();
|
||||
|
||||
ins_update(0.001f); // 1kHz 固定节拍
|
||||
|
||||
att_deg_yaw = INS.YawTotalAngle; // 多圈连续角,云台 Yaw 环直接使用
|
||||
|
||||
@@ -22,6 +22,8 @@
|
||||
* 而 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 去掉诊断位。
|
||||
* ========================================================================== */
|
||||
@@ -61,10 +63,11 @@ void debug_task(void const * argument)
|
||||
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",
|
||||
" 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_accel_cnt, (unsigned long)bmi088_gyro_cnt,
|
||||
(unsigned long)bmi088_spi_errors, (unsigned)bmi088_spi_state());
|
||||
#endif
|
||||
|
||||
#if DEBUG_ATT_HEALTH
|
||||
|
||||
@@ -337,3 +337,136 @@ void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef * hspi)
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* ==========================================================================
|
||||
* 数据路健康检查与自恢复(任务上下文,由 attitude_update() 每 1ms 调用)
|
||||
* --------------------------------------------------------------------------
|
||||
* 背景(2026-09-12 现场):USB 遥测里 ca/cg 采样计数在开机约 1s 后彻底停止
|
||||
* 增长,而 imu=1、an/gn 看起来都正常 —— 数据冻结在最后一个样本上,姿态
|
||||
* 于是表现出"yaw 以恒定角速度自漂、pitch/roll 不动、陀螺不随板子移动"。
|
||||
*
|
||||
* 为什么必须有这个函数: 中断上下文里的兜底救不了这两类停摆——
|
||||
* a) hspi1 被卡在 BUSY(某次 SPI/DMA 出错后 HAL 没回到 READY),
|
||||
* start_dma_read() 的 HAL_SPI_GetState 检查会让每次都直接返回;
|
||||
* b) BMI088 的 DRDY 是电平型信号,一旦某次失败/被挡掉的读取没把
|
||||
* 传感器的数据寄存器读走,INT 电平就保持住,之后既没有下降沿也没有
|
||||
* 上升沿 —— EXTI 再也不触发,写在 EXTI 回调里的任何兜底都不会执行。
|
||||
* 所以检查点放在任务里:停摆时先重启中断路(必要时 abort/重建 SPI),再用
|
||||
* 阻塞读保证本拍拿到真实数据。若中断路始终修不好,就退化成"每
|
||||
* BMI088_STALL_MS 补读一次"的轮询保底(约 200Hz),姿态不至于因为数据
|
||||
* 冻结而彻底失效;此时积分总量仍正确,只是运动解析粒度变粗。
|
||||
* 注意: dt 固定 1ms、1kHz 调用,所以即使采样只有 200Hz,一秒内积分出来
|
||||
* 的总转角依然是对的(保持值 × 0.001 × 1000 次 = 保持值)。
|
||||
* ========================================================================== */
|
||||
#define BMI088_STALL_MS 5U /* 陀螺 1kHz 输出,5ms 没新样本即可判停摆 */
|
||||
|
||||
volatile uint32_t bmi088_spi_errors = 0;
|
||||
|
||||
/* SPI 当前状态(HAL_SPI_StateTypeDef):1=READY,非 1 即被卡住,诊断用 */
|
||||
uint8_t bmi088_spi_state(void)
|
||||
{
|
||||
return (uint8_t)HAL_SPI_GetState(&hspi1);
|
||||
}
|
||||
|
||||
/* 阻塞式读一次加速度计与陀螺仪数据(仅恢复路径使用:此时 EXTI 已关,不会
|
||||
* 与 DMA 抢 spi_rx_buf)。解析逻辑复用 process_accel/process_gyro */
|
||||
static void bmi088_read_blocking(void)
|
||||
{
|
||||
uint8_t tx[8];
|
||||
uint8_t rx[8];
|
||||
|
||||
/* 加速度计:8 字节,首字节乱码,数据在 buf[2..7] */
|
||||
tx[0] = (uint8_t)(BMI088_ACC_DATA_START | 0x80);
|
||||
for (uint8_t i = 1; i < 8; i++)
|
||||
tx[i] = 0x00;
|
||||
BMI088_CS_ACC(0);
|
||||
HAL_SPI_TransmitReceive(&hspi1, tx, rx, 8, 10);
|
||||
BMI088_CS_ACC(1);
|
||||
for (uint8_t i = 0; i < 8; i++)
|
||||
spi_rx_buf[i] = rx[i];
|
||||
process_accel();
|
||||
|
||||
/* 陀螺仪:7 字节,数据在 buf[1..6] */
|
||||
tx[0] = (uint8_t)(BMI088_GYRO_DATA_START | 0x80);
|
||||
for (uint8_t i = 1; i < 7; i++)
|
||||
tx[i] = 0x00;
|
||||
BMI088_CS_GYRO(0);
|
||||
HAL_SPI_TransmitReceive(&hspi1, tx, rx, 7, 10);
|
||||
BMI088_CS_GYRO(1);
|
||||
for (uint8_t i = 0; i < 7; i++)
|
||||
spi_rx_buf[i] = rx[i];
|
||||
process_gyro();
|
||||
}
|
||||
|
||||
/* 重启中断路:复位相位/片选 -> 必要时重建 SPI -> 清 EXTI 挂起 -> 阻塞读一次 */
|
||||
static void bmi088_recover(void)
|
||||
{
|
||||
HAL_NVIC_DisableIRQ(EXTI4_IRQn);
|
||||
HAL_NVIC_DisableIRQ(EXTI9_5_IRQn);
|
||||
|
||||
cs_release(read_phase);
|
||||
read_phase = BMI088_PHASE_NONE;
|
||||
|
||||
/* SPI 可能卡在 BUSY:先 abort,仍不 READY 就重建(注意 hspi1.Init 里
|
||||
* 仍是 bmi088_init 设好的模式3/16 分频,重建不会把时序改回去) */
|
||||
if (HAL_SPI_GetState(&hspi1) != HAL_SPI_STATE_READY)
|
||||
(void)HAL_SPI_Abort(&hspi1);
|
||||
if (HAL_SPI_GetState(&hspi1) != HAL_SPI_STATE_READY)
|
||||
{
|
||||
HAL_SPI_DeInit(&hspi1);
|
||||
(void)HAL_SPI_Init(&hspi1);
|
||||
}
|
||||
|
||||
/* 清掉可能挂起的 EXTI 标志,否则旧电平会把下一次中断吞掉 */
|
||||
__HAL_GPIO_EXTI_CLEAR_IT(INT1_ACCEL_Pin);
|
||||
__HAL_GPIO_EXTI_CLEAR_IT(INT1_GYRO_Pin);
|
||||
|
||||
bmi088_read_blocking(); /* 保证本拍有真实数据,同时清传感器 DRDY 电平 */
|
||||
|
||||
HAL_NVIC_EnableIRQ(EXTI4_IRQn);
|
||||
HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
|
||||
}
|
||||
|
||||
/* 每 1ms 调用:采样计数没涨就说明中断路停了,恢复之 */
|
||||
void bmi088_health_check(void)
|
||||
{
|
||||
static uint32_t last_cnt = 0;
|
||||
static uint32_t stall_since = 0;
|
||||
const uint32_t now = HAL_GetTick();
|
||||
|
||||
if (!bmi088_online)
|
||||
return;
|
||||
|
||||
if (bmi088_gyro_cnt != last_cnt)
|
||||
{
|
||||
/* 正常:中断路在出新样本 */
|
||||
last_cnt = bmi088_gyro_cnt;
|
||||
stall_since = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
if (stall_since == 0)
|
||||
{
|
||||
stall_since = now;
|
||||
return;
|
||||
}
|
||||
if (now - stall_since < BMI088_STALL_MS)
|
||||
return;
|
||||
|
||||
/* 停摆:重启中断路 + 阻塞读补一拍 */
|
||||
bmi088_recover();
|
||||
last_cnt = bmi088_gyro_cnt;
|
||||
stall_since = now;
|
||||
}
|
||||
|
||||
/* SPI/DMA 出错回调:此前完全没有实现,出错后 SPI 状态可能停在 BUSY 而无人
|
||||
* 清理,数据路就此永久静止。这里至少把相位与片选复位,并计数以便遥测观察 */
|
||||
void HAL_SPI_ErrorCallback(SPI_HandleTypeDef * hspi)
|
||||
{
|
||||
if (hspi->Instance != SPI1)
|
||||
return;
|
||||
|
||||
cs_release(read_phase);
|
||||
read_phase = BMI088_PHASE_NONE;
|
||||
bmi088_spi_errors++;
|
||||
}
|
||||
|
||||
@@ -21,6 +21,16 @@ extern uint8_t bmi088_last_gyro_id; // 最近一次读到的陀螺仪 ID(诊断
|
||||
extern volatile uint32_t bmi088_accel_cnt;
|
||||
extern volatile uint32_t bmi088_gyro_cnt;
|
||||
|
||||
/* SPI 错误计数 / 当前 SPI 状态(诊断用):状态非 1 即 SPI 被卡住 */
|
||||
extern volatile uint32_t bmi088_spi_errors;
|
||||
extern uint8_t bmi088_spi_state(void);
|
||||
|
||||
/* 数据路健康检查(任务上下文):由 attitude_update() 每 1ms 调用,采样计数
|
||||
* 超过 20ms 没增长就判定停摆,重启中断路并阻塞读一次补数据。**必须在任务
|
||||
* 里调用**——停摆的两类原因(SPI 卡 BUSY、DRDY 电平锁死导致 EXTI 再不触发)
|
||||
* 都会让写在中断回调里的兜底压根不执行 */
|
||||
extern void bmi088_health_check(void);
|
||||
|
||||
/* 初始化(任务上下文调用,含阻塞式寄存器配置与 ID 检测循环;失败时绿灯报警并重试) */
|
||||
extern void bmi088_init(void);
|
||||
|
||||
|
||||
@@ -34,6 +34,10 @@ void attitude_init(void)
|
||||
|
||||
void attitude_update(void)
|
||||
{
|
||||
/* 先做数据路健康检查:IMU 数据一旦冻结,姿态会安静地"保持不动"甚至
|
||||
* 匀速漂移(见 bmi088.c 的 bmi088_health_check 说明) */
|
||||
bmi088_health_check();
|
||||
|
||||
ins_update(0.001f); // 1kHz 固定节拍
|
||||
|
||||
att_deg_yaw = INS.YawTotalAngle; // 多圈连续角,云台 Yaw 环直接使用
|
||||
|
||||
Reference in New Issue
Block a user