Simba-OmniInfantry/README.md

Simba-OmniInfantry

Simba 步兵机器人电控固件 —— 双板架构(云台板 + 底盘板),Keil + FreeRTOS + CMSIS-DSP。

---

目录

• 1. 项目概览
• 2. 硬件与工具链
• 3. 快速开始
• 4. 代码地图
• 5. 运行架构
• 6. 数据流与对外接口
• 7. 模块清单
• 8. 调试:USB CDC 遥测
• 9. 调参与开关
• 10. 上电行为与灯语
• 11. 已知陷阱与注意事项
• 12. 版本管理约定
• 13. 致谢与联系

---

1. 项目概览

本仓库包含 两套完整且结构对称的 Keil 工程,分别烧录到步兵机器人的两块 C 型开发板上:

工程	编译宏	角色
云台/	ROBOT_TYPE_GIMBAL	云台(Yaw/Pitch)+ 发射机构 + 图传链路 + 板间通讯主控
底盘/	ROBOT_TYPE_CHASSIS	麦轮底盘 + 跟随云台 + 功率管理

设计要点:

• 两板源码高度同构。Application/、Bsp/、modules/ 绝大多数文件在两棵树下是同一份拷贝,板级差异用 ROBOT_TYPE_* 宏在文件内条件编译(而不是拆成两套文件)。因此改一处通常要同步改另一处,详见 §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)。
• 云台板会做一次 yaw 装配符号自检(给 Yaw 电机一个小电压脉冲看陀螺往哪边转),此时云台会轻转几度,不要扶住云台;失败时回退到 GIMBAL_YAW_SIGN_FALLBACK。

3.3 看一眼运行状态

插上 USB 会出现虚拟串口(CDC),波特率随意,直接打开即可看到姿态与诊断输出,字段含义见 §8。

---

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。

---

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。

---

11. 已知陷阱与注意事项

每一条都是实际踩过的坑。更详细的排障过程见 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 的分配宏:

#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