Compare commits
10
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 LSMushui
|
7c25e06947 | ||
|
LSMushui
|
e3d432f3b5 | ||
|
LSMushui
|
3e5e5e3f95 | ||
|
LSMushui
|
9fc1575615 | ||
|
LSMushui
|
72bf925f47 | ||
|
LSMushui
|
3e1bae6b98 | ||
|
LSMushui
|
4f18ada2f7 | ||
|
LSMushui
|
944d0201da | ||
|
LSMushui
|
5e6c5068fa | ||
|
LSMushui
|
3c95bb0887 |
@@ -35,3 +35,6 @@ DebugConfig/
|
||||
*.tra
|
||||
*~
|
||||
Thumbs.db
|
||||
|
||||
# ---- 助手工作区状态(记忆/会话数据),不属于固件源码 ----
|
||||
.workbuddy/
|
||||
|
||||
@@ -0,0 +1,431 @@
|
||||
# Simba-OmniInfantry
|
||||
|
||||
<table>
|
||||
<tr>
|
||||
<td><img src="https://git.simba-robotics.com/assets/img/logo.svg" width="160" height="160" alt="Simba-git" /></td>
|
||||
<td><img src="https://git.simba-robotics.com/avatars/cd8e5bc7a1039e1cbadabefe03e647af1bbbd8722d659d437479b93c8c6fa799?" width="160" height="160" alt="ELEC" /></td>
|
||||
</tr>
|
||||
</table>
|
||||
|
||||
[](https://gitee.com/LSMushui)
|
||||
|
||||
|
||||
|
||||
|
||||
> Simba 步兵机器人电控固件 —— 双板架构(云台板 + 底盘板),Keil + FreeRTOS + CMSIS-DSP。
|
||||
|
||||
---
|
||||
|
||||
## 目录
|
||||
|
||||
- [1. 项目概览](#1-项目概览)
|
||||
- [2. 硬件与工具链](#2-硬件与工具链)
|
||||
- [3. 快速开始](#3-快速开始)
|
||||
- [4. 代码地图](#4-代码地图)
|
||||
- [5. 运行架构](#5-运行架构)
|
||||
- [6. 数据流与对外接口](#6-数据流与对外接口)
|
||||
- [7. 模块清单](#7-模块清单)
|
||||
- [8. 调试:USB CDC 遥测](#8-调试usb-cdc-遥测)
|
||||
- [9. 调参与开关](#9-调参与开关)
|
||||
- [10. 上电行为与灯语](#10-上电行为与灯语)
|
||||
- [11. 已知陷阱与注意事项](#11-已知陷阱与注意事项)
|
||||
- [12. 版本管理约定](#12-版本管理约定)
|
||||
- [13. 致谢与联系](#13-致谢与联系)
|
||||
|
||||
---
|
||||
|
||||
## 1. 项目概览
|
||||
|
||||
本仓库包含 **两套完整且结构对称的 Keil 工程**,分别烧录到步兵机器人的两块 C 型开发板上:
|
||||
|
||||
| 工程 | 编译宏 | 角色 |
|
||||
| --- | --- | --- |
|
||||
| [`云台/`](./云台) | `ROBOT_TYPE_GIMBAL` | 云台(Yaw/Pitch)+ 发射机构 + 图传链路 + 板间通讯主控 |
|
||||
| [`底盘/`](./底盘) | `ROBOT_TYPE_CHASSIS` | 麦轮底盘 + 跟随云台 + 功率管理 |
|
||||
|
||||
设计要点:
|
||||
|
||||
- **两板源码高度同构**。`Application/`、`Bsp/`、`modules/` 绝大多数文件在两棵树下是同一份拷贝,板级差异用 `ROBOT_TYPE_*` 宏在文件内条件编译(而不是拆成两套文件)。**因此改一处通常要同步改另一处**,详见 [§11](#11-已知陷阱与注意事项)。
|
||||
- **姿态由 QuaternionEKF 统一负责**。两板各有一颗板载 BMI088,跑同一套 Wang Hongxi 的 QuaternionEKF 四元数 EKF(陀螺积分预测 + 加速度重力观测 + 卡方检验),不再使用 legacy 的 Mahony 互补滤波,也不再依赖预编译的 `AHRS.lib`。
|
||||
- **跟随采用"双 INS 世界系"方案**。云台把自己解算出的世界系 yaw 通过 CAN 发给底盘,底盘用自身 INS 的 yaw 相减得到相对偏差。两板 INS 的上电零点任意但差值天然正确,因而绕开了皮带虚位与基准锁定时序的坑。
|
||||
|
||||
---
|
||||
|
||||
## 2. 硬件与工具链
|
||||
|
||||
| 项 | 说明 |
|
||||
| --- | --- |
|
||||
| MCU | STM32F407IGH6(Cortex-M4F,单精度硬浮点,168 MHz) |
|
||||
| SDK | STM32CubeF4(HAL 库) |
|
||||
| RTOS | FreeRTOS(CMSIS-RTOS v1 封装),`configTICK_RATE_HZ = 1000` |
|
||||
| 数学库 | ARM CMSIS-DSP(`arm_cortexM4lf_math.lib`,卡尔曼滤波的矩阵运算全依赖它) |
|
||||
| 工程 | CubeMX 生成 + Keil MDK-ARM(工程文件 `MDK-ARM/Simba-ELCT-FrameWork.uvprojx`) |
|
||||
| 上位机 | VSCode 作为编辑器(配 `.clang-format`),**不要用 Keil 编辑器改代码** |
|
||||
|
||||
> 强烈建议按社区教程把 Keil 切到 **AC6** 编译器(AC5 编译极慢)。切换方式见 RoboMaster 社区的 AC6 配置教程。
|
||||
|
||||
---
|
||||
|
||||
## 3. 快速开始
|
||||
|
||||
### 3.1 编译与烧录
|
||||
|
||||
1. 用 Keil 打开对应板子的工程:
|
||||
- 云台:`云台/MDK-ARM/Simba-ELCT-FrameWork.uvprojx`
|
||||
- 底盘:`底盘/MDK-ARM/Simba-ELCT-FrameWork.uvprojx`
|
||||
2. 确认编译宏:Target → C/C++ → Define 中应含 `ROBOT_TYPE_GIMBAL` 或 `ROBOT_TYPE_CHASSIS`(两个工程已各自配好)。
|
||||
3. Rebuild → Download。
|
||||
|
||||
### 3.2 上电前置检查
|
||||
|
||||
- **上电后头 1.5 秒请把车体放稳别碰**。IMU 的 z 轴陀螺零偏要在这段时间内静止标定,标定窗口里混入运动会让 yaw 恒定自漂(见 [§11.3](#113-z-轴零偏标定必须静止))。
|
||||
- 云台板会做一次 **yaw 装配符号自检**(给 Yaw 电机一个小电压脉冲看陀螺往哪边转),此时云台会轻转几度,**不要扶住云台**;失败时回退到 `GIMBAL_YAW_SIGN_FALLBACK`。
|
||||
|
||||
### 3.3 看一眼运行状态
|
||||
|
||||
插上 USB 会出现虚拟串口(CDC),波特率随意,直接打开即可看到姿态与诊断输出,字段含义见 [§8](#8-调试usb-cdc-遥测)。
|
||||
|
||||
---
|
||||
|
||||
## 4. 代码地图
|
||||
|
||||
```
|
||||
Simba-OmniInfantry/
|
||||
├── README.md 本文
|
||||
├── doc/
|
||||
│ └── 已知问题与排障.md 踩坑记录与排障手册(强烈建议先读)
|
||||
├── 云台/ ┐
|
||||
│ ├── Application/ │ 应用层
|
||||
│ │ ├── Task/Inc/ │ 任务头文件(app_tasks.h 汇总声明)
|
||||
│ │ ├── Task/Src/ │ 各任务实现
|
||||
│ │ ├── robot_param.h │ ★ 整车唯一调参入口(含台架调试开关)
|
||||
│ │ └── robot_typedef.h │ 应用层公共类型
|
||||
│ ├── Bsp/ │ 板级支持包(与具体外设寄存器打交道)
|
||||
│ │ ├── Inc/ Src/ │ bsp_can(过滤器+按ID分发回调)、bsp_usart(DMA收发)
|
||||
│ ├── Core/ │ CubeMX 生成的初始化与中断
|
||||
│ │ ├── Src/main.c │ 外设初始化顺序
|
||||
│ │ ├── Src/freertos.c │ ★ 任务创建与优先级/栈大小
|
||||
│ │ ├── Src/stm32f4xx_it.c │ 中断入口(含 USART 空闲中断调用)
|
||||
│ │ └── Inc/FreeRTOSConfig.h │ 节拍/堆大小等
|
||||
│ ├── Drivers/ │ ST HAL + CMSIS(勿手改)
|
||||
│ ├── Middlewares/ │ FreeRTOS / USB Device Library / CMSIS-DSP
|
||||
│ ├── USB_DEVICE/ │ USB CDC 设备栈(usbd_cdc_if.c 为应用接口)
|
||||
│ ├── modules/ │ ★ 按角色分层的功能模块(见下)
|
||||
│ └── MDK-ARM/ │ Keil 工程与启动文件
|
||||
└── 底盘/ ┘ 结构同上
|
||||
```
|
||||
|
||||
### `modules/` 分层(2026-09 重排)
|
||||
|
||||
```
|
||||
modules/
|
||||
├── algorithm/ 通用算法 pid, filter
|
||||
├── imu/ 姿态与惯导封装 attitude(对外姿态 API)、ins(QEKF 封装)
|
||||
│ └── qekf/ 第三方移植内核 QuaternionEKF、kalman_filter(移植自 Wang Hongxi INS 例程)
|
||||
├── sensor/ 板载传感器驱动 bmi088(SPI+DMA)、ist8310(I2C 磁力计)
|
||||
├── comms/ 通讯 dr16(遥控器)、referee(裁判/图传)、c_comm(板间)
|
||||
├── motor/ 电机抽象 dji_motor(大疆电机统一接口)
|
||||
├── control/ 整车控制逻辑 gimbal + shoot(云台板) / chassis(底盘板)
|
||||
└── dev/ 板级指示与发声 led、buzzer
|
||||
```
|
||||
|
||||
> `imu/qekf/` 里的两个文件是原样移植的第三方代码,**尽量不改**;要改先读 `imu/attitude.c` 与 `imu/ins.c` 的文件头注释(QEKF 内部 Pitch/Roll 命名与航空惯例互换)。
|
||||
|
||||
---
|
||||
|
||||
## 5. 运行架构
|
||||
|
||||
### 5.1 任务表
|
||||
|
||||
FreeRTOS 节拍 1 kHz。任务来自 `Core/Src/freertos.c`:
|
||||
|
||||
| 任务 | 优先级 | 栈(words) | 分配方式 | 职责 | 板 |
|
||||
| --- | --- | --- | --- | --- | --- |
|
||||
| `IMU_Task` | High | 512 | 静态 | 1 kHz 姿态解算 + 磁力计轮询 + IMU 恒温 | 双板 |
|
||||
| `gimbal_task` / `chassis_task` | AboveNormal | 512 | 动态 | 2 ms 控制环 | 各一板 |
|
||||
| `CAN_Task` | Normal | 512 | 静态 | 全部硬件初始化,完成后置 `app_ready` | 双板 |
|
||||
| `robot_cmd_task` | Normal | 384 | 动态 | 遥控/键鼠 → 统一控制指令 | 双板 |
|
||||
| `c_comm_task` | Normal | 256 | 动态 | 2 ms 板间通讯收发 | 双板 |
|
||||
| `shoot_task` | Normal | 384 | 动态 | 摩擦轮/拨弹 | 云台 |
|
||||
| `referee_task` | Low | 256 | 动态 | 裁判系统解析 | 双板 |
|
||||
| `debug_task` | Low | 384 | 动态 | USB CDC 姿态与诊断输出 | 双板 |
|
||||
| `LED_Task`(monitor) | Low | 512 | 静态 | 心跳监控 + LED/蜂鸣报警 + 喂 IWDG | 双板 |
|
||||
|
||||
**初始化顺序**:`CAN_Task` 先跑,依次 `led_init → buzzer_init → MX_USB_DEVICE_Init → can_start → 电机/发射/板间通讯初始化 → 串口初始化 → app_ready = 1`。其余任务在 `app_ready` 之前 `while` 等待。
|
||||
|
||||
### 5.2 控制周期
|
||||
|
||||
- 姿态解算 **1 kHz**(`IMU_Task`,`vTaskDelayUntil` 精确节拍,`dt` 固定 1 ms)
|
||||
- 整车控制 **2 ms**(改周期需重调所有 PID)
|
||||
- 板间通讯 **2 ms**
|
||||
|
||||
### 5.3 看门狗
|
||||
|
||||
`LED_Task` 入口立即启动 IWDG(约 2.5 s 溢出)。**上电前 5 s 无条件喂狗**,之后仅当所有任务心跳都在 500 ms 内刷新过才喂 —— 任何任务卡死都会复位整车,不会失控。
|
||||
|
||||
> ⚠️ 因此**整个初始化必须在 5 s 内完成**。传感器初始化失败重试、IMU 零偏标定都在这个预算里,扩展时要算好(见 [§11.4](#114-初始化必须留在-5-s-内)。
|
||||
|
||||
---
|
||||
|
||||
## 6. 数据流与对外接口
|
||||
|
||||
### 6.1 CAN 总线分配
|
||||
|
||||
| 板 | CAN1 | CAN2 |
|
||||
| --- | --- | --- |
|
||||
| 云台板 | Pitch GM6020 `0x206`、摩擦轮 M3508 `0x201/0x202` | Yaw GM6020 `0x205`、底盘回传 `0x019` |
|
||||
| 底盘板 | 四轮 M3508 `0x201~0x204` | 云台数据 `0x149/0x189/0x1A9` |
|
||||
|
||||
### 6.2 串口分配
|
||||
|
||||
| 外设 | 用途 | 备注 |
|
||||
| --- | --- | --- |
|
||||
| USART3 | DR16 遥控器接收机 | 100000-8E1,需 9 位字长(`bsp_usart.c` 里重新初始化) |
|
||||
| USART6 | 图传链路 / 裁判系统(3pin 口) | 变长帧 |
|
||||
| USART1 | 视觉上位机(4pin 口) | 收发 DMA 已就绪,协议待接入 |
|
||||
|
||||
三者都是 **循环 DMA + 空闲中断取整帧**,在中断上下文把整帧交给注册的回调。
|
||||
|
||||
### 6.3 板间通讯协议(CToC)
|
||||
|
||||
帧 ID 与 legacy 一致,定义见 `modules/comms/c_comm.h`:
|
||||
|
||||
| 帧 | 方向 | 内容 |
|
||||
| --- | --- | --- |
|
||||
| `0x149` | 云台 → 底盘 | 四路摇杆量(大端 16 bit) |
|
||||
| `0x189` | 云台 → 底盘 | 遥控状态、S1/S2、Shift、启动标志、小陀螺使能、云台电机故障位 |
|
||||
| `0x1A9` | 云台 → 底盘 | 云台世界系 yaw(float,LE)+ flags(att_ready / 编码器增向 / 自检完成) |
|
||||
| `0x019` | 底盘 → 云台 | 发射机构上电状态、底盘电机故障位 |
|
||||
|
||||
**故障位语义**:任一侧电机掉线即置位,对侧收到后必须停止自己的输出(整车安全)。
|
||||
|
||||
### 6.4 姿态数据流
|
||||
|
||||
```
|
||||
BMI088(SPI1 + DMA,EXTI 数据就绪)
|
||||
└─> modules/sensor/bmi088.c 解析出 bmi088_accel[] / bmi088_gyro[]
|
||||
└─> modules/imu/ins.c z 零偏补偿 + QEKF 更新(1 kHz)
|
||||
└─> modules/imu/attitude.c 输出 att_deg_yaw / att_deg_pitch / att_deg_roll
|
||||
├─> control/gimbal.c Yaw/Pitch 环反馈
|
||||
└─> comms/c_comm.c 0x1A9 帧发给底盘
|
||||
```
|
||||
|
||||
### 6.5 视觉上位机(待接入)
|
||||
|
||||
`usart1_tx_dma_init()` 已在 `CAN_Task` 中就绪,`bsp_usart.c` 提供 `usart1_tx_dma_enable()`。自瞄协议接入时直接用这套接口即可,无需改动 BSP。
|
||||
|
||||
---
|
||||
|
||||
## 7. 模块清单
|
||||
|
||||
| 模块 | 路径 | 职责 |
|
||||
| --- | --- | --- |
|
||||
| `pid` | `algorithm/` | 位置式/增量式 PID,双环共用 |
|
||||
| `filter` | `algorithm/` | 通用滤波器(低通等) |
|
||||
| `attitude` | `imu/` | 对外的姿态 API:初始化、1 kHz 更新、`att_deg_*`、磁力计轮询、IMU 加热控制 |
|
||||
| `ins` | `imu/` | QEKF 封装 + z 轴零偏标定 + 机体系/导航系变换 + 运动加速度 |
|
||||
| `QuaternionEKF` / `kalman_filter` | `imu/qekf/` | 第三方移植:四元数 EKF 与通用卡尔曼框架(**勿轻易改**) |
|
||||
| `bmi088` | `sensor/` | 六轴 IMU 驱动(SPI1 模式 3 @5.25 MHz,INT 数据就绪触发 DMA,含数据路自恢复) |
|
||||
| `ist8310` | `sensor/` | 三轴磁力计(硬件 I2C3,200 Hz 轮询) |
|
||||
| `dr16` | `comms/` | DR16 遥控器帧解析 |
|
||||
| `referee` | `comms/` | 裁判系统 / 图传链路解析 |
|
||||
| `c_comm` | `comms/` | 板间通讯收发与故障位综合 |
|
||||
| `dji_motor` | `motor/` | 大疆电机(CAN 反馈解析 + 电流/电压下发)统一抽象 |
|
||||
| `gimbal` | `control/` | 云台 Yaw/Pitch 双环 + yaw 装配符号自检 |
|
||||
| `shoot` | `control/` | 摩擦轮 + 拨弹 |
|
||||
| `chassis` | `control/` | 麦轮解算 + 轮速环 + 跟随 + 功率限制 |
|
||||
| `led` / `buzzer` | `dev/` | 三色 LED(TIM5 PWM)与无源蜂鸣器(TIM4_CH3/PD14) |
|
||||
| `bsp_can` | `Bsp/` | CAN 过滤器、按 (总线, 标准 ID) 分发回调、发送封装 |
|
||||
| `bsp_usart` | `Bsp/` | 三路串口的循环 DMA + 空闲中断整帧接收,以及 TX DMA |
|
||||
|
||||
---
|
||||
|
||||
## 8. 调试:USB CDC 遥测
|
||||
|
||||
`Application/Task/Src/debug_task.c` 每 100 ms 输出一行(单位:角度 ° / 加速度 m/s² / 角速度 rad/s):
|
||||
|
||||
```
|
||||
yaw=-0.03 pitch=-25.58 roll=-8.00 cvg=1 rdy=1 imu=1 an=9.80 gn=0.017
|
||||
gx=0.0012 gy=-0.0031 gz=0.0165 zo=0.0009 ck=1 ca=803 cg=1002 se=0 st=1
|
||||
```
|
||||
|
||||
> 实测是一行,上面为排版折行。
|
||||
|
||||
### 欧拉角
|
||||
|
||||
| 字段 | 含义 |
|
||||
| --- | --- |
|
||||
| `yaw` | 多圈连续偏航角(`INS.YawTotalAngle`)。**无绝对基准**(纯陀螺积分),上电静止时恒为该次上电的相对 0 点,所以"yaw=0.00"不是故障特征 |
|
||||
| `pitch` | 绕 y 轴角(`= QEKF_INS.Roll`) |
|
||||
| `roll` | 绕 x 轴角(`= QEKF_INS.Pitch`,云台 Pitch 环反馈用它) |
|
||||
|
||||
> QEKF 内部 Pitch/Roll 命名与航空惯例**互换**,映射关系与原因见 `modules/imu/attitude.c` 文件头。
|
||||
|
||||
### 健康位与诊断位
|
||||
|
||||
| 字段 | 含义 | 怎么用 |
|
||||
| --- | --- | --- |
|
||||
| `cvg` | `INS.ConvergeFlag`,1 = 卡方检验通过、姿态可信 | **唯一"是否真收敛"的判据**,不要和别的量混淆 |
|
||||
| `rdy` | `att_ready`,1 = 姿态更新至少跑过一次 | 只说明"在跑",不代表可信 |
|
||||
| `imu` | `bmi088_online`,1 = IMU 初始化成功 | 0 则姿态必然全 0 |
|
||||
| `an` | `\|bmi088_accel\|` | 静止应 ≈ 9.8;≈ 0 说明 IMU 没给数据 |
|
||||
| `gn` | `\|bmi088_gyro\|`(**未减零偏**的原始模长) | 静止 ≈ 0;转动时明显变大 |
|
||||
| `gx/gy/gz` | 原始陀螺三分量 | 与 `gn` 一起判断数据是否冻结 |
|
||||
| `zo` | `ins_gyro_z_offset`,上电标定得到的 z 零偏 | 实际进滤波器的是 `gz - zo`。**若 `zo` 明显大于 `gz` 而 yaw 又匀速自漂 ⇒ 零偏标歪了** |
|
||||
| `ck` | `ins_z_cal_ok`,1 = 标定时全程静止,`zo` 可信 | 0 说明三轮标定都没静下来 |
|
||||
| `ca`/`cg` | 加速度/陀螺采样计数 | **判断"数据是否还在更新"的唯一可靠依据 —— 数值可能冻结在旧值上,计数不会** |
|
||||
| `se` | SPI/DMA 出错累计次数 | 持续增长说明 SPI 出错频繁 |
|
||||
| `st` | SPI 状态(1=READY) | **长期非 1 ⇒ SPI 卡死**;`st=1` 但 `ca/cg` 不涨 ⇒ 数据就绪中断丢失 |
|
||||
|
||||
### 开关
|
||||
|
||||
`debug_task.c` 顶部:
|
||||
|
||||
- `DEBUG_ATT_HEALTH = 0` → 只输出三个欧拉角
|
||||
- `DEBUG_ATT_DIAG = 0` → 去掉 `gx..st` 这些排障字段
|
||||
|
||||
---
|
||||
|
||||
## 9. 调参与开关
|
||||
|
||||
**所有可调参数集中在 `Application/robot_param.h`**(整车唯一调参入口),分区如下:
|
||||
|
||||
| 分区 | 内容 |
|
||||
| --- | --- |
|
||||
| 云台 | Pitch 限位/零位、摩擦轮/拨弹转速、拨杆比例、Yaw 双环 PID、Pitch 双环 PID |
|
||||
| 云台符号自检 | `GIMBAL_YAW_SIGN_AUTO`、检测脉冲电压与判定阈值 |
|
||||
| 底盘 | 麦轮几何、转速换算系数、摇杆比例、轮速 PID、跟随 PID、功率限制 |
|
||||
| CToC | 四个板间帧 ID |
|
||||
| 掉线检测 | `DEVICE_OFFLINE_TIME_MS`、`CTOC_OFFLINE_TIME_MS` |
|
||||
| IMU | 恒温目标/限值、温度 PID |
|
||||
| **台架调试开关** | `DEBUG_SHOOT_WITHOUT_REFEREE`、`GIMBAL_RAMMER_INSTALLED`、`DEBUG_BUZZER_MELODY` |
|
||||
|
||||
### 三个台架调试开关
|
||||
|
||||
| 宏 | 默认 | 作用 | 比赛前 |
|
||||
| --- | --- | --- | --- |
|
||||
| `DEBUG_SHOOT_WITHOUT_REFEREE` | 1 | 无裁判系统时也允许开摩擦轮 | 改 0 |
|
||||
| `GIMBAL_RAMMER_INSTALLED` | 0 | 拨弹 M2006 是否已安装(不装则不参与掉线判定) | 按实际 |
|
||||
| `DEBUG_BUZZER_MELODY` | 1 | CAN 掉线警告音换成循环旋律(避免台架调试时连续滴滴声)。**只换声音不换灯语**,故障信息不丢 | 改 0 |
|
||||
|
||||
---
|
||||
|
||||
## 10. 上电行为与灯语
|
||||
|
||||
### 上电时序
|
||||
|
||||
1. `CAN_Task`:**蓝灯常亮**(调度器与任务栈正常的最直观证据)→ 蜂鸣器/USB/CAN/电机/串口逐个初始化
|
||||
2. `IMU_Task`:BMI088 + IST8310 初始化(有界重试)→ **z 零偏静止标定**(约 0.5~1.5 s)→ 启动 1 kHz 解算
|
||||
3. 云台板:**yaw 装配符号自检**(云台会轻转几度,勿扶)
|
||||
4. `app_ready = 1` → 其余任务开始跑 → `LED_Task` 熄灭蓝灯,交由报警逻辑接管
|
||||
|
||||
### 灯语(LED)
|
||||
|
||||
| 现象 | 含义 |
|
||||
| --- | --- |
|
||||
| 蓝灯每 2 s 短闪 | 系统健康心跳(区分"空闲"与"卡死") |
|
||||
| 红灯 1 s 内双闪 | 遥控器掉线 |
|
||||
| 红灯常亮 | CAN 设备掉线(同时蜂鸣报警) |
|
||||
| 绿灯 1 s 内连闪两下 | **BMI088 失败** |
|
||||
| 绿灯 1 s 周期慢闪 | IST8310 失败 |
|
||||
| 绿灯常亮 | IMU 过温(已关加热)或双传感器都失败 |
|
||||
|
||||
### 蜂鸣器
|
||||
|
||||
- **CAN 掉线报警**:按掉线设备序号 `n` 每 2 s 窗内响 `n` 下,只报**最先**掉线的那台设备。云台序号:`0=摩擦轮L 1=摩擦轮R 2=Pitch 3=Yaw 4=拨弹 5=底盘回传`;底盘:`0~3=四轮 4=云台数据`。所以**响三下 = Pitch 电机掉线**。
|
||||
- **遥控器连接音效**:遥控器上线瞬间两声上扬。
|
||||
- 调试模式下上述报警音可换成旋律,见 [§9](#9-调参与开关)。
|
||||
|
||||
---
|
||||
|
||||
## 11. 已知陷阱与注意事项
|
||||
|
||||
> 每一条都是实际踩过的坑。更详细的排障过程见 [doc/已知问题与排障.md](./doc/已知问题与排障.md)。
|
||||
|
||||
### 11.1 增删源文件必须同步改 `.uvprojx`(和 `.uvoptx`)
|
||||
|
||||
新增/删除 `.c` 后必须同时在 Keil 工程里增删条目,并检查:
|
||||
|
||||
- `.uvprojx` 的 `<FilePath>` 指向的文件真实存在
|
||||
- `.uvoptx` 的 `<FileNumber>` 是**全局连续序号**,删条目后末项编号要跟着改,否则 Keil 每次保存都会重写该文件
|
||||
- **`.h` 不登记在工程里**(只登记 `.c`),头文件靠 `<IncludePath>` 找到
|
||||
|
||||
### 11.2 libc 堆必须 ≥ 8 KB(QEKF 的矩阵内存靠 `malloc`)
|
||||
|
||||
`modules/imu/qekf/kalman_filter.h` 的分配宏:
|
||||
|
||||
```c
|
||||
#ifdef _CMSIS_OS_H
|
||||
#define user_malloc pvPortMalloc // ← 实际走不到这支
|
||||
#else
|
||||
#define user_malloc malloc // ← 实际走这支
|
||||
#endif
|
||||
```
|
||||
|
||||
`kalman_filter.c` 只 include 了 `kalman_filter.h`,**从未引入 `cmsis_os.h`**,所以 `user_malloc` 展开为 **libc `malloc`**,FreeRTOS 那 32 KB 堆完全用不上。而 `Kalman_Filter_Init(6, 0, 3)` 要发起 **25 次分配、合计约 1.6 KB**,全程无 NULL 检查。
|
||||
|
||||
因此 `MDK-ARM/startup_stm32f407xx.s` 里的 `Heap_Size` **必须为 `0x2000`(8 KB)**(与原始例程一致)。CubeMX 默认值是 `0x200`(512 B),会导致后半段分配返回 NULL、滤波器静默失效(角度恒定不动)。
|
||||
|
||||
### 11.3 z 轴零偏标定必须静止
|
||||
|
||||
QEKF 无法观测 z 偏置,只能上电静止标定。标定窗口里混入运动样本会把 `gyro_z_offset` 标成一个大常值,之后 **yaw 以该常值匀速自漂,与陀螺真实输出无关**。
|
||||
|
||||
`ins.c` 已有健壮性(等 50 ms 稳定 → 剔除 `|gyro| > 0.35 rad/s` 的离群样本 → 单轮有效样本占比 < 90% 判为"车体在动"整轮重测,最多 3 轮 → 均不静则置 `ins_z_cal_ok = 0`),但**根治手段仍然是上电后那 1.5 秒别碰车**。
|
||||
|
||||
判断方法:`yaw` 漂移率(rad/s)如果**大于**遥测里的 `gn`,就说明漂移来自被污染的 `zo`,不是陀螺本身。
|
||||
|
||||
### 11.4 初始化必须留在 5 s 内
|
||||
|
||||
IWDG 的上电宽限期只有 5 s,而 `IMU_Task` 在 `attitude_init()` 期间不刷新心跳。所以"整个初始化"必须留在 5 s 内 —— 这也是 `INS_Z_CAL_ATTEMPTS × INS_Z_CAL_SAMPLES` 没有放大的原因。要延长标定预算,得同时调整看门狗宽限期。
|
||||
|
||||
### 11.5 BMI088 的中断是低有效、必须配下降沿
|
||||
|
||||
INT1(加速度计)/INT3(陀螺仪)被配成推挽、低有效的数据就绪信号,`EXTI` 必须为**下降沿**。CubeMX 生成的 `gpio.c` 默认给的是 `GPIO_MODE_IT_RISING`,会落在电平恢复沿上、采样时序错位。已在 `.ioc` 里写入 `GPIO_ModeDefaultEXTI=GPIO_MODE_IT_FALLING`,但**重新生成工程后仍要复查 `gpio.c`**。
|
||||
|
||||
### 11.6 数据路可能静默冻结
|
||||
|
||||
BMI088 的 DRDY 是**电平型**信号:某次失败或被挡掉的读取没把数据寄存器读走,INT 电平就会保持住,之后**既无下降沿也无上升沿**,EXTI 永不触发;或者 `hspi1` 被卡在 BUSY,`start_dma_read()` 的 `HAL_SPI_GetState` 检查会让每次调用直接返回。
|
||||
|
||||
这两种情况下,**写在中断回调里的兜底都不会执行**,所以自恢复放在任务上下文:`bmi088_health_check()` 由 `attitude_update()` 每 1 ms 调用,采样计数 5 ms 不涨即判定停摆,重启中断路(必要时 `abort`/重建 SPI)并阻塞读一次;修不好则退化为约 200 Hz 轮询保底。
|
||||
|
||||
**排查姿态异常时永远先看 `ca`/`cg` 是否在涨。**
|
||||
|
||||
### 11.7 CubeMX 重新生成会覆盖的改动
|
||||
|
||||
以下都是"手改过的 CubeMX 生成文件",重新生成工程会被改回去,**必须复查**:
|
||||
|
||||
| 文件 | 被覆盖的内容 |
|
||||
| --- | --- |
|
||||
| `MDK-ARM/startup_stm32f407xx.s` | `Heap_Size`(需 `0x2000`) |
|
||||
| `Core/Src/gpio.c` | INT1 触发沿(需 `FALLING`) |
|
||||
|
||||
### 11.8 其他
|
||||
|
||||
- `arm_mat_*` 失败是**静默**的:尺寸不符或 `pData = NULL` 时只返回错误码、不修改输出。调用方不查返回码,滤波器就会"看起来在跑但状态不更新"。
|
||||
- `modules/imu/qekf/` 是第三方移植代码,改动前先读原例程与 `attitude.c` 的文件头约定。
|
||||
- `Middlewares/ST/ARM/DSP/Lib/*.lib` 是**源码依赖而非构建产物**,已纳入版本管理,不要把 `*.lib` 加进 `.gitignore`。
|
||||
- 无源蜂鸣器 `buzzer_on(BUZZER_P)` 即静音,旋律表里用它作断音。
|
||||
|
||||
---
|
||||
|
||||
## 12. 版本管理约定
|
||||
|
||||
- 仓库根为 git 仓库,内含两个子工程;`.gitignore` 排除 Keil 构建产物、`*.uvguix.*`、`DebugConfig/`、`.workbuddy/`。
|
||||
- **`core.autocrlf=false`**:固件源码是 LF,不要让 git 在检出时改成 CRLF。
|
||||
- 注意换行符风格并不统一:`Core/Src/*.c` 与 `.ioc` 是 CRLF,`modules/**` 等是 LF。批量脚本改文件时要按文件自动适配。
|
||||
- 两板同构:改动请**同步改两棵树**,并确认差异只出现在应当不同的文件上(`gimbal*`/`shoot*` 只在云台,`chassis*` 只在底盘)。
|
||||
|
||||
---
|
||||
|
||||
## 13. 致谢与联系
|
||||
|
||||
- 姿态解算移植自 RoboMaster-C-Board-INS-Example(Wang Hongxi)的 QuaternionEKF 与通用卡尔曼框架。
|
||||
- 感谢各战队开源代码,legacy 版本(`new-infantry-c-board-legacy`)是本项目的移植来源与重要参照。
|
||||
|
||||
**通讯信息**
|
||||
|
||||
- 作者 `连山木水` 微信:18088745066 QQ:1194309310
|
||||
|
||||
## 参考
|
||||
|
||||
- 踩坑记录与排障手册:[`doc/已知问题与排障.md`](./doc/已知问题与排障.md)
|
||||
+160
@@ -0,0 +1,160 @@
|
||||
# 已知问题与排障手册
|
||||
|
||||
本文记录本项目实际踩过的坑、判断依据和处理方式。**遇到姿态/通讯异常时先读这里。**
|
||||
|
||||
---
|
||||
|
||||
## 0. 排障总则
|
||||
|
||||
### 0.1 先看 `ca` / `cg`,再看数值
|
||||
|
||||
USB 遥测里的 `ca`(加速度采样计数)和 `cg`(陀螺采样计数)是**唯一能证明"数据还在更新"的指标**。
|
||||
|
||||
数值是**会骗人的** —— 数据路停摆时,`an`、`gn`、`gx/gy/gz`、`pitch/roll` 都会安静地停在最后一个样本上,看起来"挺正常"。
|
||||
|
||||
```
|
||||
ca/cg 在涨 → 数据路活着,问题在算法或标定
|
||||
ca/cg 不涨 → 数据路死了,先修数据路,别的都别管
|
||||
```
|
||||
|
||||
### 0.2 用算式交叉验证,别凭感觉
|
||||
|
||||
姿态异常时,把**观测到的现象**和**遥测里的量**做量级核算,往往能直接定位层次。例:
|
||||
|
||||
- yaw 匀速自漂 → 算漂移率(rad/s)
|
||||
- 若漂移率 **> `gn`** ⇒ 漂移来自被污染的 z 零偏 `zo`,不是陀螺本身
|
||||
- 若漂移率 **≈ `gz - zo`** ⇒ 算法没问题,是数据冻结了(EKF 在忠实积分冻结值)
|
||||
- 角度恒定不动 → 看 `cvg`:`cvg=0` 说明量测一直被卡方检验拒绝;`cvg=1` 而角度不动 ⇒ 多半是 `ca/cg` 停了
|
||||
|
||||
### 0.3 症状速查表
|
||||
|
||||
| 症状 | 首先怀疑 | 去看 |
|
||||
| --- | --- | --- |
|
||||
| 欧拉角恒为 0.00 | IMU 没起来,或 EKF 内存分配失败 | `imu`、`an`、`cvg`;[§2.1](#21-三个欧拉角恒为-000) |
|
||||
| 角度恒定不动(有值但不变) | 数据路停摆(数据冻结) | `ca`/`cg`;[§2.2](#22-数据路静默冻结) |
|
||||
| yaw 匀速自漂 | z 零偏标定被运动污染 | `zo` vs `gz`,漂移率 vs `gn`;[§2.3](#23-yaw-匀速自漂) |
|
||||
| 角度是垃圾值 / yaw≈180 | 矩阵运算静默失败 | `cq` 字段、`Heap_Size`;[§2.1](#21-三个欧拉角恒为-000) |
|
||||
| 蜂鸣器连续滴滴响 | CAN 设备掉线报警(正常行为) | 响几下 = 第几号设备掉线;[§2.4](#24-蜂鸣器一直响) |
|
||||
| 红灯常亮 | CAN 设备掉线 | 同上 |
|
||||
| 绿灯连闪两下 | BMI088 失败 | `imu` |
|
||||
| 反复重启(约 3~5 s 一次) | 初始化超时 / 任务卡死 | IWDG 5 s 预算;[§2.5](#25-反复重启) |
|
||||
| 串口收不到整帧 | 空闲中断 / DMA 未重启 | `bsp_usart.c` |
|
||||
|
||||
---
|
||||
|
||||
## 1. 工具与判据
|
||||
|
||||
### 1.1 USB CDC 遥测字段
|
||||
|
||||
完整字段说明见根 [README §8](../README.md#8-调试usb-cdc-遥测)。排障最常用的几个:
|
||||
|
||||
| 字段 | 关键读法 |
|
||||
| --- | --- |
|
||||
| `ca`/`cg` | **是否在增长** = 数据路是否活着 |
|
||||
| `cvg` | `INS.ConvergeFlag`,是否真收敛(不是"是否在跑") |
|
||||
| `imu` | BMI088 是否初始化成功 |
|
||||
| `an` | ≈9.8 正常;≈0 说明没数据 |
|
||||
| `gn` | 原始陀螺模长(**未减零偏**) |
|
||||
| `zo` | 实际减掉的 z 零偏;与 `gz` 量级对比 |
|
||||
| `ck` | `zo` 是否可信(标定时是否静止) |
|
||||
| `se`/`st` | SPI 出错次数 / SPI 状态(1=READY) |
|
||||
|
||||
### 1.2 LED 与蜂鸣器
|
||||
|
||||
灯语见根 [README §10](../README.md#10-上电行为与灯语)。蜂鸣器的"响几下"直接对应掉线设备序号 + 1。
|
||||
|
||||
### 1.3 与参考工程对照
|
||||
|
||||
桌面上留有本项目的参照物,**遇到"应该能跑却跑不起来"时,优先和它们逐行对照**:
|
||||
|
||||
| 参考 | 位置 | 用途 |
|
||||
| --- | --- | --- |
|
||||
| 原始 INS 例程 | `Desktop/新建文件夹/RoboMaster-C-Board-INS-Example-master` | QEKF / kalman_filter 的移植来源;**其 `Heap_Size = 0x2000`** |
|
||||
| legacy 固件 | `Desktop/new-infantry-c-board-legacy - 副本` | StdPeriph 版、自带 `AHRS.lib`;引脚分配、EXTI 触发沿、寄存器序列的权威参照 |
|
||||
| 本工程旧版 | `Desktop/Simba-OmniTest/Simba-ELCT-FrameWork` | 同一框架的历史快照 |
|
||||
|
||||
---
|
||||
|
||||
## 2. 典型案例详解
|
||||
|
||||
### 2.1 三个欧拉角恒为 0.00
|
||||
|
||||
**两个不同原因都会长成这样**,用 `cvg` 和 `an` 区分:
|
||||
|
||||
#### (a) EKF 矩阵内存分配失败 —— `cvg=1`、`an≈9.8`、角度恒 0
|
||||
|
||||
- **现象**:`yaw=pitch=roll=0.00` 且 `cvg=1`(注意 `cvg` 若是 `att_ready` 之类的量就永远看不出来)
|
||||
- **原因**:`kalman_filter.c` 的 `user_malloc` 展开为 **libc `malloc`**(它不 include `cmsis_os.h`),而 libc 堆来自 `startup_stm32f407xx.s` 的 `Heap_Size`。CubeMX 默认 `0x200`(512 B),远小于 `Kalman_Filter_Init(6,0,3)` 需要的约 1.6 KB,且全程无 NULL 检查
|
||||
- **后果**:后半段矩阵 `pData = NULL`,`arm_mat_*` 只读写地址 0 附近、不报错也不改输出 ⇒ 滤波器不再更新状态,但 `UpdateCount` 照常自增 ⇒ 看起来"在跑"、"收敛了"
|
||||
- **处理**:把 `Heap_Size` 改为 `0x2000`(与原始例程一致)。⚠️ 该行由 CubeMX 生成,重新生成工程会被改回
|
||||
|
||||
#### (b) IMU 没起来 —— `cvg=0`、`an≈0`、角度恒 0
|
||||
|
||||
- **现象**:`imu=0` 或 `an≈0`,且 `cvg=0`(卡方检验一直不过)
|
||||
- 加速度全零 → 量测被拒绝 → 四元数停在初值 `[1,0,0,0]` 反解就是三个 0.00
|
||||
- **处理**:查 SPI 通路 / 片选 / 传感器供电;并把 `debug_task` 的 `imu`、`an` 和绿灯灯语一起看
|
||||
|
||||
### 2.2 数据路静默冻结
|
||||
|
||||
- **现象**:`pitch/roll/an/gx/gy/gz` 全部纹丝不动,`ca`/`cg` **停止增长**,而 `imu` 仍是 1;yaw 可能还在匀速漂(积分冻结值)
|
||||
- **两类原因**:
|
||||
1. `hspi1` 卡在 BUSY(SPI/DMA 出错后 HAL 没回到 READY)⇒ `start_dma_read()` 里 `HAL_SPI_GetState` 检查让每次调用直接返回
|
||||
2. BMI088 的 DRDY 是**电平型**信号,某次失败或被挡掉的读没把数据寄存器读走 ⇒ INT 电平保持住 ⇒ **既无下降沿也无上升沿,EXTI 永不触发**
|
||||
- **为什么中断里的兜底没用**:两种情况都会让 EXTI 回调**压根不执行**
|
||||
- **处理**:已实现任务上下文的 `bmi088_health_check()`(由 `attitude_update()` 每 1 ms 调用),计数 5 ms 不涨即重启中断路 + 阻塞读一次,修不好则退化为约 200 Hz 轮询保底
|
||||
- **继续深挖**:看 `st` 与 `se`
|
||||
- `st` 长期非 1 ⇒ SPI 卡死(原因 1)
|
||||
- `st` 恒为 1 但 `ca/cg` 不涨 ⇒ 数据就绪中断丢失(原因 2)
|
||||
|
||||
### 2.3 yaw 匀速自漂
|
||||
|
||||
- **现象**:yaw 以**恒定**角速度漂移,`pitch/roll` 正常;移动板子时陀螺读数"不响应"
|
||||
- **判定算式**:
|
||||
```
|
||||
漂移率(rad/s) = Δyaw / Δt
|
||||
若 漂移率 > gn ⇒ 漂移来自被污染的 zo,不是陀螺
|
||||
实际进滤波器的角速度 = gz - zo
|
||||
```
|
||||
实例:实测 -3.85 °/s = -0.0672 rad/s,而 `gn = 0.017 rad/s` —— 漂移率比整个陀螺模长还大近 4 倍,只可能是假零偏
|
||||
- **原因**:z 轴零偏标定窗口(~1 s)里混入了运动样本。上电瞬间 SPI 路刚起步、且车体常正被摆弄
|
||||
- **处理**:`ins.c` 已加健壮性(稳定等待 + 离群剔除 + 整轮重测 + `ck` 标志);**根治办法是上电后那 1.5 秒别碰车**
|
||||
- 注意 `gn` 是**未减零偏**的原始模长,所以 `gn` 小并不代表进滤波器的值小
|
||||
|
||||
### 2.4 蜂鸣器一直响
|
||||
|
||||
- 这是**正常报警**,不是故障:某台 CAN 设备掉线,`monitor_task` 按 legacy 语义每 2 s 窗内响 `n` 下,`n = 最先掉线的设备序号 + 1`
|
||||
- 云台序号:`0=摩擦轮L 1=摩擦轮R 2=Pitch 3=Yaw 4=拨弹 5=底盘回传`;底盘:`0~3=四轮 4=云台数据`
|
||||
- **响三下 ⇒ 第 3 号设备(云台板的 Pitch 电机 0x206)掉线**
|
||||
- 台架调试嫌吵:把 `robot_param.h` 的 `DEBUG_BUZZER_MELODY` 置 1,警告音换成旋律;**灯语不变,故障信息不丢**。比赛前记得改回 0
|
||||
|
||||
### 2.5 反复重启
|
||||
|
||||
- **现象**:约 3~5 s 复位一次,USB 输出每隔几秒重来
|
||||
- **原因**:IWDG 约 2.5 s 溢出;上电 5 s 内无条件喂狗,之后要求**所有任务**心跳都在 500 ms 内刷新过。因此
|
||||
- 某个任务卡死(死循环 / 阻塞在无超时的等待)
|
||||
- 或者**整个初始化超过 5 s**,`IMU_Task` 还没进主循环、心跳不刷新
|
||||
- **处理**:检查各初始化耗时(传感器重试次数、IMU 零偏标定轮数);`IMU_Task` 在 `attitude_init()` 期间不刷心跳,扩展初始化要算进这 5 s 预算
|
||||
|
||||
### 2.6 工程编译/链接相关的坑(非运行期)
|
||||
|
||||
| 症状 | 原因 | 处理 |
|
||||
| --- | --- | --- |
|
||||
| 链接报找不到 `arm_mat_*` | `Middlewares/ST/ARM/DSP/Lib/*.lib` 丢失 | 该目录是源码依赖,已入库;确认文件在 |
|
||||
| 新加的 `.c` 没被编译 | 没加进 `.uvprojx` | 同步加 `.uvprojx`(和 `.uvoptx`) |
|
||||
| Keil 每次保存工程文件都在变 | `.uvoptx` 的 `<FileNumber>` 断号 | 删除条目后把末尾编号改为连续值 |
|
||||
| 移动目录后编译不过 | `<IncludePath>` 没跟着改 | `.h` 不登记在工程里,靠 IncludePath 定位;移动目录时同步改 |
|
||||
| CubeMX 重新生成后行为变了 | 手改过的生成文件被覆盖 | 复查 `startup_stm32f407xx.s` 的 `Heap_Size` 与 `Core/Src/gpio.c` 的 INT1 触发沿 |
|
||||
|
||||
---
|
||||
|
||||
## 3. 改动本仓库时的检查清单
|
||||
|
||||
改完代码准备提交前,过一遍:
|
||||
|
||||
- [ ] 两棵树都改了?(同构文件需要同步)
|
||||
- [ ] 增删了 `.c`?→ 同步改 `.uvprojx` 与 `.uvoptx`
|
||||
- [ ] 移动了目录?→ 同步改 `.uvprojx`/`.uvoptx` 的文件路径与 `<IncludePath>`
|
||||
- [ ] 工程引用的文件是否都真实存在?磁盘上的 `.c` 是否都被工程引用(没有掉出编译)?
|
||||
- [ ] 动过 `modules/imu/qekf/`?→ 先读 `imu/attitude.c`、`imu/ins.c` 文件头约定
|
||||
- [ ] 姿态相关改动?→ 在 `README §8` 的字段里想清楚**怎么验证**(哪个字段能证明它对了)
|
||||
- [ ] 改了 `robot_param.h` 里带"比赛请改 0"的开关?→ 记得改回来
|
||||
Vendored
+2
-2
@@ -1,8 +1,8 @@
|
||||
{
|
||||
"python.analysis.extraPaths": [
|
||||
"C:\\Users\\LSMushui\\.kendryte\\k230_canmv_stubs\\437a521fe0de963c312edc4cace3c197d58b4a56"
|
||||
"C:\\Users\\LSMushui\\.kendryte\\k230_canmv_stubs\\8f620e570e90363678bd2fb9fdb22f4e16ba8fbf"
|
||||
],
|
||||
"python.analysis.stubPath": "C:\\Users\\LSMushui\\.kendryte\\k230_canmv_pylance\\757cb9fbc0d8cd84\\typings",
|
||||
"python.analysis.stubPath": "C:\\Users\\LSMushui\\.kendryte\\k230_canmv_pylance\\556a1ad2a81d431a\\typings",
|
||||
"python.analysis.diagnosticSeverityOverrides": {
|
||||
"reportMissingModuleSource": "none"
|
||||
}
|
||||
|
||||
@@ -1,8 +1,9 @@
|
||||
/* debug_task.h - USB 虚拟串口诊断任务声明(仅诊断用) */
|
||||
/* debug_task.h - USB 虚拟串口(CDC)欧拉角输出任务声明 */
|
||||
#ifndef DEBUG_TASK_H
|
||||
#define DEBUG_TASK_H
|
||||
|
||||
/* debug_task:USB 虚拟串口(CDC)诊断输出(仅诊断用,稳定后可整组移除) */
|
||||
/* debug_task:USB 虚拟串口(CDC)输出 QuaternionEKF 欧拉角(yaw/pitch/roll,单位度)
|
||||
* 输出格式与字段含义见 debug_task.c 文件头 */
|
||||
extern void debug_task(void const * argument);
|
||||
|
||||
#endif
|
||||
|
||||
@@ -1,82 +1,89 @@
|
||||
/* =============================================================================
|
||||
* debug_task.c - USB 虚拟串口(CDC)诊断输出(仅诊断用,稳定后可整组移除)
|
||||
* 每 500ms 一行状态:各芯片实测 ID/DR16 帧计数/USART 空闲中断计数/任务循环
|
||||
* 计数等;板子插电脑出现虚拟 COM 口,串口助手打开即可(波特率随意)。
|
||||
* 字段含义见重构说明与编译指南.md 的诊断章节
|
||||
* debug_task.c - USB 虚拟串口(CDC)姿态输出(欧拉角 + 姿态链路诊断)
|
||||
* 每 DEBUG_OUT_PERIOD_MS 输出一行。单位: 角度 ° / 加速度 m/s² / 角速度 rad/s
|
||||
*
|
||||
* ---- 欧拉角(QuaternionEKF 解算结果,见 modules/ins) ----
|
||||
* yaw 多圈连续偏航角 (att_deg_yaw = INS.YawTotalAngle,云台 Yaw 环反馈)
|
||||
* pitch 绕 y 轴角 (att_deg_pitch = QEKF_INS.Roll )
|
||||
* roll 绕 x 轴角 (att_deg_roll = QEKF_INS.Pitch,云台 Pitch 环反馈)
|
||||
* QEKF 内部 Pitch/Roll 命名与航空惯例互换,映射与原因见 attitude.c 文件头。
|
||||
* 注意 yaw 无绝对基准(纯陀螺积分),上电静止时恒为该次上电的相对 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
|
||||
* 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 加速度/陀螺采样计数:停止增长即数据路已死(数值冻结时它们不涨)
|
||||
* se bmi088_spi_errors:SPI/DMA 出错次数;st=bmi088_spi_state()
|
||||
* (1=READY,非 1 说明 SPI 被卡住,数据路会因此停摆)
|
||||
*
|
||||
* 开关: DEBUG_ATT_HEALTH=0 只输出三个欧拉角;DEBUG_ATT_DIAG=0 去掉诊断位。
|
||||
* ========================================================================== */
|
||||
#include "debug_task.h"
|
||||
#include "app_tasks.h"
|
||||
#include "cmsis_os.h"
|
||||
#include "FreeRTOS.h"
|
||||
#include "task.h"
|
||||
#include "usb_device.h"
|
||||
#include "usbd_cdc_if.h"
|
||||
#include "usart.h"
|
||||
#include "bsp_can.h"
|
||||
#include "bsp_usart.h"
|
||||
#include <stdio.h>
|
||||
#include "dr16.h"
|
||||
#include "referee.h"
|
||||
#include "c_comm.h"
|
||||
#if defined(ROBOT_TYPE_GIMBAL)
|
||||
#include "bmi088.h"
|
||||
#include "ist8310.h"
|
||||
#include "ins.h"
|
||||
#include "attitude.h"
|
||||
#include "gimbal.h"
|
||||
#elif defined(ROBOT_TYPE_CHASSIS)
|
||||
#include "chassis.h"
|
||||
#include "bmi088.h"
|
||||
#include "ins.h"
|
||||
#include "robot_typedef.h"
|
||||
#endif
|
||||
#include "bmi088.h"
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
|
||||
/* debug_task:USB 虚拟串口(CDC)诊断输出,每 500ms 一行
|
||||
* 板子插上电脑会出现虚拟 COM 口,用串口助手打开即可看到(波特率随意)
|
||||
* 输出为纯整数,用于远程定位传感器/通讯初始化问题 */
|
||||
/* 1-输出健康位(cvg/rdy/imu/an/gn); 0-只输出三个欧拉角 */
|
||||
#define DEBUG_ATT_HEALTH 1
|
||||
/* 1-再附加原始陀螺/零偏/采样计数(排障用); 0-只留上面的通用健康位 */
|
||||
#define DEBUG_ATT_DIAG 1
|
||||
|
||||
/* 各模块导出的诊断变量(定义在 imu_task.c / can_task.c) */
|
||||
extern volatile uint32_t dbg_imu_loop;
|
||||
extern volatile uint32_t dbg_can_loop;
|
||||
/* 输出周期(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 (;;)
|
||||
{
|
||||
int len = 0;
|
||||
#if defined(ROBOT_TYPE_GIMBAL)
|
||||
len = snprintf(line, sizeof(line),
|
||||
"app=%u can=%u imu=%u acc=%02X gyro=%02X bmi=%u ist=%u who=%02X "
|
||||
"cvg=%u ys=%d yaw=%.1f u3g=%02X i3=%u l3=%u d16=%u ref=%u/%u mot=%u\r\n",
|
||||
app_ready, dbg_can_loop, dbg_imu_loop,
|
||||
bmi088_last_acc_id, bmi088_last_gyro_id, bmi088_online,
|
||||
ist8310_online, ist8310_last_who,
|
||||
INS.ConvergeFlag,
|
||||
(int)gimbal_yaw_sign(), att_deg_yaw,
|
||||
(unsigned)(huart3.gState), dbg_usart3_idle, dbg_usart3_last_len,
|
||||
dr16_frame_count,
|
||||
referee.link_status, (unsigned)(HAL_GetTick() - referee.last_update_tick),
|
||||
gimbal_motor_offline());
|
||||
#elif defined(ROBOT_TYPE_CHASSIS)
|
||||
len = snprintf(line, sizeof(line),
|
||||
"app=%u can=%u imu=%u acc=%02X gyro=%02X cvg=%u "
|
||||
"ctoc=%u/%u att=%u/%d mot=%u md=%u err=%.1f fw=%d spin=%u lim=%u\r\n",
|
||||
app_ready, dbg_can_loop, dbg_imu_loop,
|
||||
bmi088_last_acc_id, bmi088_last_gyro_id,
|
||||
INS.ConvergeFlag,
|
||||
ctoc_remote.status,
|
||||
(unsigned)(ctoc_remote.last_tick ? HAL_GetTick() - ctoc_remote.last_tick : 0),
|
||||
ctoc_att_fresh(), (int)ctoc_gimbal_yaw,
|
||||
chassis_motor_offline(),
|
||||
cmd_robot.chassis_mode,
|
||||
dbg_follow_err_deg, (int)dbg_follow_out,
|
||||
ctoc_remote.spin_flag, referee.chassis_power_limit);
|
||||
const float an = sqrtf(bmi088_accel[0] * bmi088_accel[0] +
|
||||
bmi088_accel[1] * bmi088_accel[1] +
|
||||
bmi088_accel[2] * bmi088_accel[2]);
|
||||
const float gn = sqrtf(bmi088_gyro[0] * bmi088_gyro[0] +
|
||||
bmi088_gyro[1] * bmi088_gyro[1] +
|
||||
bmi088_gyro[2] * bmi088_gyro[2]);
|
||||
|
||||
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 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_spi_errors, (unsigned)bmi088_spi_state());
|
||||
#endif
|
||||
|
||||
#if DEBUG_ATT_HEALTH
|
||||
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
|
||||
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)
|
||||
CDC_Transmit_FS((uint8_t *)line, (uint16_t)len);
|
||||
|
||||
osDelay(500);
|
||||
osDelay(DEBUG_OUT_PERIOD_MS);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -8,6 +8,8 @@
|
||||
* 连接音效 = 遥控器上线瞬间两声上扬(M3->H3)
|
||||
* CAN 掉线蜂鸣(legacy 语义): 按设备序号 n 响 n 下,2s 一轮,每次只报
|
||||
* 最先掉线的设备,设备序号表见 linkcheck_first_offline()
|
||||
* 调试模式(DEBUG_BUZZER_MELODY=1,见 robot_param.h): 上述蜂鸣改为循环
|
||||
* 播放旋律,避免台架调试时连续滴滴声;红灯常亮等灯语不受影响
|
||||
* 看门狗: IWDG 约 2.5s;上电 5s 宽限期无条件喂,之后仅当全部任务心跳正常
|
||||
* 才喂——任何任务卡死都会复位,不会整车失控
|
||||
* ========================================================================== */
|
||||
@@ -170,10 +172,24 @@ void LED_Task(void const * argument)
|
||||
if (can_alarm)
|
||||
{
|
||||
led_set_red(1);
|
||||
int8_t id = linkcheck_first_offline();
|
||||
if (id >= 0)
|
||||
|
||||
/* 掉线设备序号(0 起)。旋律模式下不用它的返回值,仍保留调用:
|
||||
* 一是调试时可直接在调试器里看 linkcheck 结果,二是避免该静态
|
||||
* 函数在 DEBUG_BUZZER_MELODY=1 时被判为未使用而报警告 */
|
||||
int8_t first_offline = linkcheck_first_offline();
|
||||
|
||||
#if DEBUG_BUZZER_MELODY
|
||||
/* 调试模式:警告音替换为循环旋律(见 modules/BUZZER/buzzer.h)。
|
||||
* 红灯常亮与其它灯语不变,"哪台设备掉线"这一信息不会丢。
|
||||
* 步进用本任务自身的 100ms 节拍,不靠 HAL_GetTick 采样,避免周期
|
||||
* 抖动漏掉或重复某个音符 */
|
||||
static uint16_t melody_step = 0;
|
||||
(void)first_offline;
|
||||
buzzer_play_melody(melody_step++);
|
||||
#else
|
||||
if (first_offline >= 0)
|
||||
{
|
||||
uint8_t n = (uint8_t)(id + 1);
|
||||
uint8_t n = (uint8_t)(first_offline + 1);
|
||||
uint8_t w = counter % 20; // 2s 窗口
|
||||
/* 第 j 声:占 w=[2j,2j+1],响 100ms 停 100ms */
|
||||
if ((w / 2) < n && (w % 2) == 0)
|
||||
@@ -183,6 +199,7 @@ void LED_Task(void const * argument)
|
||||
}
|
||||
else
|
||||
buzzer_off();
|
||||
#endif
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
@@ -125,6 +125,11 @@
|
||||
#define DEBUG_SHOOT_WITHOUT_REFEREE 1 // 1=无裁判系统时也允许开摩擦轮(台架调试用),比赛请改 0
|
||||
#define GIMBAL_RAMMER_INSTALLED 0 // 1=拨弹 M2006 已安装;0=暂缺,不参与掉线判定
|
||||
|
||||
/* 1=调试模式:把 CAN 掉线警告音替换成循环播放的旋律(台架调试时避免连续滴滴声
|
||||
* 干扰)。红灯常亮与其它灯语完全不变,仍能看出是哪台设备掉线;比赛请改 0。
|
||||
* 旋律音符表见 modules/BUZZER/buzzer.c 的 buzzer_melody[] */
|
||||
#define DEBUG_BUZZER_MELODY 1
|
||||
|
||||
/* ============================ 掉线检测 ============================ */
|
||||
#define DEVICE_OFFLINE_TIME_MS 100 // CAN 设备/裁判系统掉线判定时间
|
||||
#define CTOC_OFFLINE_TIME_MS 100 // 板间通讯掉线判定时间
|
||||
|
||||
+10
-2
@@ -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);
|
||||
|
||||
|
||||
@@ -1215,7 +1215,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\PID\pid.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\algorithm\pid.c</PathWithFileName>
|
||||
<FilenameWithoutPath>pid.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1227,7 +1227,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\filter\filter.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\algorithm\filter.c</PathWithFileName>
|
||||
<FilenameWithoutPath>filter.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1251,7 +1251,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\DR16\dr16.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\comms\dr16.c</PathWithFileName>
|
||||
<FilenameWithoutPath>dr16.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1263,7 +1263,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\referee\referee.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\comms\referee.c</PathWithFileName>
|
||||
<FilenameWithoutPath>referee.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1275,7 +1275,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\LED\led.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\dev\led.c</PathWithFileName>
|
||||
<FilenameWithoutPath>led.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1287,7 +1287,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\BUZZER\buzzer.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\dev\buzzer.c</PathWithFileName>
|
||||
<FilenameWithoutPath>buzzer.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1299,7 +1299,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\c_comm\c_comm.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\comms\c_comm.c</PathWithFileName>
|
||||
<FilenameWithoutPath>c_comm.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1311,7 +1311,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\BMI088\bmi088.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\sensor\bmi088.c</PathWithFileName>
|
||||
<FilenameWithoutPath>bmi088.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1323,7 +1323,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\IST8310\ist8310.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\sensor\ist8310.c</PathWithFileName>
|
||||
<FilenameWithoutPath>ist8310.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1335,7 +1335,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\attitude\attitude.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\imu\attitude.c</PathWithFileName>
|
||||
<FilenameWithoutPath>attitude.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1347,7 +1347,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\ins\kalman_filter.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\imu\qekf\kalman_filter.c</PathWithFileName>
|
||||
<FilenameWithoutPath>kalman_filter.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1359,7 +1359,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\ins\QuaternionEKF.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\imu\qekf\QuaternionEKF.c</PathWithFileName>
|
||||
<FilenameWithoutPath>QuaternionEKF.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1371,7 +1371,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\ins\ins.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\imu\ins.c</PathWithFileName>
|
||||
<FilenameWithoutPath>ins.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1383,7 +1383,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\gimbal\gimbal.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\control\gimbal.c</PathWithFileName>
|
||||
<FilenameWithoutPath>gimbal.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1395,7 +1395,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\shoot\shoot.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\control\shoot.c</PathWithFileName>
|
||||
<FilenameWithoutPath>shoot.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1466,7 +1466,7 @@
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>12</GroupNumber>
|
||||
<FileNumber>100</FileNumber>
|
||||
<FileNumber>98</FileNumber>
|
||||
<FileType>4</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
|
||||
@@ -341,7 +341,7 @@
|
||||
<MiscControls></MiscControls>
|
||||
<Define>USE_HAL_DRIVER,STM32F407xx,ARM_MATH_CM4,ROBOT_TYPE_GIMBAL</Define>
|
||||
<Undefine></Undefine>
|
||||
<IncludePath>../Core/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Drivers/CMSIS/Device/ST/STM32F4xx/Include;../Drivers/CMSIS/Include;../USB_DEVICE/App;../USB_DEVICE/Target;../Middlewares/ST/STM32_USB_Device_Library/Core/Inc;../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc;../Application/Task/;../Bsp/Inc;../Application;../Application/Task/Inc;../modules/PID;../modules/filter;../modules/motor;../modules/DR16;../modules/referee;../modules/LED;../modules/BUZZER;../modules/c_comm;../modules/BMI088;../modules/IST8310;../modules/AHRS;../modules/attitude;../modules/gimbal;../modules/shoot;../modules/ins;../Middlewares/ST/ARM/DSP/Inc</IncludePath>
|
||||
<IncludePath>../Core/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Drivers/CMSIS/Device/ST/STM32F4xx/Include;../Drivers/CMSIS/Include;../USB_DEVICE/App;../USB_DEVICE/Target;../Middlewares/ST/STM32_USB_Device_Library/Core/Inc;../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc;../Application/Task/;../Bsp/Inc;../Application;../Application/Task/Inc;../Middlewares/ST/ARM/DSP/Inc;../modules/algorithm;../modules/imu;../modules/imu/qekf;../modules/sensor;../modules/comms;../modules/motor;../modules/control;../modules/dev</IncludePath>
|
||||
</VariousControls>
|
||||
</Cads>
|
||||
<Aads>
|
||||
@@ -359,7 +359,7 @@
|
||||
<MiscControls></MiscControls>
|
||||
<Define></Define>
|
||||
<Undefine></Undefine>
|
||||
<IncludePath>../Core/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Drivers/CMSIS/Device/ST/STM32F4xx/Include;../Drivers/CMSIS/Include;..\Application\Task\Inc;..\bsp;../Application;../Application/Task/Inc;../modules/PID;../modules/filter;../modules/motor;../modules/DR16;../modules/referee;../modules/LED;../modules/BUZZER;../modules/c_comm;../modules/BMI088;../modules/IST8310;../modules/AHRS;../modules/attitude;../modules/gimbal;../modules/shoot;../modules/ins;../Middlewares/ST/ARM/DSP/Inc</IncludePath>
|
||||
<IncludePath>../Core/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Drivers/CMSIS/Device/ST/STM32F4xx/Include;../Drivers/CMSIS/Include;..\Application\Task\Inc;../Application;../Application/Task/Inc;../Middlewares/ST/ARM/DSP/Inc;../modules/algorithm;../modules/imu;../modules/imu/qekf;../modules/sensor;../modules/comms;../modules/motor;../modules/control;../modules/dev</IncludePath>
|
||||
</VariousControls>
|
||||
</Aads>
|
||||
<LDads>
|
||||
@@ -2402,12 +2402,12 @@
|
||||
<File>
|
||||
<FileName>pid.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\PID\pid.c</FilePath>
|
||||
<FilePath>..\modules\algorithm\pid.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>filter.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\filter\filter.c</FilePath>
|
||||
<FilePath>..\modules\algorithm\filter.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>dji_motor.c</FileName>
|
||||
@@ -2417,67 +2417,67 @@
|
||||
<File>
|
||||
<FileName>dr16.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\DR16\dr16.c</FilePath>
|
||||
<FilePath>..\modules\comms\dr16.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>referee.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\referee\referee.c</FilePath>
|
||||
<FilePath>..\modules\comms\referee.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>led.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\LED\led.c</FilePath>
|
||||
<FilePath>..\modules\dev\led.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>buzzer.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\BUZZER\buzzer.c</FilePath>
|
||||
<FilePath>..\modules\dev\buzzer.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>c_comm.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\c_comm\c_comm.c</FilePath>
|
||||
<FilePath>..\modules\comms\c_comm.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>bmi088.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\BMI088\bmi088.c</FilePath>
|
||||
<FilePath>..\modules\sensor\bmi088.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>ist8310.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\IST8310\ist8310.c</FilePath>
|
||||
<FilePath>..\modules\sensor\ist8310.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>attitude.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\attitude\attitude.c</FilePath>
|
||||
<FilePath>..\modules\imu\attitude.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>kalman_filter.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\ins\kalman_filter.c</FilePath>
|
||||
<FilePath>..\modules\imu\qekf\kalman_filter.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>QuaternionEKF.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\ins\QuaternionEKF.c</FilePath>
|
||||
<FilePath>..\modules\imu\qekf\QuaternionEKF.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>ins.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\ins\ins.c</FilePath>
|
||||
<FilePath>..\modules\imu\ins.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>gimbal.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\gimbal\gimbal.c</FilePath>
|
||||
<FilePath>..\modules\control\gimbal.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>shoot.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\shoot\shoot.c</FilePath>
|
||||
<FilePath>..\modules\control\shoot.c</FilePath>
|
||||
</File>
|
||||
</Files>
|
||||
</Group>
|
||||
|
||||
@@ -40,7 +40,14 @@ __initial_sp
|
||||
; <o> Heap Size (in Bytes) <0x0-0xFFFFFFFF:8>
|
||||
; </h>
|
||||
|
||||
Heap_Size EQU 0x200
|
||||
; Heap raised from 0x200 (512B) to 0x2000 (8KB):
|
||||
; modules/ins/kalman_filter.c allocates all KF matrix storage via libc
|
||||
; malloc (it does not include cmsis_os.h, so user_malloc expands to
|
||||
; malloc, not pvPortMalloc). Kalman_Filter_Init(6,0,3) issues 25
|
||||
; allocations totalling about 1.6KB with no NULL checks; a 512B heap
|
||||
; made the later ones return NULL and silently broke the filter.
|
||||
; 8KB matches the original RoboMaster-C-Board-INS-Example value.
|
||||
Heap_Size EQU 0x2000
|
||||
|
||||
AREA HEAP, NOINIT, READWRITE, ALIGN=3
|
||||
__heap_base
|
||||
|
||||
Binary file not shown.
+40
-114
@@ -1,133 +1,59 @@
|
||||
# Simba-ELCT-FrameWork
|
||||
# 云台板固件(Simba-ELCT-FrameWork)
|
||||
|
||||
> 步兵机器人**云台板**工程 · STM32F407IGH6 · Keil + FreeRTOS + CMSIS-DSP
|
||||
|
||||
<table>
|
||||
<tr>
|
||||
<td><img src="https://git.simba-robotics.com/assets/img/logo.svg" width = "200" height = "200" alt="Simba-git" /></td>
|
||||
<td><img src="https://git.simba-robotics.com/avatars/cd8e5bc7a1039e1cbadabefe03e647af1bbbd8722d659d437479b93c8c6fa799?" width = "200" height = "200" alt="ELEC" /></td>
|
||||
</tr>
|
||||
</table>
|
||||
项目总览、代码地图、任务表、调试与注意事项请先读 **[根 README](../README.md)** 与 **[已知问题与排障](../doc/已知问题与排障.md)**。本文只讲这块板特有的东西。
|
||||
|
||||
[](https://gitee.com/LSMushui)
|
||||
|
||||
|
||||
---
|
||||
|
||||
## 这块板负责什么
|
||||
|
||||
## 简介
|
||||
- **云台控制**:Yaw(GM6020 `0x205`,CAN2)/ Pitch(GM6020 `0x206`,CAN1)双环
|
||||
- **发射机构**:摩擦轮 M3508 `0x201`/`0x202` + 拨弹 M2006 `0x207`(装与不装由 `GIMBAL_RAMMER_INSTALLED` 决定)
|
||||
- **图传链路 / 裁判系统**:USART6(3pin 口)
|
||||
- **DR16 遥控器**:USART3
|
||||
- **板间通讯主控**:云台把世界系 yaw(`0x1A9`)与摇杆/控制状态(`0x149`/`0x189`)发给底盘
|
||||
- **视觉上位机接口**:USART1(4pin 口),收发 DMA 已就绪,协议待接入
|
||||
|
||||
> Simba自己的C板电控框架！
|
||||
## 编译宏
|
||||
|
||||
本项目的计划是基于 **DJI C型开发板** 的基础上打造一个更适合Simba同学们的代码框架
|
||||
```
|
||||
USE_HAL_DRIVER, STM32F407xx, ARM_MATH_CM4, ROBOT_TYPE_GIMBAL
|
||||
```
|
||||
|
||||
本框架致力于实现不同类型的机器人的代码通用化，只需要选择底盘云台的类型，修改一下对应物理参数即可实现对机器人的适配
|
||||
## 相对底盘板多出来的东西
|
||||
|
||||
## 配套套件
|
||||
| 项 | 位置 |
|
||||
| --- | --- |
|
||||
| `gimbal` 云台双环 + 装配符号自检 | `modules/control/gimbal.c` |
|
||||
| `shoot` 摩擦轮/拨弹 | `modules/control/shoot.c` |
|
||||
| `gimbal_task` / `shoot_task` | `Application/Task/Src/` |
|
||||
| DR16 遥控器、视觉串口 | `Bsp/bsp_usart.c` 的 `usart3_init` / `usart1_*` |
|
||||
| 板间帧 0x149 / 0x189 / 0x1A9 的**发送**侧 | `modules/comms/c_comm.c` |
|
||||
| IST8310 磁力计与本板 IMU 恒温 | `modules/imu/attitude.c`(双板都有,云台用于云台控制) |
|
||||
|
||||
> 上位机，ROS接口待开发
|
||||
底盘板有的 `control/chassis.c` 与 `chassis_task` 本工程没有(所以两棵树的 `modules/control/` 内容不同,属预期差异)。
|
||||
|
||||
## 模块支持
|
||||
## 上电时本板特有的行为
|
||||
|
||||
详细信息请参考 [Simba-ELCT-FrameWork文档](./doc/framework.md)
|
||||
1. `led_init` 点亮蓝灯 → 各外设初始化
|
||||
2. IMU 初始化 + **z 轴零偏静止标定(约 0.5~1.5 s,这段时间别碰车)**
|
||||
3. **Yaw 装配符号自检**:给 Yaw 电机一个正向小电压脉冲,看陀螺 z 轴往哪边转,自动定符号;**云台会轻转几度,不要扶住云台**。失败(被按住 / 电机离线)时回退到 `GIMBAL_YAW_SIGN_FALLBACK`
|
||||
4. 自检结果经 `0x1A9` 的 flags 下发给底盘,与底盘自测的车身转向相乘完成跟随符号综合
|
||||
|
||||
### 主要模块
|
||||
## 蜂鸣器序号(本板)
|
||||
|
||||
- **Robot cmd**\
|
||||
机器人控制模块：
|
||||
- [ ] 大疆电机
|
||||
- [ ] 达妙电机
|
||||
- [ ] 瓴控电机
|
||||
- [ ] 小米电机
|
||||
- [ ] 超级电容控制板
|
||||
- [ ] 板间通信(CAN)
|
||||
- [ ] PWM 控制舵机
|
||||
- [ ] PWM 控制气泵
|
||||
- **Chassis**\
|
||||
底盘模块：
|
||||
- [ ] 麦轮底盘
|
||||
- [ ] 全向轮底盘
|
||||
- [ ] 舵轮底盘
|
||||
- [ ] 平衡底盘
|
||||
- **Gimbal**\
|
||||
云台模块：
|
||||
- [ ] yaw-pitch 直连云台
|
||||
- [ ] yaw直连-pitch丝杆 云台
|
||||
- [ ] yaw-pitch 丝杆云台
|
||||
- **Shoot**\
|
||||
发射机构模块：
|
||||
- [ ] 摩擦轮+拨弹盘
|
||||
- [ ] 摩擦轮+弹鼓+推杆
|
||||
- **Communication**\
|
||||
通信模块：
|
||||
- [ ] 板间通信(CAN2)
|
||||
- [ ] 上下位机通信
|
||||
- **Music**\
|
||||
音乐模块：
|
||||
- [ ] 开机声
|
||||
- [ ] 报错音
|
||||
- [ ] 裁判系统接入提示音
|
||||
- [ ] 《卡农》
|
||||
- [ ] 《天空之城》
|
||||
- [ ] 《恭喜发财》
|
||||
- [ ] 《好运来》
|
||||
- [ ] 《See You Again》
|
||||
- [ ] 《Unity》
|
||||
- [ ] 《你》
|
||||
CAN 掉线报警按设备序号响 `n` 下:
|
||||
|
||||
## 开发工具
|
||||
```
|
||||
0 = 摩擦轮L(0x201) 1 = 摩擦轮R(0x202) 2 = Pitch(0x206)
|
||||
3 = Yaw(0x205) 4 = 拨弹(0x207) 5 = 底盘回传(0x019)
|
||||
```
|
||||
|
||||
本项目使用HAL库编写，通过CubeMX生成项目，使用FreeRTOS
|
||||
**响三下 = Pitch 电机掉线。** 台架调试嫌吵时把 `robot_param.h` 的 `DEBUG_BUZZER_MELODY` 置 1(只换声音不换灯语)。
|
||||
|
||||
使用VSCode作为代码编辑器，Keil5作为IDE
|
||||
## 构建
|
||||
|
||||
Keil5编译器使用AC6版本
|
||||
> 虽然keil5老，但在嵌入式调试以及环境配置方面是非常简单易上手的。**千万不要用keil5来编辑代码！！！**
|
||||
\
|
||||
\
|
||||
> VSCode作为现代化代码编辑器 ，通过各种插件即可实现各种提高效率的开发方式，**尤其是Github Copilot**
|
||||
\
|
||||
\
|
||||
> AC6编译器相比古早AC5编译器效率更高，AC5编译空项目就要足足1min47s，非常不中，相比之下AC6只需要6s，遥遥领先
|
||||
\
|
||||
\
|
||||
> 但是CubeMX开启FreeRTOS无法直接使用AC6编译器
|
||||
\
|
||||
> 强烈建议通过此[教程](https://bbs.robomaster.com/article/18268)(From COD战队GrassFan Wang)配置好自己的环境
|
||||
用 Keil 打开 `MDK-ARM/Simba-ELCT-FrameWork.uvprojx`,确认 Define 里是 `ROBOT_TYPE_GIMBAL`,Rebuild → Download。
|
||||
|
||||
|
||||
|
||||
|
||||
## 欢迎贡献
|
||||
|
||||
如果你发现了本项目中的问题和可优化的点，可用创建issue进行讨论
|
||||
|
||||
并且fork本项目后提交pr来贡献你的代码
|
||||
|
||||
我们的管理员审核后会将你的代码合并进来
|
||||
|
||||
> ***贡献代码时请参照 [注意事项](./doc/注意事项.md/#贡献代码) 中的贡献代码部分。***
|
||||
>
|
||||
> 编写代码时参考[API](./doc/API.md)
|
||||
|
||||
## 开发日志
|
||||
|
||||
[LOG.md](./doc/LOG.md) 记录了大家在开发过程中的各种奇思妙想和经验。多翻一翻或许能发现惊喜哦。大家有什么想法也都可以往里面写
|
||||
|
||||
[API.md](./doc/API.md) 为开发过程中所使用的模块接口标准，若在编写代码时发现代码与标准冲突，基于本标准修改代码
|
||||
|
||||
## 后续计划
|
||||
|
||||
具体要做的事情在[TODO](./doc/TODO.md)中
|
||||
|
||||
## 致谢
|
||||
|
||||
感谢各个战队的代码开源，所有本框架涉及到的主要的参考资料位于[reference.md](./doc/reference.md)
|
||||
|
||||
## 附录
|
||||
|
||||
本文的详细补充内容写在 [附录](./doc/appendix.md) 中
|
||||
|
||||
如发现本项目中的问题，请添加至[问题列表](./doc/questions.md)并与相关负责人联系，如果有解决方案可以提交pull request
|
||||
|
||||
## 通讯信息
|
||||
|
||||
- 作者 `连山木水` 微信：18088745066 QQ：1194309310
|
||||
> ⚠️ 本工程与底盘工程源码高度同构,改一处通常要同步改另一处。动手前请读根 README 的 [§11 已知陷阱](../README.md#11-已知陷阱与注意事项)。
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -12,11 +12,10 @@
|
||||
* 底盘误判沿会误开小陀螺,勿再使用)
|
||||
* Data[6] bit0=键1 bit1=键2 bit2=小陀螺使能(本工程扩展)
|
||||
* bit7=云台电机故障(本工程扩展)
|
||||
* 0x1A9 云台->底盘:Data[0..3]=云台世界系 yaw(float,LE) Data[4]=att_ready
|
||||
* (双 INS 跟随方案的数据源,2026-09 新增)
|
||||
* 0x1A9 云台->底盘:Data[0..3]=云台世界系 yaw(float,LE)
|
||||
* Data[4] flags:bit0=att_ready bit1=编码器tick增向为CCW
|
||||
* bit2=该位有效(自检完成) [跟随符号自动综合的数据源]
|
||||
* bit2=该位有效(自检完成)
|
||||
* (双 INS 跟随方案 + 跟随符号自动综合的数据源,2026-09)
|
||||
* 0x019 底盘->云台:Data[0]发射机构上电状态 Data[1] bit0=底盘电机故障(本工程扩展)
|
||||
*
|
||||
* 故障位语义:任一侧电机掉线即置位,对侧收到后必须停止自己的输出(整车安全) */
|
||||
@@ -42,3 +42,23 @@ void buzzer_off(void)
|
||||
{
|
||||
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 0);
|
||||
}
|
||||
|
||||
/* ==========================================================================
|
||||
* 调试用旋律(见 buzzer.h):每步 100ms,空拍 BUZZER_P 为静音。
|
||||
* 无源蜂鸣器只有单音,这里用"上行分解和弦 + 回落"拼成一句短旋律;选音限定
|
||||
* 在中音 3 ~ 高音 2,避免刺耳。循环播放不会漏掉"有设备掉线"这一事实,
|
||||
* 因为红灯常亮由 monitor_task 独立控制,与该旋律无关。
|
||||
* 改旋律:直接增删这里的音符即可,长度会自动生效。
|
||||
* ========================================================================== */
|
||||
static const buzzer_tone_e buzzer_melody[] = {
|
||||
BUZZER_M3, BUZZER_M5, BUZZER_M6, BUZZER_H1, BUZZER_P,
|
||||
BUZZER_M6, BUZZER_H1, BUZZER_H2, BUZZER_P,
|
||||
BUZZER_H1, BUZZER_M6, BUZZER_M5, BUZZER_M3, BUZZER_P,
|
||||
};
|
||||
|
||||
void buzzer_play_melody(uint16_t step)
|
||||
{
|
||||
const uint16_t steps =
|
||||
(uint16_t)(sizeof(buzzer_melody) / sizeof(buzzer_melody[0]));
|
||||
buzzer_on(buzzer_melody[step % steps]);
|
||||
}
|
||||
@@ -16,4 +16,10 @@ typedef enum
|
||||
extern void buzzer_init(void);
|
||||
extern void buzzer_on(buzzer_tone_e tone);
|
||||
extern void buzzer_off(void);
|
||||
|
||||
/* ---- 调试用旋律 ----
|
||||
* 按固定 100ms/步 推进的短旋律(音符表见 buzzer.c),用于把掉线警告音换成
|
||||
* 音乐。step 由调用方每 100ms 递增 1,内部对旋律总步数取模自动循环;
|
||||
* 旋律中的空拍 BUZZER_P 由 buzzer_on() 解释为静音,用来断音。 */
|
||||
extern void buzzer_play_melody(uint16_t step);
|
||||
#endif
|
||||
@@ -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 环直接使用
|
||||
@@ -16,11 +16,33 @@
|
||||
#include "main.h"
|
||||
#include <string.h>
|
||||
|
||||
/* 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;
|
||||
@@ -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);
|
||||
@@ -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)
|
||||
@@ -303,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++;
|
||||
}
|
||||
@@ -15,6 +15,22 @@ 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;
|
||||
|
||||
/* 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);
|
||||
|
||||
@@ -1,8 +1,9 @@
|
||||
/* debug_task.h - USB 虚拟串口诊断任务声明(仅诊断用) */
|
||||
/* debug_task.h - USB 虚拟串口(CDC)欧拉角输出任务声明 */
|
||||
#ifndef DEBUG_TASK_H
|
||||
#define DEBUG_TASK_H
|
||||
|
||||
/* debug_task:USB 虚拟串口(CDC)诊断输出(仅诊断用,稳定后可整组移除) */
|
||||
/* debug_task:USB 虚拟串口(CDC)输出 QuaternionEKF 欧拉角(yaw/pitch/roll,单位度)
|
||||
* 输出格式与字段含义见 debug_task.c 文件头 */
|
||||
extern void debug_task(void const * argument);
|
||||
|
||||
#endif
|
||||
|
||||
@@ -1,82 +1,89 @@
|
||||
/* =============================================================================
|
||||
* debug_task.c - USB 虚拟串口(CDC)诊断输出(仅诊断用,稳定后可整组移除)
|
||||
* 每 500ms 一行状态:各芯片实测 ID/DR16 帧计数/USART 空闲中断计数/任务循环
|
||||
* 计数等;板子插电脑出现虚拟 COM 口,串口助手打开即可(波特率随意)。
|
||||
* 字段含义见重构说明与编译指南.md 的诊断章节
|
||||
* debug_task.c - USB 虚拟串口(CDC)姿态输出(欧拉角 + 姿态链路诊断)
|
||||
* 每 DEBUG_OUT_PERIOD_MS 输出一行。单位: 角度 ° / 加速度 m/s² / 角速度 rad/s
|
||||
*
|
||||
* ---- 欧拉角(QuaternionEKF 解算结果,见 modules/ins) ----
|
||||
* yaw 多圈连续偏航角 (att_deg_yaw = INS.YawTotalAngle,云台 Yaw 环反馈)
|
||||
* pitch 绕 y 轴角 (att_deg_pitch = QEKF_INS.Roll )
|
||||
* roll 绕 x 轴角 (att_deg_roll = QEKF_INS.Pitch,云台 Pitch 环反馈)
|
||||
* QEKF 内部 Pitch/Roll 命名与航空惯例互换,映射与原因见 attitude.c 文件头。
|
||||
* 注意 yaw 无绝对基准(纯陀螺积分),上电静止时恒为该次上电的相对 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
|
||||
* 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 加速度/陀螺采样计数:停止增长即数据路已死(数值冻结时它们不涨)
|
||||
* se bmi088_spi_errors:SPI/DMA 出错次数;st=bmi088_spi_state()
|
||||
* (1=READY,非 1 说明 SPI 被卡住,数据路会因此停摆)
|
||||
*
|
||||
* 开关: DEBUG_ATT_HEALTH=0 只输出三个欧拉角;DEBUG_ATT_DIAG=0 去掉诊断位。
|
||||
* ========================================================================== */
|
||||
#include "debug_task.h"
|
||||
#include "app_tasks.h"
|
||||
#include "cmsis_os.h"
|
||||
#include "FreeRTOS.h"
|
||||
#include "task.h"
|
||||
#include "usb_device.h"
|
||||
#include "usbd_cdc_if.h"
|
||||
#include "usart.h"
|
||||
#include "bsp_can.h"
|
||||
#include "bsp_usart.h"
|
||||
#include <stdio.h>
|
||||
#include "dr16.h"
|
||||
#include "referee.h"
|
||||
#include "c_comm.h"
|
||||
#if defined(ROBOT_TYPE_GIMBAL)
|
||||
#include "bmi088.h"
|
||||
#include "ist8310.h"
|
||||
#include "ins.h"
|
||||
#include "attitude.h"
|
||||
#include "gimbal.h"
|
||||
#elif defined(ROBOT_TYPE_CHASSIS)
|
||||
#include "chassis.h"
|
||||
#include "bmi088.h"
|
||||
#include "ins.h"
|
||||
#include "robot_typedef.h"
|
||||
#endif
|
||||
#include "bmi088.h"
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
|
||||
/* debug_task:USB 虚拟串口(CDC)诊断输出,每 500ms 一行
|
||||
* 板子插上电脑会出现虚拟 COM 口,用串口助手打开即可看到(波特率随意)
|
||||
* 输出为纯整数,用于远程定位传感器/通讯初始化问题 */
|
||||
/* 1-输出健康位(cvg/rdy/imu/an/gn); 0-只输出三个欧拉角 */
|
||||
#define DEBUG_ATT_HEALTH 1
|
||||
/* 1-再附加原始陀螺/零偏/采样计数(排障用); 0-只留上面的通用健康位 */
|
||||
#define DEBUG_ATT_DIAG 1
|
||||
|
||||
/* 各模块导出的诊断变量(定义在 imu_task.c / can_task.c) */
|
||||
extern volatile uint32_t dbg_imu_loop;
|
||||
extern volatile uint32_t dbg_can_loop;
|
||||
/* 输出周期(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 (;;)
|
||||
{
|
||||
int len = 0;
|
||||
#if defined(ROBOT_TYPE_GIMBAL)
|
||||
len = snprintf(line, sizeof(line),
|
||||
"app=%u can=%u imu=%u acc=%02X gyro=%02X bmi=%u ist=%u who=%02X "
|
||||
"cvg=%u ys=%d yaw=%.1f u3g=%02X i3=%u l3=%u d16=%u ref=%u/%u mot=%u\r\n",
|
||||
app_ready, dbg_can_loop, dbg_imu_loop,
|
||||
bmi088_last_acc_id, bmi088_last_gyro_id, bmi088_online,
|
||||
ist8310_online, ist8310_last_who,
|
||||
INS.ConvergeFlag,
|
||||
(int)gimbal_yaw_sign(), att_deg_yaw,
|
||||
(unsigned)(huart3.gState), dbg_usart3_idle, dbg_usart3_last_len,
|
||||
dr16_frame_count,
|
||||
referee.link_status, (unsigned)(HAL_GetTick() - referee.last_update_tick),
|
||||
gimbal_motor_offline());
|
||||
#elif defined(ROBOT_TYPE_CHASSIS)
|
||||
len = snprintf(line, sizeof(line),
|
||||
"app=%u can=%u imu=%u acc=%02X gyro=%02X cvg=%u "
|
||||
"ctoc=%u/%u att=%u/%d mot=%u md=%u err=%.1f fw=%d spin=%u lim=%u\r\n",
|
||||
app_ready, dbg_can_loop, dbg_imu_loop,
|
||||
bmi088_last_acc_id, bmi088_last_gyro_id,
|
||||
INS.ConvergeFlag,
|
||||
ctoc_remote.status,
|
||||
(unsigned)(ctoc_remote.last_tick ? HAL_GetTick() - ctoc_remote.last_tick : 0),
|
||||
ctoc_att_fresh(), (int)ctoc_gimbal_yaw,
|
||||
chassis_motor_offline(),
|
||||
cmd_robot.chassis_mode,
|
||||
dbg_follow_err_deg, (int)dbg_follow_out,
|
||||
ctoc_remote.spin_flag, referee.chassis_power_limit);
|
||||
const float an = sqrtf(bmi088_accel[0] * bmi088_accel[0] +
|
||||
bmi088_accel[1] * bmi088_accel[1] +
|
||||
bmi088_accel[2] * bmi088_accel[2]);
|
||||
const float gn = sqrtf(bmi088_gyro[0] * bmi088_gyro[0] +
|
||||
bmi088_gyro[1] * bmi088_gyro[1] +
|
||||
bmi088_gyro[2] * bmi088_gyro[2]);
|
||||
|
||||
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 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_spi_errors, (unsigned)bmi088_spi_state());
|
||||
#endif
|
||||
|
||||
#if DEBUG_ATT_HEALTH
|
||||
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
|
||||
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)
|
||||
CDC_Transmit_FS((uint8_t *)line, (uint16_t)len);
|
||||
|
||||
osDelay(500);
|
||||
osDelay(DEBUG_OUT_PERIOD_MS);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -8,6 +8,8 @@
|
||||
* 连接音效 = 遥控器上线瞬间两声上扬(M3->H3)
|
||||
* CAN 掉线蜂鸣(legacy 语义): 按设备序号 n 响 n 下,2s 一轮,每次只报
|
||||
* 最先掉线的设备,设备序号表见 linkcheck_first_offline()
|
||||
* 调试模式(DEBUG_BUZZER_MELODY=1,见 robot_param.h): 上述蜂鸣改为循环
|
||||
* 播放旋律,避免台架调试时连续滴滴声;红灯常亮等灯语不受影响
|
||||
* 看门狗: IWDG 约 2.5s;上电 5s 宽限期无条件喂,之后仅当全部任务心跳正常
|
||||
* 才喂——任何任务卡死都会复位,不会整车失控
|
||||
* ========================================================================== */
|
||||
@@ -170,10 +172,24 @@ void LED_Task(void const * argument)
|
||||
if (can_alarm)
|
||||
{
|
||||
led_set_red(1);
|
||||
int8_t id = linkcheck_first_offline();
|
||||
if (id >= 0)
|
||||
|
||||
/* 掉线设备序号(0 起)。旋律模式下不用它的返回值,仍保留调用:
|
||||
* 一是调试时可直接在调试器里看 linkcheck 结果,二是避免该静态
|
||||
* 函数在 DEBUG_BUZZER_MELODY=1 时被判为未使用而报警告 */
|
||||
int8_t first_offline = linkcheck_first_offline();
|
||||
|
||||
#if DEBUG_BUZZER_MELODY
|
||||
/* 调试模式:警告音替换为循环旋律(见 modules/BUZZER/buzzer.h)。
|
||||
* 红灯常亮与其它灯语不变,"哪台设备掉线"这一信息不会丢。
|
||||
* 步进用本任务自身的 100ms 节拍,不靠 HAL_GetTick 采样,避免周期
|
||||
* 抖动漏掉或重复某个音符 */
|
||||
static uint16_t melody_step = 0;
|
||||
(void)first_offline;
|
||||
buzzer_play_melody(melody_step++);
|
||||
#else
|
||||
if (first_offline >= 0)
|
||||
{
|
||||
uint8_t n = (uint8_t)(id + 1);
|
||||
uint8_t n = (uint8_t)(first_offline + 1);
|
||||
uint8_t w = counter % 20; // 2s 窗口
|
||||
/* 第 j 声:占 w=[2j,2j+1],响 100ms 停 100ms */
|
||||
if ((w / 2) < n && (w % 2) == 0)
|
||||
@@ -183,6 +199,7 @@ void LED_Task(void const * argument)
|
||||
}
|
||||
else
|
||||
buzzer_off();
|
||||
#endif
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
@@ -125,6 +125,11 @@
|
||||
#define DEBUG_SHOOT_WITHOUT_REFEREE 1 // 1=无裁判系统时也允许开摩擦轮(台架调试用),比赛请改 0
|
||||
#define GIMBAL_RAMMER_INSTALLED 0 // 1=拨弹 M2006 已安装;0=暂缺,不参与掉线判定
|
||||
|
||||
/* 1=调试模式:把 CAN 掉线警告音替换成循环播放的旋律(台架调试时避免连续滴滴声
|
||||
* 干扰)。红灯常亮与其它灯语完全不变,仍能看出是哪台设备掉线;比赛请改 0。
|
||||
* 旋律音符表见 modules/BUZZER/buzzer.c 的 buzzer_melody[] */
|
||||
#define DEBUG_BUZZER_MELODY 1
|
||||
|
||||
/* ============================ 掉线检测 ============================ */
|
||||
#define DEVICE_OFFLINE_TIME_MS 100 // CAN 设备/裁判系统掉线判定时间
|
||||
#define CTOC_OFFLINE_TIME_MS 100 // 板间通讯掉线判定时间
|
||||
|
||||
+10
-2
@@ -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);
|
||||
|
||||
|
||||
@@ -1186,7 +1186,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\PID\pid.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\algorithm\pid.c</PathWithFileName>
|
||||
<FilenameWithoutPath>pid.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1198,7 +1198,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\filter\filter.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\algorithm\filter.c</PathWithFileName>
|
||||
<FilenameWithoutPath>filter.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1222,7 +1222,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\DR16\dr16.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\comms\dr16.c</PathWithFileName>
|
||||
<FilenameWithoutPath>dr16.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1234,7 +1234,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\referee\referee.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\comms\referee.c</PathWithFileName>
|
||||
<FilenameWithoutPath>referee.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1246,7 +1246,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\LED\led.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\dev\led.c</PathWithFileName>
|
||||
<FilenameWithoutPath>led.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1258,7 +1258,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\BUZZER\buzzer.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\dev\buzzer.c</PathWithFileName>
|
||||
<FilenameWithoutPath>buzzer.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1270,7 +1270,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\c_comm\c_comm.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\comms\c_comm.c</PathWithFileName>
|
||||
<FilenameWithoutPath>c_comm.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1282,7 +1282,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\chassis\chassis.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\control\chassis.c</PathWithFileName>
|
||||
<FilenameWithoutPath>chassis.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1294,7 +1294,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\BMI088\bmi088.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\sensor\bmi088.c</PathWithFileName>
|
||||
<FilenameWithoutPath>bmi088.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1306,7 +1306,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\IST8310\ist8310.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\sensor\ist8310.c</PathWithFileName>
|
||||
<FilenameWithoutPath>ist8310.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1318,7 +1318,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\attitude\attitude.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\imu\attitude.c</PathWithFileName>
|
||||
<FilenameWithoutPath>attitude.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1330,7 +1330,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\ins\kalman_filter.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\imu\qekf\kalman_filter.c</PathWithFileName>
|
||||
<FilenameWithoutPath>kalman_filter.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1342,7 +1342,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\ins\QuaternionEKF.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\imu\qekf\QuaternionEKF.c</PathWithFileName>
|
||||
<FilenameWithoutPath>QuaternionEKF.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1354,7 +1354,7 @@
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>..\modules\ins\ins.c</PathWithFileName>
|
||||
<PathWithFileName>..\modules\imu\ins.c</PathWithFileName>
|
||||
<FilenameWithoutPath>ins.c</FilenameWithoutPath>
|
||||
<RteFlg>0</RteFlg>
|
||||
<bShared>0</bShared>
|
||||
@@ -1419,13 +1419,13 @@
|
||||
|
||||
<Group>
|
||||
<GroupName>Middlewares/Library/DSP Library/DSP Library</GroupName>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<RteFlg>0</RteFlg>
|
||||
<File>
|
||||
<GroupNumber>12</GroupNumber>
|
||||
<FileNumber>97</FileNumber>
|
||||
<FileNumber>95</FileNumber>
|
||||
<FileType>4</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
|
||||
@@ -341,7 +341,7 @@
|
||||
<MiscControls></MiscControls>
|
||||
<Define>USE_HAL_DRIVER,STM32F407xx,ARM_MATH_CM4,ROBOT_TYPE_CHASSIS</Define>
|
||||
<Undefine></Undefine>
|
||||
<IncludePath>../Core/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Drivers/CMSIS/Device/ST/STM32F4xx/Include;../Drivers/CMSIS/Include;../USB_DEVICE/App;../USB_DEVICE/Target;../Middlewares/ST/STM32_USB_Device_Library/Core/Inc;../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc;../Application/Task/;../Bsp/Inc;../Application;../Application/Task/Inc;../modules/PID;../modules/filter;../modules/motor;../modules/DR16;../modules/referee;../modules/LED;../modules/BUZZER;../modules/c_comm;../modules/chassis;../modules/BMI088;../modules/IST8310;../modules/attitude;../modules/ins;../Middlewares/ST/ARM/DSP/Inc</IncludePath>
|
||||
<IncludePath>../Core/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Drivers/CMSIS/Device/ST/STM32F4xx/Include;../Drivers/CMSIS/Include;../USB_DEVICE/App;../USB_DEVICE/Target;../Middlewares/ST/STM32_USB_Device_Library/Core/Inc;../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc;../Application/Task/;../Bsp/Inc;../Application;../Application/Task/Inc;../Middlewares/ST/ARM/DSP/Inc;../modules/algorithm;../modules/imu;../modules/imu/qekf;../modules/sensor;../modules/comms;../modules/motor;../modules/control;../modules/dev</IncludePath>
|
||||
</VariousControls>
|
||||
</Cads>
|
||||
<Aads>
|
||||
@@ -359,7 +359,7 @@
|
||||
<MiscControls></MiscControls>
|
||||
<Define></Define>
|
||||
<Undefine></Undefine>
|
||||
<IncludePath>../Core/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Drivers/CMSIS/Device/ST/STM32F4xx/Include;../Drivers/CMSIS/Include;..\Application\Task\Inc;..\bsp;../Application;../Application/Task/Inc;../modules/PID;../modules/filter;../modules/motor;../modules/DR16;../modules/referee;../modules/LED;../modules/BUZZER;../modules/c_comm;../modules/chassis;../modules/BMI088;../modules/IST8310;../modules/attitude;../modules/ins;../Middlewares/ST/ARM/DSP/Inc</IncludePath>
|
||||
<IncludePath>../Core/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Drivers/CMSIS/Device/ST/STM32F4xx/Include;../Drivers/CMSIS/Include;..\Application\Task\Inc;../Application;../Application/Task/Inc;../Middlewares/ST/ARM/DSP/Inc;../modules/algorithm;../modules/imu;../modules/imu/qekf;../modules/sensor;../modules/comms;../modules/motor;../modules/control;../modules/dev</IncludePath>
|
||||
</VariousControls>
|
||||
</Aads>
|
||||
<LDads>
|
||||
@@ -2341,12 +2341,12 @@
|
||||
<File>
|
||||
<FileName>pid.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\PID\pid.c</FilePath>
|
||||
<FilePath>..\modules\algorithm\pid.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>filter.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\filter\filter.c</FilePath>
|
||||
<FilePath>..\modules\algorithm\filter.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>dji_motor.c</FileName>
|
||||
@@ -2356,62 +2356,62 @@
|
||||
<File>
|
||||
<FileName>dr16.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\DR16\dr16.c</FilePath>
|
||||
<FilePath>..\modules\comms\dr16.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>referee.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\referee\referee.c</FilePath>
|
||||
<FilePath>..\modules\comms\referee.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>led.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\LED\led.c</FilePath>
|
||||
<FilePath>..\modules\dev\led.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>buzzer.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\BUZZER\buzzer.c</FilePath>
|
||||
<FilePath>..\modules\dev\buzzer.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>c_comm.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\c_comm\c_comm.c</FilePath>
|
||||
<FilePath>..\modules\comms\c_comm.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>chassis.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\chassis\chassis.c</FilePath>
|
||||
<FilePath>..\modules\control\chassis.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>bmi088.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\BMI088\bmi088.c</FilePath>
|
||||
<FilePath>..\modules\sensor\bmi088.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>ist8310.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\IST8310\ist8310.c</FilePath>
|
||||
<FilePath>..\modules\sensor\ist8310.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>attitude.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\attitude\attitude.c</FilePath>
|
||||
<FilePath>..\modules\imu\attitude.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>kalman_filter.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\ins\kalman_filter.c</FilePath>
|
||||
<FilePath>..\modules\imu\qekf\kalman_filter.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>QuaternionEKF.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\ins\QuaternionEKF.c</FilePath>
|
||||
<FilePath>..\modules\imu\qekf\QuaternionEKF.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>ins.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>..\modules\ins\ins.c</FilePath>
|
||||
<FilePath>..\modules\imu\ins.c</FilePath>
|
||||
</File>
|
||||
</Files>
|
||||
</Group>
|
||||
|
||||
@@ -40,7 +40,14 @@ __initial_sp
|
||||
; <o> Heap Size (in Bytes) <0x0-0xFFFFFFFF:8>
|
||||
; </h>
|
||||
|
||||
Heap_Size EQU 0x200
|
||||
; Heap raised from 0x200 (512B) to 0x2000 (8KB):
|
||||
; modules/ins/kalman_filter.c allocates all KF matrix storage via libc
|
||||
; malloc (it does not include cmsis_os.h, so user_malloc expands to
|
||||
; malloc, not pvPortMalloc). Kalman_Filter_Init(6,0,3) issues 25
|
||||
; allocations totalling about 1.6KB with no NULL checks; a 512B heap
|
||||
; made the later ones return NULL and silently broke the filter.
|
||||
; 8KB matches the original RoboMaster-C-Board-INS-Example value.
|
||||
Heap_Size EQU 0x2000
|
||||
|
||||
AREA HEAP, NOINIT, READWRITE, ALIGN=3
|
||||
__heap_base
|
||||
|
||||
Binary file not shown.
+36
-113
@@ -1,133 +1,56 @@
|
||||
# Simba-ELCT-FrameWork
|
||||
# 底盘板固件(Simba-ELCT-FrameWork)
|
||||
|
||||
> 步兵机器人**底盘板**工程 · STM32F407IGH6 · Keil + FreeRTOS + CMSIS-DSP
|
||||
|
||||
<table>
|
||||
<tr>
|
||||
<td><img src="https://git.simba-robotics.com/assets/img/logo.svg" width = "200" height = "200" alt="Simba-git" /></td>
|
||||
<td><img src="https://git.simba-robotics.com/avatars/cd8e5bc7a1039e1cbadabefe03e647af1bbbd8722d659d437479b93c8c6fa799?" width = "200" height = "200" alt="ELEC" /></td>
|
||||
</tr>
|
||||
</table>
|
||||
项目总览、代码地图、任务表、调试与注意事项请先读 **[根 README](../README.md)** 与 **[已知问题与排障](../doc/已知问题与排障.md)**。本文只讲这块板特有的东西。
|
||||
|
||||
[](https://gitee.com/LSMushui)
|
||||
|
||||
|
||||
---
|
||||
|
||||
## 这块板负责什么
|
||||
|
||||
## 简介
|
||||
- **麦轮底盘**:四轮 M3508(`0x201~0x204`,CAN1)解算 + 轮速环
|
||||
- **跟随云台**:双 INS 世界系方案 —— 收云台的 `0x1A9`(世界系 yaw),与本板 INS 的 yaw 相减得相对偏差
|
||||
- **功率限制**:缓冲能量不足时按比例缩放轮电流
|
||||
- **裁判系统**:USART6
|
||||
- **板间通讯从侧**:解析云台发来的摇杆/控制状态(`0x149`/`0x189`),回传发射机构上电状态与故障位(`0x019`)
|
||||
|
||||
> Simba自己的C板电控框架！
|
||||
底盘板**没有** DR16 遥控器串口:遥控数据由云台板解析后经板间帧转发过来,底盘侧表现为一个"虚拟遥控器"(`ctoc_remote_t`)。
|
||||
|
||||
本项目的计划是基于 **DJI C型开发板** 的基础上打造一个更适合Simba同学们的代码框架
|
||||
## 编译宏
|
||||
|
||||
本框架致力于实现不同类型的机器人的代码通用化，只需要选择底盘云台的类型，修改一下对应物理参数即可实现对机器人的适配
|
||||
```
|
||||
USE_HAL_DRIVER, STM32F407xx, ARM_MATH_CM4, ROBOT_TYPE_CHASSIS
|
||||
```
|
||||
|
||||
## 配套套件
|
||||
## 相对云台板多出来的东西
|
||||
|
||||
> 上位机，ROS接口待开发
|
||||
| 项 | 位置 |
|
||||
| --- | --- |
|
||||
| `chassis` 麦轮解算 + 轮速环 + 跟随 + 功率限制 | `modules/control/chassis.c` |
|
||||
| `chassis_task` | `Application/Task/Src/chassis_task.c` |
|
||||
| 板间帧 0x149/0x189/0x1A9 的**接收**侧与 0x019 的发送侧 | `modules/comms/c_comm.c` |
|
||||
|
||||
## 模块支持
|
||||
云台板有的 `control/gimbal.c`、`control/shoot.c` 与对应任务本工程没有(所以两棵树的 `modules/control/` 内容不同,属预期差异)。
|
||||
|
||||
详细信息请参考 [Simba-ELCT-FrameWork文档](./doc/framework.md)
|
||||
## 上电时本板特有的行为
|
||||
|
||||
### 主要模块
|
||||
1. `led_init` 点亮蓝灯 → 各外设初始化
|
||||
2. IMU 初始化 + **z 轴零偏静止标定(约 0.5~1.5 s,这段时间别碰车)**
|
||||
3. 跟随符号综合:本板用小 `w` 脉冲 + INS 测出"w 正向对应的车身转向",与云台下发的编码器增向相乘;失败时回退 `CHASSIS_FOLLOW_SIGN`
|
||||
|
||||
- **Robot cmd**\
|
||||
机器人控制模块：
|
||||
- [ ] 大疆电机
|
||||
- [ ] 达妙电机
|
||||
- [ ] 瓴控电机
|
||||
- [ ] 小米电机
|
||||
- [ ] 超级电容控制板
|
||||
- [ ] 板间通信(CAN)
|
||||
- [ ] PWM 控制舵机
|
||||
- [ ] PWM 控制气泵
|
||||
- **Chassis**\
|
||||
底盘模块：
|
||||
- [ ] 麦轮底盘
|
||||
- [ ] 全向轮底盘
|
||||
- [ ] 舵轮底盘
|
||||
- [ ] 平衡底盘
|
||||
- **Gimbal**\
|
||||
云台模块：
|
||||
- [ ] yaw-pitch 直连云台
|
||||
- [ ] yaw直连-pitch丝杆 云台
|
||||
- [ ] yaw-pitch 丝杆云台
|
||||
- **Shoot**\
|
||||
发射机构模块：
|
||||
- [ ] 摩擦轮+拨弹盘
|
||||
- [ ] 摩擦轮+弹鼓+推杆
|
||||
- **Communication**\
|
||||
通信模块：
|
||||
- [ ] 板间通信(CAN2)
|
||||
- [ ] 上下位机通信
|
||||
- **Music**\
|
||||
音乐模块：
|
||||
- [ ] 开机声
|
||||
- [ ] 报错音
|
||||
- [ ] 裁判系统接入提示音
|
||||
- [ ] 《卡农》
|
||||
- [ ] 《天空之城》
|
||||
- [ ] 《恭喜发财》
|
||||
- [ ] 《好运来》
|
||||
- [ ] 《See You Again》
|
||||
- [ ] 《Unity》
|
||||
- [ ] 《你》
|
||||
> 车身自检时**整车会轻转**,确保周围安全。
|
||||
|
||||
## 开发工具
|
||||
## 蜂鸣器序号(本板)
|
||||
|
||||
本项目使用HAL库编写，通过CubeMX生成项目，使用FreeRTOS
|
||||
```
|
||||
0~3 = 四轮(0x201 右前 / 0x202 左前 / 0x203 左后 / 0x204 右后)
|
||||
4 = 云台数据(0x149/0x189/0x1A9)
|
||||
```
|
||||
|
||||
使用VSCode作为代码编辑器，Keil5作为IDE
|
||||
台架调试嫌吵时把 `robot_param.h` 的 `DEBUG_BUZZER_MELODY` 置 1(只换声音不换灯语)。
|
||||
|
||||
Keil5编译器使用AC6版本
|
||||
> 虽然keil5老，但在嵌入式调试以及环境配置方面是非常简单易上手的。**千万不要用keil5来编辑代码！！！**
|
||||
\
|
||||
\
|
||||
> VSCode作为现代化代码编辑器 ，通过各种插件即可实现各种提高效率的开发方式，**尤其是Github Copilot**
|
||||
\
|
||||
\
|
||||
> AC6编译器相比古早AC5编译器效率更高，AC5编译空项目就要足足1min47s，非常不中，相比之下AC6只需要6s，遥遥领先
|
||||
\
|
||||
\
|
||||
> 但是CubeMX开启FreeRTOS无法直接使用AC6编译器
|
||||
\
|
||||
> 强烈建议通过此[教程](https://bbs.robomaster.com/article/18268)(From COD战队GrassFan Wang)配置好自己的环境
|
||||
## 构建
|
||||
|
||||
用 Keil 打开 `MDK-ARM/Simba-ELCT-FrameWork.uvprojx`,确认 Define 里是 `ROBOT_TYPE_CHASSIS`,Rebuild → Download。
|
||||
|
||||
|
||||
|
||||
## 欢迎贡献
|
||||
|
||||
如果你发现了本项目中的问题和可优化的点，可用创建issue进行讨论
|
||||
|
||||
并且fork本项目后提交pr来贡献你的代码
|
||||
|
||||
我们的管理员审核后会将你的代码合并进来
|
||||
|
||||
> ***贡献代码时请参照 [注意事项](./doc/注意事项.md/#贡献代码) 中的贡献代码部分。***
|
||||
>
|
||||
> 编写代码时参考[API](./doc/API.md)
|
||||
|
||||
## 开发日志
|
||||
|
||||
[LOG.md](./doc/LOG.md) 记录了大家在开发过程中的各种奇思妙想和经验。多翻一翻或许能发现惊喜哦。大家有什么想法也都可以往里面写
|
||||
|
||||
[API.md](./doc/API.md) 为开发过程中所使用的模块接口标准，若在编写代码时发现代码与标准冲突，基于本标准修改代码
|
||||
|
||||
## 后续计划
|
||||
|
||||
具体要做的事情在[TODO](./doc/TODO.md)中
|
||||
|
||||
## 致谢
|
||||
|
||||
感谢各个战队的代码开源，所有本框架涉及到的主要的参考资料位于[reference.md](./doc/reference.md)
|
||||
|
||||
## 附录
|
||||
|
||||
本文的详细补充内容写在 [附录](./doc/appendix.md) 中
|
||||
|
||||
如发现本项目中的问题，请添加至[问题列表](./doc/questions.md)并与相关负责人联系，如果有解决方案可以提交pull request
|
||||
|
||||
## 通讯信息
|
||||
|
||||
- 作者 `连山木水` 微信：18088745066 QQ：1194309310
|
||||
> ⚠️ 本工程与云台工程源码高度同构,改一处通常要同步改另一处。动手前请读根 README 的 [§11 已知陷阱](../README.md#11-已知陷阱与注意事项)。
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -12,11 +12,10 @@
|
||||
* 底盘误判沿会误开小陀螺,勿再使用)
|
||||
* Data[6] bit0=键1 bit1=键2 bit2=小陀螺使能(本工程扩展)
|
||||
* bit7=云台电机故障(本工程扩展)
|
||||
* 0x1A9 云台->底盘:Data[0..3]=云台世界系 yaw(float,LE) Data[4]=att_ready
|
||||
* (双 INS 跟随方案的数据源,2026-09 新增)
|
||||
* 0x1A9 云台->底盘:Data[0..3]=云台世界系 yaw(float,LE)
|
||||
* Data[4] flags:bit0=att_ready bit1=编码器tick增向为CCW
|
||||
* bit2=该位有效(自检完成) [跟随符号自动综合的数据源]
|
||||
* bit2=该位有效(自检完成)
|
||||
* (双 INS 跟随方案 + 跟随符号自动综合的数据源,2026-09)
|
||||
* 0x019 底盘->云台:Data[0]发射机构上电状态 Data[1] bit0=底盘电机故障(本工程扩展)
|
||||
*
|
||||
* 故障位语义:任一侧电机掉线即置位,对侧收到后必须停止自己的输出(整车安全) */
|
||||
@@ -42,3 +42,23 @@ void buzzer_off(void)
|
||||
{
|
||||
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 0);
|
||||
}
|
||||
|
||||
/* ==========================================================================
|
||||
* 调试用旋律(见 buzzer.h):每步 100ms,空拍 BUZZER_P 为静音。
|
||||
* 无源蜂鸣器只有单音,这里用"上行分解和弦 + 回落"拼成一句短旋律;选音限定
|
||||
* 在中音 3 ~ 高音 2,避免刺耳。循环播放不会漏掉"有设备掉线"这一事实,
|
||||
* 因为红灯常亮由 monitor_task 独立控制,与该旋律无关。
|
||||
* 改旋律:直接增删这里的音符即可,长度会自动生效。
|
||||
* ========================================================================== */
|
||||
static const buzzer_tone_e buzzer_melody[] = {
|
||||
BUZZER_M3, BUZZER_M5, BUZZER_M6, BUZZER_H1, BUZZER_P,
|
||||
BUZZER_M6, BUZZER_H1, BUZZER_H2, BUZZER_P,
|
||||
BUZZER_H1, BUZZER_M6, BUZZER_M5, BUZZER_M3, BUZZER_P,
|
||||
};
|
||||
|
||||
void buzzer_play_melody(uint16_t step)
|
||||
{
|
||||
const uint16_t steps =
|
||||
(uint16_t)(sizeof(buzzer_melody) / sizeof(buzzer_melody[0]));
|
||||
buzzer_on(buzzer_melody[step % steps]);
|
||||
}
|
||||
@@ -16,4 +16,10 @@ typedef enum
|
||||
extern void buzzer_init(void);
|
||||
extern void buzzer_on(buzzer_tone_e tone);
|
||||
extern void buzzer_off(void);
|
||||
|
||||
/* ---- 调试用旋律 ----
|
||||
* 按固定 100ms/步 推进的短旋律(音符表见 buzzer.c),用于把掉线警告音换成
|
||||
* 音乐。step 由调用方每 100ms 递增 1,内部对旋律总步数取模自动循环;
|
||||
* 旋律中的空拍 BUZZER_P 由 buzzer_on() 解释为静音,用来断音。 */
|
||||
extern void buzzer_play_melody(uint16_t step);
|
||||
#endif
|
||||
@@ -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 环直接使用
|
||||
@@ -16,11 +16,33 @@
|
||||
#include "main.h"
|
||||
#include <string.h>
|
||||
|
||||
/* 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;
|
||||
@@ -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);
|
||||
@@ -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)
|
||||
@@ -303,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++;
|
||||
}
|
||||
@@ -15,6 +15,22 @@ 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;
|
||||
|
||||
/* 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);
|
||||
|
||||
Reference in New Issue
Block a user