From 9fc15756159be46bd16d83bc0b9e887dafd865dd Mon Sep 17 00:00:00 2001 From: LSMushui Date: Sat, 12 Sep 2026 11:27:42 +0800 Subject: [PATCH] =?UTF-8?q?fix(ins/bmi088):=20=E4=BF=AE=E5=A4=8D=20yaw=20?= =?UTF-8?q?=E5=8C=80=E9=80=9F=E8=87=AA=E6=BC=82(=E9=9B=B6=E5=81=8F?= =?UTF-8?q?=E6=A0=87=E5=AE=9A=E8=A2=AB=E6=B1=A1=E6=9F=93)=E4=B8=8E=20IMU?= =?UTF-8?q?=20=E6=95=B0=E6=8D=AE=E5=86=BB=E7=BB=93=E9=9A=90=E6=82=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 现场数据(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 去掉这些排障字段)。 两个工程同步修改,内容一致。 --- 云台/Application/Task/Src/debug_task.c | 53 +++++++++++++-------- 云台/Core/Src/gpio.c | 12 ++++- 云台/Simba-ELCT-FrameWork.ioc | 6 ++- 云台/modules/BMI088/bmi088.c | 36 ++++++++++++++- 云台/modules/BMI088/bmi088.h | 6 +++ 云台/modules/ins/ins.c | 64 ++++++++++++++++++++++---- 云台/modules/ins/ins.h | 7 +++ 底盘/Application/Task/Src/debug_task.c | 53 +++++++++++++-------- 底盘/Core/Src/gpio.c | 12 ++++- 底盘/Simba-ELCT-FrameWork.ioc | 6 ++- 底盘/modules/BMI088/bmi088.c | 36 ++++++++++++++- 底盘/modules/BMI088/bmi088.h | 6 +++ 底盘/modules/ins/ins.c | 64 ++++++++++++++++++++++---- 底盘/modules/ins/ins.h | 7 +++ 14 files changed, 302 insertions(+), 66 deletions(-) diff --git a/云台/Application/Task/Src/debug_task.c b/云台/Application/Task/Src/debug_task.c index 0d04872..c85e56d 100644 --- a/云台/Application/Task/Src/debug_task.c +++ b/云台/Application/Task/Src/debug_task.c @@ -1,5 +1,5 @@ /* ============================================================================= - * debug_task.c - USB 虚拟串口(CDC)姿态输出(欧拉角 + 姿态链路健康位) + * debug_task.c - USB 虚拟串口(CDC)姿态输出(欧拉角 + 姿态链路诊断) * 每 DEBUG_OUT_PERIOD_MS 输出一行。单位: 角度 ° / 加速度 m/s² / 角速度 rad/s * * ---- 欧拉角(QuaternionEKF 解算结果,见 modules/ins) ---- @@ -7,19 +7,23 @@ * pitch 绕 y 轴角 (att_deg_pitch = QEKF_INS.Roll ) * roll 绕 x 轴角 (att_deg_roll = QEKF_INS.Pitch,云台 Pitch 环反馈) * QEKF 内部 Pitch/Roll 命名与航空惯例互换,映射与原因见 attitude.c 文件头。 - * 注意 yaw 没有绝对基准(纯陀螺积分),上电静止时恒为该次上电的相对 0 点, - * 因此"yaw=0.00"本身不是故障特征;判断故障要看下面的健康位。 + * 注意 yaw 无绝对基准(纯陀螺积分),上电静止时恒为该次上电的相对 0 点。 * - * ---- 健康位(排查"角度不动"时必须与角度一起看) ---- - * cvg 1-INS.ConvergeFlag:EKF 卡方检验通过、姿态可信(原诊断输出的 cvg - * 就是它,不要改成别的量,否则会丢掉"是否真的收敛"这一唯一判据); - * 0-未收敛/发散,此时角度不可信 + * ---- 健康位 / 诊断位(排查"角度不动或自漂"时必须一起看) ---- + * cvg 1-INS.ConvergeFlag:EKF 卡方检验通过、姿态可信(与原诊断输出同义, + * 不要改成别的量,否则会丢掉"是否真的收敛"这一唯一判据) * rdy 1-att_ready:ins_update() 至少执行过一次,只说明"在跑",不代表可信 * imu 1-bmi088_online:IMU 初始化成功;0=ID 检测失败,姿态必然全 0 * an |bmi088_accel|:静止时≈9.8;≈0 说明 IMU 根本没给出数据 - * gn |bmi088_gyro|:静止时≈0;转动时明显变大说明陀螺通路正常 + * gn |bmi088_gyro|:原始陀螺模长(**未减零偏**),静止时≈0 + * gx/gy/gz 原始陀螺三分量(rad/s) —— 与 gn 一起可判断数据是否冻结 + * zo ins_gyro_z_offset:上电静止标定得到的 z 零偏(rad/s)。**关键**: + * 实际进入滤波器的 gyro_z = gz - zo。若 zo 与 gz 量级相近甚至更大, + * 而 yaw 又匀速自漂,即标定窗口内车体在动把零偏标歪了 + * ck ins_z_cal_ok:1-标定时全程静止,zo 可信;0-三轮都在动,zo 存疑 + * ca/cg 加速度/陀螺采样计数:停止增长即数据路已死(数值冻结时它们不涨) * - * 不需要健康位时把 DEBUG_ATT_HEALTH 置 0,即只输出三个欧拉角。 + * 开关: DEBUG_ATT_HEALTH=0 只输出三个欧拉角;DEBUG_ATT_DIAG=0 去掉诊断位。 * ========================================================================== */ #include "debug_task.h" #include "cmsis_os.h" @@ -32,15 +36,18 @@ #include #include -/* 1-输出欧拉角 + 姿态链路健康位; 0-只输出欧拉角 */ +/* 1-输出健康位(cvg/rdy/imu/an/gn); 0-只输出三个欧拉角 */ #define DEBUG_ATT_HEALTH 1 +/* 1-再附加原始陀螺/零偏/采样计数(排障用); 0-只留上面的通用健康位 */ +#define DEBUG_ATT_DIAG 1 /* 输出周期(ms)。姿态观测建议 100ms(10Hz) */ #define DEBUG_OUT_PERIOD_MS 100 void debug_task(void const * argument) { - char line[128]; + char line[192]; + char tail[112]; for (;;) { @@ -51,16 +58,24 @@ void debug_task(void const * argument) bmi088_gyro[1] * bmi088_gyro[1] + bmi088_gyro[2] * bmi088_gyro[2]); - int len; + 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", + 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); +#endif + #if DEBUG_ATT_HEALTH - len = snprintf(line, sizeof(line), - "yaw=%.2f pitch=%.2f roll=%.2f cvg=%u rdy=%u imu=%u an=%.2f gn=%.3f\r\n", - att_deg_yaw, att_deg_pitch, att_deg_roll, - (unsigned)INS.ConvergeFlag, (unsigned)att_ready, - (unsigned)bmi088_online, an, gn); + int len = snprintf(line, sizeof(line), + "yaw=%.2f pitch=%.2f roll=%.2f cvg=%u rdy=%u imu=%u an=%.2f gn=%.3f%s\r\n", + att_deg_yaw, att_deg_pitch, att_deg_roll, + (unsigned)INS.ConvergeFlag, (unsigned)att_ready, + (unsigned)bmi088_online, an, gn, tail); #else - len = snprintf(line, sizeof(line), "yaw=%.2f pitch=%.2f roll=%.2f\r\n", - att_deg_yaw, att_deg_pitch, att_deg_roll); + int len = snprintf(line, sizeof(line), "yaw=%.2f pitch=%.2f roll=%.2f\r\n", + att_deg_yaw, att_deg_pitch, att_deg_roll); #endif if (len > 0) diff --git a/云台/Core/Src/gpio.c b/云台/Core/Src/gpio.c index 7850132..00a15e4 100644 --- a/云台/Core/Src/gpio.c +++ b/云台/Core/Src/gpio.c @@ -102,9 +102,17 @@ void MX_GPIO_Init(void) GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; HAL_GPIO_Init(CS1_ACCEL_GPIO_Port, &GPIO_InitStruct); - /*Configure GPIO pins : INT1_ACCEL_Pin INT1_GYRO_Pin */ + /*Configure GPIO pins : INT1_ACCEL_Pin INT1_GYRO_Pin + * BMI088 的 INT1(加速度计)/INT3(陀螺仪)在 bmi088_init 里被配成推挽、 + * 低有效的数据就绪信号(INT1_IO_CONF=0x08 / INT3_INT4_IO_CONF=0x00), + * 即"数据就绪"表现为电平拉低,对应下降沿。bmi088.c/h 文件头也写明 + * "数据就绪下降沿触发",legacy 用的同样是 EXTI_Trigger_Falling。 + * CubeMX 默认生成的是上升沿,那会落在这个信号的电平恢复沿上,采样时序 + * 整体错位 —— 故此处改回下降沿。 + * 注意: 本文件由 CubeMX 生成,重新生成工程会改回 RISING;.ioc 里已同步 + * 写入 GPIO_MODE_IT_FALLING,正常用 CubeMX 重新生成可保留。 */ GPIO_InitStruct.Pin = INT1_ACCEL_Pin|INT1_GYRO_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; + GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING; GPIO_InitStruct.Pull = GPIO_PULLUP; HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); diff --git a/云台/Simba-ELCT-FrameWork.ioc b/云台/Simba-ELCT-FrameWork.ioc index b1be49e..44873a1 100644 --- a/云台/Simba-ELCT-FrameWork.ioc +++ b/云台/Simba-ELCT-FrameWork.ioc @@ -361,12 +361,14 @@ PC2.GPIOParameters=GPIO_Label PC2.GPIO_Label=HW2 PC2.Locked=true PC2.Signal=GPIO_Input -PC4.GPIOParameters=GPIO_PuPd,GPIO_Label +PC4.GPIOParameters=GPIO_PuPd,GPIO_Label,GPIO_ModeDefaultEXTI +PC4.GPIO_ModeDefaultEXTI=GPIO_MODE_IT_FALLING PC4.GPIO_Label=INT1_ACCEL PC4.GPIO_PuPd=GPIO_PULLUP PC4.Locked=true PC4.Signal=GPXTI4 -PC5.GPIOParameters=GPIO_PuPd,GPIO_Label +PC5.GPIOParameters=GPIO_PuPd,GPIO_Label,GPIO_ModeDefaultEXTI +PC5.GPIO_ModeDefaultEXTI=GPIO_MODE_IT_FALLING PC5.GPIO_Label=INT1_GYRO PC5.GPIO_PuPd=GPIO_PULLUP PC5.Locked=true diff --git a/云台/modules/BMI088/bmi088.c b/云台/modules/BMI088/bmi088.c index f45a197..8dd7deb 100644 --- a/云台/modules/BMI088/bmi088.c +++ b/云台/modules/BMI088/bmi088.c @@ -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) diff --git a/云台/modules/BMI088/bmi088.h b/云台/modules/BMI088/bmi088.h index 22cd599..20aceb4 100644 --- a/云台/modules/BMI088/bmi088.h +++ b/云台/modules/BMI088/bmi088.h @@ -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); diff --git a/云台/modules/ins/ins.c b/云台/modules/ins/ins.c index 93c25f6..77b823d 100644 --- a/云台/modules/ins/ins.c +++ b/云台/modules/ins/ins.c @@ -16,11 +16,33 @@ #include "main.h" #include -/* 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; diff --git a/云台/modules/ins/ins.h b/云台/modules/ins/ins.h index a35be87..fe87a8e 100644 --- a/云台/modules/ins/ins.h +++ b/云台/modules/ins/ins.h @@ -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); diff --git a/底盘/Application/Task/Src/debug_task.c b/底盘/Application/Task/Src/debug_task.c index 0d04872..c85e56d 100644 --- a/底盘/Application/Task/Src/debug_task.c +++ b/底盘/Application/Task/Src/debug_task.c @@ -1,5 +1,5 @@ /* ============================================================================= - * debug_task.c - USB 虚拟串口(CDC)姿态输出(欧拉角 + 姿态链路健康位) + * debug_task.c - USB 虚拟串口(CDC)姿态输出(欧拉角 + 姿态链路诊断) * 每 DEBUG_OUT_PERIOD_MS 输出一行。单位: 角度 ° / 加速度 m/s² / 角速度 rad/s * * ---- 欧拉角(QuaternionEKF 解算结果,见 modules/ins) ---- @@ -7,19 +7,23 @@ * pitch 绕 y 轴角 (att_deg_pitch = QEKF_INS.Roll ) * roll 绕 x 轴角 (att_deg_roll = QEKF_INS.Pitch,云台 Pitch 环反馈) * QEKF 内部 Pitch/Roll 命名与航空惯例互换,映射与原因见 attitude.c 文件头。 - * 注意 yaw 没有绝对基准(纯陀螺积分),上电静止时恒为该次上电的相对 0 点, - * 因此"yaw=0.00"本身不是故障特征;判断故障要看下面的健康位。 + * 注意 yaw 无绝对基准(纯陀螺积分),上电静止时恒为该次上电的相对 0 点。 * - * ---- 健康位(排查"角度不动"时必须与角度一起看) ---- - * cvg 1-INS.ConvergeFlag:EKF 卡方检验通过、姿态可信(原诊断输出的 cvg - * 就是它,不要改成别的量,否则会丢掉"是否真的收敛"这一唯一判据); - * 0-未收敛/发散,此时角度不可信 + * ---- 健康位 / 诊断位(排查"角度不动或自漂"时必须一起看) ---- + * cvg 1-INS.ConvergeFlag:EKF 卡方检验通过、姿态可信(与原诊断输出同义, + * 不要改成别的量,否则会丢掉"是否真的收敛"这一唯一判据) * rdy 1-att_ready:ins_update() 至少执行过一次,只说明"在跑",不代表可信 * imu 1-bmi088_online:IMU 初始化成功;0=ID 检测失败,姿态必然全 0 * an |bmi088_accel|:静止时≈9.8;≈0 说明 IMU 根本没给出数据 - * gn |bmi088_gyro|:静止时≈0;转动时明显变大说明陀螺通路正常 + * gn |bmi088_gyro|:原始陀螺模长(**未减零偏**),静止时≈0 + * gx/gy/gz 原始陀螺三分量(rad/s) —— 与 gn 一起可判断数据是否冻结 + * zo ins_gyro_z_offset:上电静止标定得到的 z 零偏(rad/s)。**关键**: + * 实际进入滤波器的 gyro_z = gz - zo。若 zo 与 gz 量级相近甚至更大, + * 而 yaw 又匀速自漂,即标定窗口内车体在动把零偏标歪了 + * ck ins_z_cal_ok:1-标定时全程静止,zo 可信;0-三轮都在动,zo 存疑 + * ca/cg 加速度/陀螺采样计数:停止增长即数据路已死(数值冻结时它们不涨) * - * 不需要健康位时把 DEBUG_ATT_HEALTH 置 0,即只输出三个欧拉角。 + * 开关: DEBUG_ATT_HEALTH=0 只输出三个欧拉角;DEBUG_ATT_DIAG=0 去掉诊断位。 * ========================================================================== */ #include "debug_task.h" #include "cmsis_os.h" @@ -32,15 +36,18 @@ #include #include -/* 1-输出欧拉角 + 姿态链路健康位; 0-只输出欧拉角 */ +/* 1-输出健康位(cvg/rdy/imu/an/gn); 0-只输出三个欧拉角 */ #define DEBUG_ATT_HEALTH 1 +/* 1-再附加原始陀螺/零偏/采样计数(排障用); 0-只留上面的通用健康位 */ +#define DEBUG_ATT_DIAG 1 /* 输出周期(ms)。姿态观测建议 100ms(10Hz) */ #define DEBUG_OUT_PERIOD_MS 100 void debug_task(void const * argument) { - char line[128]; + char line[192]; + char tail[112]; for (;;) { @@ -51,16 +58,24 @@ void debug_task(void const * argument) bmi088_gyro[1] * bmi088_gyro[1] + bmi088_gyro[2] * bmi088_gyro[2]); - int len; + 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", + 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); +#endif + #if DEBUG_ATT_HEALTH - len = snprintf(line, sizeof(line), - "yaw=%.2f pitch=%.2f roll=%.2f cvg=%u rdy=%u imu=%u an=%.2f gn=%.3f\r\n", - att_deg_yaw, att_deg_pitch, att_deg_roll, - (unsigned)INS.ConvergeFlag, (unsigned)att_ready, - (unsigned)bmi088_online, an, gn); + int len = snprintf(line, sizeof(line), + "yaw=%.2f pitch=%.2f roll=%.2f cvg=%u rdy=%u imu=%u an=%.2f gn=%.3f%s\r\n", + att_deg_yaw, att_deg_pitch, att_deg_roll, + (unsigned)INS.ConvergeFlag, (unsigned)att_ready, + (unsigned)bmi088_online, an, gn, tail); #else - len = snprintf(line, sizeof(line), "yaw=%.2f pitch=%.2f roll=%.2f\r\n", - att_deg_yaw, att_deg_pitch, att_deg_roll); + int len = snprintf(line, sizeof(line), "yaw=%.2f pitch=%.2f roll=%.2f\r\n", + att_deg_yaw, att_deg_pitch, att_deg_roll); #endif if (len > 0) diff --git a/底盘/Core/Src/gpio.c b/底盘/Core/Src/gpio.c index 7850132..00a15e4 100644 --- a/底盘/Core/Src/gpio.c +++ b/底盘/Core/Src/gpio.c @@ -102,9 +102,17 @@ void MX_GPIO_Init(void) GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; HAL_GPIO_Init(CS1_ACCEL_GPIO_Port, &GPIO_InitStruct); - /*Configure GPIO pins : INT1_ACCEL_Pin INT1_GYRO_Pin */ + /*Configure GPIO pins : INT1_ACCEL_Pin INT1_GYRO_Pin + * BMI088 的 INT1(加速度计)/INT3(陀螺仪)在 bmi088_init 里被配成推挽、 + * 低有效的数据就绪信号(INT1_IO_CONF=0x08 / INT3_INT4_IO_CONF=0x00), + * 即"数据就绪"表现为电平拉低,对应下降沿。bmi088.c/h 文件头也写明 + * "数据就绪下降沿触发",legacy 用的同样是 EXTI_Trigger_Falling。 + * CubeMX 默认生成的是上升沿,那会落在这个信号的电平恢复沿上,采样时序 + * 整体错位 —— 故此处改回下降沿。 + * 注意: 本文件由 CubeMX 生成,重新生成工程会改回 RISING;.ioc 里已同步 + * 写入 GPIO_MODE_IT_FALLING,正常用 CubeMX 重新生成可保留。 */ GPIO_InitStruct.Pin = INT1_ACCEL_Pin|INT1_GYRO_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; + GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING; GPIO_InitStruct.Pull = GPIO_PULLUP; HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); diff --git a/底盘/Simba-ELCT-FrameWork.ioc b/底盘/Simba-ELCT-FrameWork.ioc index 128ee68..37d9aae 100644 --- a/底盘/Simba-ELCT-FrameWork.ioc +++ b/底盘/Simba-ELCT-FrameWork.ioc @@ -362,12 +362,14 @@ PC2.GPIOParameters=GPIO_Label PC2.GPIO_Label=HW2 PC2.Locked=true PC2.Signal=GPIO_Input -PC4.GPIOParameters=GPIO_PuPd,GPIO_Label +PC4.GPIOParameters=GPIO_PuPd,GPIO_Label,GPIO_ModeDefaultEXTI +PC4.GPIO_ModeDefaultEXTI=GPIO_MODE_IT_FALLING PC4.GPIO_Label=INT1_ACCEL PC4.GPIO_PuPd=GPIO_PULLUP PC4.Locked=true PC4.Signal=GPXTI4 -PC5.GPIOParameters=GPIO_PuPd,GPIO_Label +PC5.GPIOParameters=GPIO_PuPd,GPIO_Label,GPIO_ModeDefaultEXTI +PC5.GPIO_ModeDefaultEXTI=GPIO_MODE_IT_FALLING PC5.GPIO_Label=INT1_GYRO PC5.GPIO_PuPd=GPIO_PULLUP PC5.Locked=true diff --git a/底盘/modules/BMI088/bmi088.c b/底盘/modules/BMI088/bmi088.c index f45a197..8dd7deb 100644 --- a/底盘/modules/BMI088/bmi088.c +++ b/底盘/modules/BMI088/bmi088.c @@ -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) diff --git a/底盘/modules/BMI088/bmi088.h b/底盘/modules/BMI088/bmi088.h index 22cd599..20aceb4 100644 --- a/底盘/modules/BMI088/bmi088.h +++ b/底盘/modules/BMI088/bmi088.h @@ -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); diff --git a/底盘/modules/ins/ins.c b/底盘/modules/ins/ins.c index 93c25f6..77b823d 100644 --- a/底盘/modules/ins/ins.c +++ b/底盘/modules/ins/ins.c @@ -16,11 +16,33 @@ #include "main.h" #include -/* 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; diff --git a/底盘/modules/ins/ins.h b/底盘/modules/ins/ins.h index a35be87..fe87a8e 100644 --- a/底盘/modules/ins/ins.h +++ b/底盘/modules/ins/ins.h @@ -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);