ELCT/Simba-OmniInfantry
9
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
main
Simba-OmniInfantry/底盘/Core/Src/gpio.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

151 lines
5.3 KiB
C
Raw Permalink Blame History

/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file gpio.c
* @brief This file provides code for the configuration
* of all used GPIO pins.
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "gpio.h"
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/*----------------------------------------------------------------------------*/
/* Configure GPIO */
/*----------------------------------------------------------------------------*/
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/** Configure pins as
* Analog
* Input
* Output
* EVENT_OUT
* EXTI
*/
void MX_GPIO_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
/* GPIO Ports Clock Enable */
__HAL_RCC_GPIOB_CLK_ENABLE();
__HAL_RCC_GPIOG_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOD_CLK_ENABLE();
__HAL_RCC_GPIOC_CLK_ENABLE();
__HAL_RCC_GPIOI_CLK_ENABLE();
__HAL_RCC_GPIOF_CLK_ENABLE();
__HAL_RCC_GPIOH_CLK_ENABLE();
__HAL_RCC_GPIOE_CLK_ENABLE();
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(RSTN_IST8310_GPIO_Port, RSTN_IST8310_Pin, GPIO_PIN_RESET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, GPIO_PIN_SET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOB, SPI2_CS_Pin|CS1_GYRO_Pin, GPIO_PIN_SET);
/*Configure GPIO pin : BUTTON_TRIG_Pin */
GPIO_InitStruct.Pin = BUTTON_TRIG_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_PULLUP;
HAL_GPIO_Init(BUTTON_TRIG_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pin : RSTN_IST8310_Pin */
GPIO_InitStruct.Pin = RSTN_IST8310_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_MEDIUM;
HAL_GPIO_Init(RSTN_IST8310_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pins : DRDY_IST8310_Pin PG0 */
GPIO_InitStruct.Pin = DRDY_IST8310_Pin|GPIO_PIN_0;
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
GPIO_InitStruct.Pull = GPIO_PULLUP;
HAL_GPIO_Init(GPIOG, &GPIO_InitStruct);
/*Configure GPIO pins : HW0_Pin HW1_Pin HW2_Pin */
GPIO_InitStruct.Pin = HW0_Pin|HW1_Pin|HW2_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
/*Configure GPIO pin : KEY_Pin */
GPIO_InitStruct.Pin = KEY_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_PULLUP;
HAL_GPIO_Init(KEY_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pin : CS1_ACCEL_Pin */
GPIO_InitStruct.Pin = CS1_ACCEL_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_PULLUP;
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
* 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_FALLING;
GPIO_InitStruct.Pull = GPIO_PULLUP;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
/*Configure GPIO pin : SPI2_CS_Pin */
GPIO_InitStruct.Pin = SPI2_CS_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_MEDIUM;
HAL_GPIO_Init(SPI2_CS_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pin : CS1_GYRO_Pin */
GPIO_InitStruct.Pin = CS1_GYRO_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(CS1_GYRO_GPIO_Port, &GPIO_InitStruct);
/* EXTI interrupt init*/
HAL_NVIC_SetPriority(EXTI0_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(EXTI0_IRQn);
HAL_NVIC_SetPriority(EXTI3_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(EXTI3_IRQn);
HAL_NVIC_SetPriority(EXTI4_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(EXTI4_IRQn);
HAL_NVIC_SetPriority(EXTI9_5_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
}
/* USER CODE BEGIN 2 */
/* USER CODE END 2 */
Reference in New Issue View Git Blame Copy Permalink