diff --git a/云台/Application/Task/Src/debug_task.c b/云台/Application/Task/Src/debug_task.c index c85e56d..2112b4e 100644 --- a/云台/Application/Task/Src/debug_task.c +++ b/云台/Application/Task/Src/debug_task.c @@ -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 diff --git a/云台/modules/BMI088/bmi088.c b/云台/modules/BMI088/bmi088.c index 8dd7deb..69265ca 100644 --- a/云台/modules/BMI088/bmi088.c +++ b/云台/modules/BMI088/bmi088.c @@ -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++; +} diff --git a/云台/modules/BMI088/bmi088.h b/云台/modules/BMI088/bmi088.h index 20aceb4..1d5f43f 100644 --- a/云台/modules/BMI088/bmi088.h +++ b/云台/modules/BMI088/bmi088.h @@ -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); diff --git a/云台/modules/attitude/attitude.c b/云台/modules/attitude/attitude.c index 3075b4e..f715412 100644 --- a/云台/modules/attitude/attitude.c +++ b/云台/modules/attitude/attitude.c @@ -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 环直接使用 diff --git a/底盘/Application/Task/Src/debug_task.c b/底盘/Application/Task/Src/debug_task.c index c85e56d..2112b4e 100644 --- a/底盘/Application/Task/Src/debug_task.c +++ b/底盘/Application/Task/Src/debug_task.c @@ -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 diff --git a/底盘/modules/BMI088/bmi088.c b/底盘/modules/BMI088/bmi088.c index 8dd7deb..69265ca 100644 --- a/底盘/modules/BMI088/bmi088.c +++ b/底盘/modules/BMI088/bmi088.c @@ -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++; +} diff --git a/底盘/modules/BMI088/bmi088.h b/底盘/modules/BMI088/bmi088.h index 20aceb4..1d5f43f 100644 --- a/底盘/modules/BMI088/bmi088.h +++ b/底盘/modules/BMI088/bmi088.h @@ -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); diff --git a/底盘/modules/attitude/attitude.c b/底盘/modules/attitude/attitude.c index 3075b4e..f715412 100644 --- a/底盘/modules/attitude/attitude.c +++ b/底盘/modules/attitude/attitude.c @@ -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 环直接使用