ELCT/Simba-OmniInfantry
9
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: ELCT/Simba-OmniInfantry:a9d3d97ed8dc03296c0ac5f3110f5eda4ddb186a
Branches Tags
ELCT/Simba-OmniInfantry:main
...
compare: ELCT/Simba-OmniInfantry:main
Branches Tags
ELCT/Simba-OmniInfantry:main
15 Commits
a9d3d97ed8 ... main
Author SHA1 Message Date
lyf 267a659242 feat(ui): 新增裁判系统自定义UI(0x0301)并打通键鼠→底盘链路 
键鼠控制(此前 WASD 无反应):
- 云台 robot_cmd_task:0x0304 与 USART3 帧内键鼠合并为共享视图 keyboard_merged(0x0304 优先);
  键鼠模式由帧内键鼠动作进入,动云台左摇杆退出
- c_comm_task/c_comm:键鼠在控时底盘摇杆由 WASD+鼠标合成,改读共享视图(此前只认 0x0304)
- dr16/调试输出:新增帧内键鼠原始字节诊断(key_lo/key_hi/mouse_*/s1/frames)

自定义UI(底盘板经裁判系统 USART6 绘制到选手端):
- 新增 底盘/modules/comms/referee_ui.c|h:15 字节图形组包(带编译期断言),
  v5 卡片式 HUD = 俯仰梯(pitch) + 罗盘刻度带(底盘朝向) + 预估底盘功率 + 超级电容
  + 摩擦轮 L/R 转速 + 小陀螺状态,yaw 在刻度带右侧;5Hz 刷新,约 50 图元/8 包
- referee.c 新增 referee_send_frame()(cmd 0x0301 机器人交互数据,CRC8+CRC16,USART6 TX DMA)
- 新增 CToC 0x1B9:云台把 pitch 与摩擦轮转速带给底盘(底盘本地没有这两个量)
- 版式避开官方保留区:顶部比分条/左下小地图/右下战车模块/中部两块官方保留

验证:云台 Code=99540 RO=1764 RW=420 ZI=62572;底盘 Code=99452 RO=1928 RW=416 ZI=62584;均 0 错误 0 警告。
未做实机验证(当前无板/无图传);UI 协议常量(UI_RECEIVER_ID/UI_SEND_HZ)与版式坐标集中在 referee_ui.h 宏区,便于按协议 PDF 一处校正。
 2026-10-16 04:24:32 +08:00
lyf 370d4519b5 电流环控制，并实现简单的补偿控制，还差参数具体化。 2026-10-16 00:54:52 +08:00
lyf 67eea77789 yaw正方向修正 2026-10-10 21:17:58 +08:00
lyf 79a545b379 修复上电云台正方向问题，以及初步调好了底盘跟随PID，以及小陀螺模式下的运动偏移问题。 2026-09-12 19:05:26 +08:00
LSMushui cf736569fb 更新README 2026-09-12 12:13:40 +08:00
LSMushui 7c25e06947 docs: 重写 README,新增排障手册,补板级说明 
原 README.md 是上游框架模板的拷贝:两棵树内容完全相同,描述的是"计划要做的
模块"(一堆未勾选的 checkbox),且大量链接指向根本不存在的 ./doc/*.md
(framework/注意事项/API/LOG/TODO/reference/appendix/questions 全都没有)。
它没有描述本项目实际重构后的代码。

本次文档落地为三份:

1. README.md(根,详细)
   - 项目概览(双板架构、两板同构、QEKF、双 INS 跟随方案)
   - 硬件与工具链、快速开始(编译烧录 + 上电前置检查)
   - 代码地图(Application/Bsp/Core/modules/MDK-ARM 逐层说明)
   - 运行架构(9 个任务的优先级/栈/职责表、控制周期、IWDG 看门狗)
   - 数据流与对外接口(CAN 分配、串口分配、CToC 帧表、姿态数据流)
   - 模块清单、USB CDC 遥测字段详解、调参入口与三个台架开关
   - 上电时序与灯语/蜂鸣器语义
   - 已知陷阱(整合了本次排障的全部硬知识)
   - 版本管理约定、致谢与联系

2. doc/已知问题与排障.md(新增)
   排障总则(先看 ca/cg 而非数值;用算式交叉验证)+ 症状速查表 + 6 个典型
   案例详解 + 提交前检查清单。把"踩过的坑"沉淀成可复查的手册。

3. 云台/README.md、底盘/README.md(重写为简短的板级说明)
   只讲本板特有的职责、编译宏、独占模块、上电特有行为、蜂鸣器序号,并指回
   根 README;顺便去掉指向不存在文件的死链。

另修一处文档缺陷:modules/comms/c_comm.h 里 0x1A9 帧被写了两遍,且两处对
Data[4] 的描述互相矛盾(一处说只有 att_ready,一处说还有编码器增向与自检位),
已合并为一份准确描述。
 2026-09-12 11:53:50 +08:00
LSMushui e3d432f3b5 refactor(modules): 按角色重排 modules/ 目录,消除命名不一致 
原 modules/ 下目录大小写混用(BMI088/BUZZER/DR16/IST8310/LED/PID 为大写,
attitude/c_comm/filter/gimbal/ins/motor/referee/shoot 为小写),且把第三方
移植代码(QuaternionEKF/kalman_filter)与本工程封装(ins)混在同一目录。

新结构(全部小写,按角色分层,第三方代码单独隔离):

  modules/
    algorithm/   pid.c/h  filter.c/h          通用算法
    imu/         attitude.c/h  ins.c/h        姿态与惯导封装
    imu/qekf/    QuaternionEKF.c/h  kalman_filter.c/h
                                              (移植自 Wang Hongxi INS 例程)
    sensor/      bmi088.c/h  ist8310.c/h      板载传感器驱动
    comms/       dr16.c/h  c_comm.c/h  referee.c/h
    motor/       dji_motor.c/h                电机抽象
    control/     gimbal.c/h  shoot.c/h        整车控制逻辑
                 (底盘为 chassis.c/h)
    dev/         led.c/h  buzzer.c/h          板级指示/发声

共移动 58 个文件(每板 29 个),旧目录全部删除。

工程文件同步(每个工程各 2 个):
- .uvprojx: Modules 分组 16 个 <FilePath> 改为新路径;
  2 个非空 <IncludePath> 元素:15 个旧模块路径收敛为 8 个新目录,
  并顺带删除陈旧项 ../modules/AHRS(目录根本不存在,legacy 时代残留)
  与冗余项 ..\bsp(与 ../Bsp/Inc 重复)
- .uvoptx: 16 个 <PathWithFileName> 同步

关于"为什么不用改源码": Keil 的 include 靠 IncludePath 解析,本工程所有
#include 都是纯文件名形式(无路径限定),所以移动目录只需改工程配置。
另外 .h 完全不登记在工程里(只有 16 个 .c 被登记),故头文件只需移动。

回归验证(无法在此环境编译,用等价判据):
- 两个工程引用的 98 / 95 个 <FilePath> 全部真实存在
- 磁盘上 47 / 45 个 .c 全部仍被工程引用 —— 即没有任何文件掉出编译
- 2 个 IncludePath 元素均含全部 8 个新模块目录,无旧路径残留
- 所有 IncludePath 指向的目录均真实存在
- .uvprojx/.uvoptx XML 良构
- 两棵树结构一致(chassis/gimbal 等板级差异符合预期)

⚠️ 仍需在 Keil 里点一次编译确认(见 README 的构建章节)。
 2026-09-12 11:51:41 +08:00
LSMushui 3e5e5e3f95 fix(bmi088): 数据路停摆的自恢复(任务上下文健康检查) 
现场遥测(zo/ck 字段生效,说明新固件在跑):
  yaw -18.13 -> -18.61 / 0.7s , pitch/roll/an 全部纹丝不动
  ca=832 cg=1032 —— **8 行完全一致,采样计数停止增长**
  zo=-0.0009 ck=1 (零偏标定正常) , gx/gy/gz 也恒定

## 判定
ca/cg 不增长 = SPI 完成回调不再产生样本,IMU 数据已冻结在最后一个样本上。
此时 yaw 的"匀速自漂"只是 EKF 在忠实积分冻结值:
  gz - zo = -0.0117 - (-0.0009) = -0.0108 rad/s ≈ 观测 -0.0119 rad/s
所以姿态算法本身是对的,问题全在数据路。上一轮加的 z 标定健壮性生效了
(zo 从 0.067 降到 0.0009,ck=1),但数据路仍会停摆。

## 为什么必须放在任务上下文
停摆只有两类原因,而**中断回调里的兜底两类都救不了**:
  a) hspi1 卡在 BUSY(某次 SPI/DMA 出错后 HAL 没回到 READY):
     start_dma_read() 的 HAL_SPI_GetState 检查让每次调用都直接返回;
  b) BMI088 的 DRDY 是电平型信号:某次失败/被挡掉的读没把传感器的数据
     寄存器读走,INT 电平就保持住,之后既无下降沿也无上升沿 ——
     EXTI 永不触发,写在 EXTI 回调里的任何兜底都不会执行。
故新增 bmi088_health_check(),由 attitude_update() 每 1ms 调用:采样计数
超过 BMI088_STALL_MS(5ms)没增长即判停摆,执行
  关 EXTI -> 复位 read_phase/片选 -> SPI 卡住则 abort,仍不 READY 就
  DeInit+Init(hspi1.Init 里仍是模式3/16分频,不会被改回)
  -> 清 EXTI 挂起标志 -> 阻塞读一次加速度+陀螺(顺带清传感器 DRDY 电平)
  -> 重新使能 EXTI
若中断路始终修不好,则退化为每 5ms 补读一次(约 200Hz)的轮询保底。因为
dt 固定 1ms、1kHz 调用,1 秒内积分出的总转角仍然正确(保持值×0.001×1000),
只是运动解析粒度变粗 —— 用于排障足够,正式使用应查清停摆根源。

## 附带
- 实现 HAL_SPI_ErrorCallback(HAL 里是 __weak):此前完全没实现,SPI/DMA
  出错后相位与片选无人复位;现在复位相位/片选并累加 bmi088_spi_errors。
- 新增 bmi088_spi_state() 暴露 HAL_SPI_GetState,debug_task 追加
  se=<SPI错误数> st=<SPI状态,1=READY>。若 st 长期非 1 即命中原因 (a);
  若 st 恒为 1 而 ca/cg 不涨,即命中原因 (b)。
- 已静态核对 DMA 流分配无冲突:SPI1 RX/TX = DMA2_Stream2/3 ch3,
  USART1 = Stream5/7, USART6 = Stream1/6, USART3_RX = DMA1_S1,
  I2C2_TX = DMA1_S7,互不重叠。

两个工程同步修改,内容一致。
 2026-09-12 11:32:42 +08:00
LSMushui 9fc1575615 fix(ins/bmi088): 修复 yaw 匀速自漂(零偏标定被污染)与 IMU 数据冻结隐患 
现场数据(yaw 以恒定角速度自漂,陀螺却不响应板子移动):
  yaw=-213.09 -> -223.10,2.60s 内 -10.01° => -3.8500 °/s = -0.06720 rad/s
  同窗口 gn = 0.017 rad/s = 0.9740 °/s          比值 3.95
  an=9.80 恒定、pitch/roll 基本不动、cvg=1、imu=1

## 根因一:gyro_z_offset 被运动样本污染(本次主因)
gn 打印的是 bmi088_gyro[] 原始模长(未减零偏),而实际进滤波器的是
gyro_z = gz - gyro_z_offset。漂移率 0.0672 rad/s 比原始 |gyro| 0.017
还大近 4 倍,只可能是 gyro_z_offset 本身是个约 +0.067 rad/s 的假零偏。
ins_init() 原来直接对前 1000 个样本取均值:上电瞬间 BMI088 的 SPI DMA
路刚起步,且车体常正被摆弄,运动样本一旦混进窗口,零偏就被标歪 ——
之后 yaw 便以该常值匀速自漂,与陀螺真实输出无关。

修: ins.c 的 z 标定改为
  - 先等采样路稳定(INS_Z_CAL_SETTLE_MS=50ms)
  - 单样本 |gyro| > 0.35rad/s(20°/s)视为无效,静止时不该出现
  - 一轮有效样本占比 <90% 视为"上电时车体在动",整轮作废重测,最多 3 轮
  - 单轮取 0.5s:LED_Task 的 IWDG 上电宽限期只有 5s,而 IMU_Task 在
    attitude_init() 期间不刷心跳,整个初始化必须留在 5s 内,可靠性靠
    "整轮重测"而非拉长单轮(3 轮最坏 1.55s,仅比原来 1s 略长)
  - 多轮都不静则用最后一轮均值并置 ins_z_cal_ok=0

## 根因二(顺带修):EXTI 触发沿与传感器/文档不符
bmi088.c 文件头与 bmi088.h 都写"数据就绪下降沿触发",legacy 用的也是
EXTI_Trigger_Falling,但 CubeMX 生成的 gpio.c 配的是 GPIO_MODE_IT_RISING
—— 会落在数据就绪电平的恢复沿上,采样时序整体错位。已改为
GPIO_MODE_IT_FALLING,并在 .ioc 里补 GPIO_ModeDefaultEXTI=GPIO_MODE_IT_FALLING
(否则重新生成工程又会被改回 RISING)。

## 根因三(隐患):数据路可静默冻结
start_dma_read 里 read_phase 先置位、片选先拉低,之后若
HAL_SPI_TransmitReceive_DMA 启动失败就直接返回,完成回调永不到来,
read_phase 永久卡在非 NONE:此后每个数据就绪中断都被
if (read_phase == NONE) 挡掉,数据冻结在最后一个样本,而 bmi088_online
仍是 1、an/gn 看着还挺正常。修:失败则释放片选并回滚 read_phase;
另在 EXTI 回调加 20ms 超时兜底(一次 SPI 读远小于 1ms),回调丢失时
强行复位,保证能自恢复。

## 遥测:让这类问题一眼可见
bmi088.c 增加 bmi088_accel_cnt / bmi088_gyro_cnt(解析成功后自增),
ins.c 暴露 ins_gyro_z_offset / ins_z_cal_ok,debug_task 输出扩展为
  yaw= pitch= roll= cvg= rdy= imu= an= gn= gx= gy= gz= zo= ck= ca= cg=
其中 ca/cg 是否增长是判断"数据是否还在更新"的唯一可靠依据(数值冻结时
它们不涨);zo 与 gz 量级对比可直接看出零偏是否被标歪。新增
DEBUG_ATT_DIAG 开关(置 0 去掉这些排障字段)。

两个工程同步修改,内容一致。
 2026-09-12 11:27:42 +08:00
LSMushui 72bf925f47 feat(buzzer): 台架调试模式把 CAN 掉线警告音换成循环旋律 
台架调试时 CAN 设备(尤其 Pitch)掉线会按 legacy 语义每 2s 响 n 下,连续
滴滴声很吵且影响听其它声音,故加一个开关:

  robot_param.h -> 台架调试开关
  #define DEBUG_BUZZER_MELODY  1   // 1=警告音换旋律;0=legacy 报警(比赛用)

实现分三处:
- robot_param.h: 新增开关,与 DEBUG_SHOOT_WITHOUT_REFEREE 放在同一分区
- modules/BUZZER: 新增 buzzer_play_melody(step) —— 按固定 100ms/步 推进,
  内部对旋律总步数取模自动循环;旋律表 buzzer_melody[] 用"上行分解和弦 +
  回落"共 14 步(1.4s),空拍 BUZZER_P 作断音,选音限定中音3~高音2 避免刺耳。
  驱动层本身不判断开关,保持纯外设职责,只是多了一个音符序列接口。
- monitor_task.c: CAN 掉线分支按开关二选一。旋律模式下步进用本任务自身的
  100ms 节拍(与文件里已有的遥控器连接音效同一写法),不用 HAL_GetTick
  采样,避免周期抖动漏掉或重复音符。

设计取舍: 只替换"声音",不替换"灯语" —— 红灯常亮、遥控器掉线红灯双闪、
BMI088 失败绿灯双闪等 LED 语义完全不变,因此调试时仍能一眼看出是哪台
设备掉线;linkcheck_first_offline() 也保留调用,方便在调试器里直接看结果。
比赛前把 DEBUG_BUZZER_MELODY 改回 0 即恢复 legacy 报警。

两个工程同步修改,内容保持一致。
 2026-09-12 11:19:59 +08:00
LSMushui 3e1bae6b98 chore: 同步 Keil/VSCode 本地状态(非本次代码改动) 
- 底盘 .uvoptx: DSP Library 分组展开状态 tvExp 0 -> 1(Keil 中展开工程树
  所致,不影响编译)
- 云台 .vscode/settings.json: K230 Pylance stub 路径随工具链更新

两处均为工具自动写入,一并入库以保持工作区干净。
 2026-09-12 11:15:24 +08:00
LSMushui 4f18ada2f7 fix(ins): 修复 QEKF 矩阵内存分配失败(堆 512B -> 8KB),并恢复姿态诊断位 
## 根因: 堆太小导致卡尔曼滤波静默失效
modules/ins/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,因此 _CMSIS_OS_H 未定义 -> user_malloc 展开为 libc malloc,
FreeRTOS 那 32KB(configTOTAL_HEAP_SIZE=32768)堆完全用不上。

libc malloc 的堆来自 MDK-ARM/startup_stm32f407xx.s 的 Heap_Size,CubeMX
默认值是 0x200 = 512 字节。而 Kalman_Filter_Init(6, 0, 3) 会发起 25 次
分配、合计约 1602 字节(P/Pminus/F/FT/Q/S/temp_matrix/temp_matrix1 各
144B,H/HT/K 各 72B,R 36B,Q 系列 24B 等),且全程没有一处 NULL 检查,
分配失败后紧接着 memset(NULL, 0, n) 并把 NULL 直接交给 arm_mat_* 使用:

  - 前半段小分配成功、后半段 144B 级分配返回 NULL
  - arm_mat_* 对 pData=NULL 只读写地址 0 附近,函数本身不报错、只是不改
    动输出 -> 滤波器不再更新状态,但 QEKF_INS.UpdateCount 照常自增
  - 表现为: att_ready=1(看起来"在跑")、欧拉角却恒定不动

对照原始例程(能正常工作的那版)RoboMaster-C-Board-INS-Example-master,
其 MDK-ARM/startup_stm32f407xx.s 的 Heap_Size 为 0x2000 = 8KB,是本工程
的 16 倍。本次将两个工程的 Heap_Size 一并提到 0x2000,与该例程一致。

注意: startup_stm32f407xx.s 由 CubeMX 生成,若日后重新生成工程,这行会
被改回 0x200,需再次修改(或改走 pvPortMalloc 路线)。注释用纯 ASCII,
避免 armasm 处理非 ASCII 源文件。

## 恢复姿态诊断位
上一版 debug_task.c 把 cvg 换成了 att_ready,丢掉了唯一的"是否真的收敛"
判据 —— 于是"IMU 没数据"和"滤波器没跑"都表现为 cvg=1 + 三个 0.00,无法
区分。本次:
  - cvg 恢复为 INS.ConvergeFlag(与原诊断输出同义)
  - 新增 rdy(=att_ready)、imu(=bmi088_online)、an(=|accel|)、
    gn(=|gyro|) 四个姿态链路健康位
  - 保留 DEBUG_ATT_HEALTH 开关,置 0 即只输出三个欧拉角
  - 文件头补注: yaw 无绝对基准,上电静止时恒为 0,不能作为故障判据
 2026-09-12 11:15:16 +08:00
LSMushui 944d0201da chore(mdk): 对齐 uvoptx 的文件编号,消除删除条目留下的空档 
删除 bsp_uart 的两个文件条目后,.uvoptx 的 <FileNumber> 序列在删除点后面
留下断号(云台 1..97 后直接跳到 100,底盘 1..94 后跳到 97)。Keil 重新保存
工程时会自行把末项改回连续编号(底盘已被 Keil 规范为 95),此处把云台
一并规范为 98,使两个工程的编号均为 1..N 连续。

仅改编号数值,无文件增删,不影响编译。
 2026-09-12 11:03:44 +08:00
LSMushui 5e6c5068fa fix(dsp): 补回缺失的 CMSIS-DSP 预编译库 
上一个提交放开了 *.lib 的忽略规则,但两个工程下
Middlewares/ST/ARM/DSP/Lib/ 目录本身就不在磁盘上(此前工作区异常中随
其他被忽略文件一并丢失),导致 .uvprojx 引用的
arm_cortexM4lf_math.lib 找不到,工程无法链接。

现从同源副本 Desktop/workspace 下的对应文件还原:
- 云台/Middlewares/ST/ARM/DSP/Lib/arm_cortexM4lf_math.lib
- 底盘/Middlewares/ST/ARM/DSP/Lib/arm_cortexM4lf_math.lib
两份 SHA-256 一致(89bd896a...),各 2,729,868 字节。

还原后重新校验:两个 .uvprojx 共 193 条 <FilePath> 引用,磁盘缺失 0 个。

同时把 .workbuddy/(助手工作区状态)加入 .gitignore,不纳入固件仓库。
 2026-09-12 11:02:09 +08:00
LSMushui 3c95bb0887 feat(debug): USB CDC 输出改为 QuaternionEKF 欧拉角 
原 debug_task 每 500ms 经 CDC 打印一行混装诊断信息(芯片 ID/帧计数/空闲中断
计数/链路状态/电机掉线/跟随误差等 18 个字段),稳定后已无持续观察价值。
现改为直接输出姿态解算结果,便于上位机/串口助手直接看欧拉角与验证 EKF 收敛。

输出格式(单位: 度): yaw=<...> pitch=<...> roll=<...> cvg=<0|1>
  yaw   = att_deg_yaw   (多圈连续偏航角,= INS.YawTotalAngle,云台 Yaw 环反馈)
  pitch = att_deg_pitch  (= QEKF_INS.Roll, 绕 y 轴角)
  roll  = att_deg_roll   (= QEKF_INS.Pitch,绕 x 轴角,云台 Pitch 环反馈)
  cvg   = att_ready      (0 表示 EKF 未收敛,此时角度不可信)
字段命名沿用工程侧语义,QEKF 内部 Pitch/Roll 与航空惯例互换的映射见
modules/attitude/attitude.c 文件头。

其他调整:
- 云台/底盘共用本文件,因两板 IMU_Task 都跑 attitude_update(),att_deg_*
  均有效,故移除原 ROBOT_TYPE_GIMBAL / ROBOT_TYPE_CHASSIS 分支;
- 输出周期 500ms -> 100ms(10Hz,宏 DEBUG_OUT_PERIOD_MS 可调),2Hz 下无法
  观察姿态动态响应;
- 清理因此不再需要的 include 与 dbg_* 外部声明(bmi088/ist8310/dr16/referee/
  c_comm/bsp_can/bsp_usart/usart.h/dbg_imu_loop/dbg_can_loop);
- 同步更新两份 debug_task.h 的注释说明。
 2026-09-12 10:33:29 +08:00
102 changed files with 3309 additions and 922 deletions
Expand all files Collapse all files
.gitignore
+17
View File
@@ -28,6 +28,20 @@
*.uvguix *.uvguix
DebugConfig/ DebugConfig/
# ---- J-Link 调试器自动生成(与具体探针/机器绑定, 非源码) ----
# JLinkSettings.ini: 点"下载/调试"时 Keil 写下的连接参数(器件/接口/速率)
# JLinkLog.txt: J-Link 驱动日志
# 两者都会随换机器、换探针、换烧录方式而变, 不应入库; 缺失时 Keil 会自动重建
JLinkSettings.ini
JLinkLog.txt
JLinkLog.txt.*
*.jlink
# ---- Keil UV4 编译/烧录日志(uv4 -b/-f -o 生成) ----
build_log.txt
*_build.txt
*_flash.txt
# ---- 编辑器 / 系统 ---- # ---- 编辑器 / 系统 ----
*.bak *.bak
*.ddk *.ddk
@@ -35,3 +49,6 @@ DebugConfig/
*.tra *.tra
*~ *~
Thumbs.db Thumbs.db
# ---- 助手工作区状态(记忆/会话数据),不属于固件源码 ----
.workbuddy/
.vscode/settings.json
Vendored
+3
View File
@@ -0,0 +1,3 @@
{
"python.analysis.stubPath": "C:\\Users\\16936\\.kendryte\\k230_canmv_pylance\\4702c5814722ec2f\\typings"
}
README.md
+438
View File
@@ -0,0 +1,438 @@
# 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>
[![Author](https://img.shields.io/badge/Author-LSMushui-orange.svg)](https://gitee.com/LSMushui)
![language](https://img.shields.io/badge/language-C-blue.svg)
![license](https://img.shields.io/badge/license-GPLv3-green.svg)
![mcu](https://img.shields.io/badge/MCU-STM32F407IGH6-lightgrey.svg)
> 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`(0x1FE)、摩擦轮 M3508 `0x201/0x202`(0x200) | Yaw GM6020 `0x205`(0x1FE)、拨弹 M2006 `0x201`(0x200)、底盘回传 `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.0 已知底盘1号电机，云台pitch电机会掉线，记得检修
### 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 'GLM 5.3Flash' 'DeepSeek V4.1Flash'
## 参考
- 踩坑记录与排障手册:[`doc/已知问题与排障.md`](./doc/已知问题与排障.md)
doc/已知问题与排障.md
+160
View File
@@ -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"的开关?→ 记得改回来
云台/.vscode/settings.json
Vendored
+2 -2
View File
@@ -1,8 +1,8 @@
{ {
"python.analysis.extraPaths": [ "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": { "python.analysis.diagnosticSeverityOverrides": {
"reportMissingModuleSource": "none" "reportMissingModuleSource": "none"
} }
云台/Application/Task/Inc/debug_task.h
+3 -2
View File
@@ -1,8 +1,9 @@
/* debug_task.h - USB 虚拟串口诊断任务声明(仅诊断用) */ /* debug_task.h - USB 虚拟串口(CDC)欧拉角输出任务声明 */
#ifndef DEBUG_TASK_H #ifndef DEBUG_TASK_H
#define 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); extern void debug_task(void const * argument);
#endif #endif
云台/Application/Task/Inc/robot_cmd_task.h
+9
View File
@@ -24,4 +24,13 @@ typedef struct
extern keyboard_data_t keyboard_data; extern keyboard_data_t keyboard_data;
/* ---- 键鼠来源合并视图(2026-10-16) ----
* 客户端键鼠既可能从裁判系统 0x0304 进来,也可能从 USART3 那 18 字节帧进来
* (官方 18 字节控制帧里就带鼠标与按键,由 dr16 解析成 dr16.mouse_rl / dr16.key_w 等)。
* robot_cmd_task 每周期把两路合并到下面这个视图:
* 云台/发射控制 与 c_comm_task 的底盘摇杆合成 都读它,
* 避免两处各判一次出现"云台跟鼠标、底盘却还在收遥控器原始摇杆"。 */
extern keyboard_data_t keyboard_merged; /* 合并后的键鼠数据(0x0304 优先) */
extern uint8_t keyboard_merged_online; /* 1=键鼠在控(0x0304 在线 或 帧内键鼠有动作) */
#endif #endif
云台/Application/Task/Src/c_comm_task.c
+25 -15
View File
@@ -33,28 +33,38 @@ void c_comm_task(void const * argument)
for (;;) for (;;)
{ {
#if defined(ROBOT_TYPE_GIMBAL) #if defined(ROBOT_TYPE_GIMBAL)
uint8_t kb_online = (keyboard_data.last_tick != 0 && /* 键鼠来源已由 robot_cmd_task 合并(0x0304 优先,否则 USART3 帧内键鼠)。
HAL_GetTick() - keyboard_data.last_tick < DEVICE_OFFLINE_TIME_MS * 10); * 直接用共享视图,保证"云台怎么控、底盘就怎么收"——否则 WASD 合成的
* 底盘摇杆会因为这里只认 0x0304 而被整段跳过(客户端键鼠走帧内时平移失效)。 */
uint8_t kb_online = keyboard_merged_online;
uint8_t kb[12]; uint8_t kb[12];
kb[0] = (uint8_t)(keyboard_data.mouse_rl & 0xFF); kb[0] = (uint8_t)(keyboard_merged.mouse_rl & 0xFF);
kb[1] = (uint8_t)((uint16_t)keyboard_data.mouse_rl >> 8); kb[1] = (uint8_t)((uint16_t)keyboard_merged.mouse_rl >> 8);
kb[2] = (uint8_t)(keyboard_data.mouse_du & 0xFF); kb[2] = (uint8_t)(keyboard_merged.mouse_du & 0xFF);
kb[3] = (uint8_t)((uint16_t)keyboard_data.mouse_du >> 8); kb[3] = (uint8_t)((uint16_t)keyboard_merged.mouse_du >> 8);
kb[4] = (uint8_t)(keyboard_data.mouse_wheel & 0xFF); kb[4] = (uint8_t)(keyboard_merged.mouse_wheel & 0xFF);
kb[5] = (uint8_t)((uint16_t)keyboard_data.mouse_wheel >> 8); kb[5] = (uint8_t)((uint16_t)keyboard_merged.mouse_wheel >> 8);
kb[6] = keyboard_data.mouse_key_l; kb[6] = keyboard_merged.mouse_key_l;
kb[7] = keyboard_data.mouse_key_r; kb[7] = keyboard_merged.mouse_key_r;
kb[8] = (uint8_t)(keyboard_data.key_w | (keyboard_data.key_s << 1) | kb[8] = (uint8_t)(keyboard_merged.key_w | (keyboard_merged.key_s << 1) |
(keyboard_data.key_a << 2) | (keyboard_data.key_d << 3) | (keyboard_merged.key_a << 2) | (keyboard_merged.key_d << 3) |
(keyboard_data.key_shift << 4) | (keyboard_data.key_ctrl << 5) | (keyboard_merged.key_shift << 4) | (keyboard_merged.key_ctrl << 5) |
(keyboard_data.key_q << 6) | (keyboard_data.key_e << 7)); (keyboard_merged.key_q << 6) | (keyboard_merged.key_e << 7));
kb[9] = (uint8_t)(keyboard_data.key_alpha1 | (keyboard_data.key_alpha2 << 1)); kb[9] = (uint8_t)(keyboard_merged.key_alpha1 | (keyboard_merged.key_alpha2 << 1));
kb[10] = 0; kb[10] = 0;
kb[11] = 0; kb[11] = 0;
c_comm_master_send(kb_online, kb, gimbal_motor_offline(), c_comm_master_send(kb_online, kb, gimbal_motor_offline(),
cmd_robot.spin_flag); // 故障位+小陀螺使能位 cmd_robot.spin_flag); // 故障位+小陀螺使能位
/* 0x1B9 自定义UI 专用量:底盘本地没有 pitch 与摩擦轮转速,
* 由云台随板间通讯带给它(见 底盘/modules/comms/referee_ui.c)。
* pitch 取云台 pitch 环反馈 -att_deg_roll;摩擦轮转速取电机反馈 rpm。 */
c_comm_master_send_ui(-att_deg_roll,
(float)motor_friction_l.fdb.rotor_speed,
(float)motor_friction_r.fdb.rotor_speed,
cmd_robot.spin_flag);
/* 0x1A9 姿态帧:跟随符号自动综合的数据源 /* 0x1A9 姿态帧:跟随符号自动综合的数据源
* flags:bit0=att_ready bit1=tick增向CCW bit2=bit1有效 */ * flags:bit0=att_ready bit1=tick增向CCW bit2=bit1有效 */
{ {
云台/Application/Task/Src/debug_task.c
+188 -57
View File
@@ -1,82 +1,213 @@
/* ============================================================================= /* =============================================================================
* debug_task.c - USB 虚拟串口(CDC)诊断输出(仅诊断用,稳定后可整组移除) * debug_task.c - USB 虚拟串口(CDC)姿态输出(欧拉角 + 姿态链路诊断 + 控制量)
* 每 500ms 一行状态:各芯片实测 ID/DR16 帧计数/USART 空闲中断计数/任务循环 * 每 DEBUG_OUT_PERIOD_MS 输出一行。单位: 角度 ° / 加速度 m/s² / 角速度 rad/s
* 计数等;板子插电脑出现虚拟 COM 口,串口助手打开即可(波特率随意)。 *
* 字段含义见重构说明与编译指南.md 的诊断章节 * ---- 欧拉角(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_CMD_OUT=1 时追加;用于排查 控制源→cmd_robot→执行机构) ----
* online 1=**遥控器(DR16)在线** —— 本板这一位只反映 DR16,键鼠不置位!
* 没有遥控器时整段控制分支都不执行(见 robot_cmd_task.c 的 online 门控),
* 所以"键鼠测试"也必须有遥控器在线,否则这里恒为 0。
* src 0=遥控器 1=键鼠(判据:0x0304 在 1s 内到过 keyboard_data.last_tick)
* spin 小陀螺使能(云台侧检测,随 0x189 下发给底盘)
* yaw_d/pitch_d 本周期云台角度增量(°/2ms 任务周期,×500 即 °/s)
* fric/fire/eject 发射机构开关 reset 云台角度复位请求
* s2/ctrl 遥控器 S2 档位与 Ctrl 按下沿 —— 底盘就是靠这两个量推出
* chassis_mode,所以本板不输出模式本身(本板该字段恒为 OFF)
* wheel 遥控拨轮原始值(1000~1050 为中立)
* --- 以下为 DR16 帧内容诊断(判断"键鼠数据到底有没有到机器人") ---
* s1 左侧拨杆(1上 2下 3中)。注意官方 RoboRTS 的 dbus.c 与本工程一致:
* sw2=bit44-45→s2, sw1=bit46-47→s1;若与实物相反,用本列与 s2 对照即可判定
* mouse_rl/mouse_du/mouse_wheel 帧里 data[6..11] 解析出的鼠标三轴
* mouse_l/mouse_r 帧里 data[12]/data[13](鼠标左右键)
* key_lo/key_hi 帧里 data[14]/data[15] **原始字节**(不做位拆分)
* ★ 按下 W 时 key_lo 应变 0x01;若恒为 0,说明帧里根本没有键鼠数据
* frames dr16_frame_count;持续增长说明 USART3 一直在收帧
* 无健康位(DEBUG_ATT_HEALTH=0)时输出为纯数字 CSV,列序即上面顺序,
* 可被 Lissio FireWater 直接解析成通道绘图。
*
* 开关: DEBUG_ATT_HEALTH=0 只输出三个欧拉角;DEBUG_ATT_DIAG=0 去掉诊断位;
* DEBUG_CMD_OUT=1 追加控制量字段。
* ========================================================================== */ * ========================================================================== */
#include "debug_task.h" #include "debug_task.h"
#include "app_tasks.h"
#include "cmsis_os.h" #include "cmsis_os.h"
#include "FreeRTOS.h" #include "FreeRTOS.h"
#include "task.h" #include "task.h"
#include "usb_device.h"
#include "usbd_cdc_if.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 "attitude.h"
#include "gimbal.h"
#elif defined(ROBOT_TYPE_CHASSIS)
#include "chassis.h"
#include "bmi088.h"
#include "ins.h" #include "ins.h"
#include "bmi088.h"
#include "gimbal.h"
#include "dr16.h"
#include "robot_typedef.h" #include "robot_typedef.h"
#include <stdio.h>
#include <math.h>
#include <string.h>
/* ===== 2026-10-15 台架调参临时遥测(已停用,保留备查) =====
* 启用时把 #if 0 改 #if 1:本任务改为输出 5 列 CSV
* setpoint_deg, actual_deg, out_current, ecd_deg, gyro_x_dps
* (逗号分隔+换行,可被 Lissio FireWater 自动识别为 5 通道),采样率提到 200Hz,
* 用于定量观测阶跃响应与判断"轴是否真的在转动"。 */
#if 0
#include "robot_param.h"
#define DEBUG_PITCH_TUNE 1
#if DEBUG_PITCH_TUNE
#define DEBUG_OUT_PERIOD_MS 5 /* 200Hz:2ms 控制环的阶跃响应需 ≥100Hz 采样才量得准 */
#endif
#endif #endif
/* debug_task:USB 虚拟串口(CDC)诊断输出,每 500ms 一行 /* 1-输出健康位(cvg/rdy/imu/an/gn); 0-只输出三个欧拉角 */
* 板子插上电脑会出现虚拟 COM 口,用串口助手打开即可看到(波特率随意) #define DEBUG_ATT_HEALTH 0
* 输出为纯整数,用于远程定位传感器/通讯初始化问题 */ /* 1-再附加原始陀螺/零偏/采样计数(排障用); 0-只留上面的通用健康位 */
#define DEBUG_ATT_DIAG 0
/* 1-仅输出 Pitch 调参行(便于用 VOFA+/串口示波器绘图); 0-同时输出姿态行与 Pitch 调参行 */
#define DEBUG_PITCH_ONLY 0
/* 1-追加控制量字段(见文件头); 0-保持原姿态输出不变 */
#define DEBUG_CMD_OUT 1
/* 各模块导出的诊断变量(定义在 imu_task.c / can_task.c) */ /* 输出周期(ms)。姿态观测建议 100ms(10Hz) */
extern volatile uint32_t dbg_imu_loop; #define DEBUG_OUT_PERIOD_MS 100
extern volatile uint32_t dbg_can_loop;
void debug_task(void const * argument) void debug_task(void const * argument)
{ {
char line[128]; char line[384];
char tail[112];
char pitch_line[128];
for (;;) for (;;)
{ {
int len = 0; #if 0
#if defined(ROBOT_TYPE_GIMBAL) /* ===== 2026-10-15 台架 Pitch 调参遥测(已停用,保留备查) =====
len = snprintf(line, sizeof(line), * 5 列 CSV → Lissio FireWater 直接成 5 通道:
"app=%u can=%u imu=%u acc=%02X gyro=%02X bmi=%u ist=%u who=%02X " * setpoint(位置环设定值含台架偏置), actual(-att_deg_roll),
"cvg=%u ys=%d yaw=%.1f u3g=%02X i3=%u l3=%u d16=%u ref=%u/%u mot=%u\r\n", * out(最终电流), ecd_deg(编码器机械角), gyro_x_dps(陀螺 x 轴角速度) */
app_ready, dbg_can_loop, dbg_imu_loop, const float tune_ecd_deg = ((float)motor_pitch_gimbal.fdb.raw_angle -
bmi088_last_acc_id, bmi088_last_gyro_id, bmi088_online, (float)PITCH_GM6020_ENCODER_ZERO) * (360.0f / 8192.0f);
ist8310_online, ist8310_last_who, const float tune_gyro_dps = bmi088_gyro[0] * 57.29578f;
INS.ConvergeFlag, int len = snprintf(line, sizeof(line), "%.3f,%.3f,%.1f,%.3f,%.2f\n",
(int)gimbal_yaw_sign(), att_deg_yaw, gimbal_pitch_setpoint_deg(),
(unsigned)(huart3.gState), dbg_usart3_idle, dbg_usart3_last_len, -att_deg_roll,
dr16_frame_count, pitch_current,
referee.link_status, (unsigned)(HAL_GetTick() - referee.last_update_tick), tune_ecd_deg,
gimbal_motor_offline()); tune_gyro_dps);
#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);
#endif #endif
#if DEBUG_PITCH_ONLY
/* 仅输出 Pitch 调参数据(编码值、roll、PID最后输出值、遥控pitch杆量) */
int len = snprintf(line, sizeof(line),
"p_ecd=%u roll=%.2f p_out=%.1f rc_pitch=%u\r\n",
(unsigned)motor_pitch_gimbal.fdb.raw_angle,
att_deg_roll,
pitch_current,
(unsigned)dr16.rc_l_UD);
#else
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
#if DEBUG_CMD_OUT
/* 健康位分支是文本格式,控制量就按 key=value 追加,便于人读 */
char ctl[160];
snprintf(ctl, sizeof(ctl),
" online=%u src=%u spin=%u yaw_d=%.4f pitch_d=%.4f fric=%u fire=%u"
" eject=%u reset=%u s2=%u ctrl=%u wheel=%d",
(unsigned)cmd_robot.online, (unsigned)cmd_robot.source,
(unsigned)cmd_robot.spin_flag,
cmd_robot.gimbal_yaw_delta, cmd_robot.gimbal_pitch_delta,
(unsigned)cmd_robot.friction_on, (unsigned)cmd_robot.fire,
(unsigned)cmd_robot.eject, (unsigned)cmd_robot.gimbal_reset,
(unsigned)dr16.s2, (unsigned)dr16.key_push_ctrl,
(int)cmd_robot.thumb_wheel);
#else
const char * ctl = "";
#endif
int len = snprintf(line, sizeof(line),
"ecd=%u yaw=%.2f pitch=%.2f roll=%.2f cvg=%u rdy=%u imu=%u an=%.2f gn=%.3f%s%s\r\n",
(unsigned)motor_yaw_gimbal.fdb.raw_angle,
att_deg_yaw, att_deg_pitch, att_deg_roll,
(unsigned)INS.ConvergeFlag, (unsigned)att_ready,
(unsigned)bmi088_online, an, gn, tail, ctl);
#else
#if DEBUG_CMD_OUT
/* 纯数字 CSV。前 3 列保持原样(pitch_ecd, pitch_current, yaw_ecd),其余:
* online,src,spin, yaw_d,pitch_d, fric,fire,eject,reset, s2,ctrl,wheel,
* s1, mouse_rl,mouse_du,mouse_wheel,mouse_l,mouse_r, key_lo,key_hi,
* dr16_frame_count
* 末尾这 9 列是 DR16 帧内容诊断:key_lo/key_hi 就是帧里的 data[14]/data[15]。
* 按住 W 时若它们仍为 0,说明帧里本就没有键鼠数据(不是解析问题)。 */
int len = snprintf(line, sizeof(line),
"%u,%.1f,%u,%u,%u,%u,%.4f,%.4f,%u,%u,%u,%u,%u,%u,%d,%u,%d,%d,%d,%u,%u,%u,%u,%lu\r\n",
(unsigned)motor_pitch_gimbal.fdb.raw_angle,
pitch_current,
(unsigned)motor_yaw_gimbal.fdb.raw_angle,
(unsigned)cmd_robot.online,
(unsigned)cmd_robot.source,
(unsigned)cmd_robot.spin_flag,
cmd_robot.gimbal_yaw_delta,
cmd_robot.gimbal_pitch_delta,
(unsigned)cmd_robot.friction_on,
(unsigned)cmd_robot.fire,
(unsigned)cmd_robot.eject,
(unsigned)cmd_robot.gimbal_reset,
(unsigned)dr16.s2,
(unsigned)dr16.key_push_ctrl,
(int)cmd_robot.thumb_wheel,
(unsigned)dr16.s1,
(int)dr16.mouse_rl,
(int)dr16.mouse_du,
(int)dr16.mouse_wheel,
(unsigned)dr16.mouse_key_l,
(unsigned)dr16.mouse_key_r,
(unsigned)dr16.key_code_lo,
(unsigned)dr16.key_code_hi,
(unsigned long)dr16_frame_count);
#else
int len = snprintf(line, sizeof(line), "%u,%.1f,%u\r\n",
(unsigned)motor_pitch_gimbal.fdb.raw_angle,
pitch_current,(unsigned)motor_yaw_gimbal.fdb.raw_angle);
#endif
#endif
#endif
if (len > 0) if (len > 0)
CDC_Transmit_FS((uint8_t *)line, (uint16_t)len); CDC_Transmit_FS((uint8_t *)line, (uint16_t)len);
osDelay(500); osDelay(DEBUG_OUT_PERIOD_MS);
} }
} }
云台/Application/Task/Src/gimbal_task.c
+32 -4
View File
@@ -1,14 +1,14 @@
/* ============================================================================= /* =============================================================================
* gimbal_task.c - 云台控制任务(2ms) * gimbal_task.c - 云台控制任务(2ms/500Hz)
* 状态机: * 状态机:
* YAW_WAIT 等待"姿态就绪+云台电机在线"(遥控可后到) * YAW_WAIT 等待"姿态就绪+云台电机在线"(遥控可后到)
* YAW_RUN 正常串级控制(符号用参数回退值,直至手势自检完成) * YAW_RUN 正常串级控制(符号用参数回退值,直至手势自检完成)
* YAW_DETECT 装配符号自检——**仅由操作手手势显式触发,绝不自动执行**: * YAW_DETECT 装配符号自检——**仅由操作手手势显式触发,绝不自动执行**:
* 遥控在线时"左摇杆推到最左 + 右摇杆推到最右"保持 0.5s。 * 遥控在线时"左摇杆推到最左 + 右摇杆推到最右"保持 0.5s。
* 检测期间云台会轻转几度(约 20% 电压脉冲 60ms),操作手应 * 检测期间云台会轻转几度(约 20% 电流脉冲 60ms),操作手应
* 确认云台周围无人后再做手势。失败重试 3 次后回退参数符号。 * 确认云台周围无人后再做手势。失败重试 3 次后回退参数符号。
* 任意门控失效(遥控离线/电机掉线/姿态不可用/底盘故障)立即清零输出 * 任意门控失效(遥控离线/电机掉线/姿态不可用/底盘故障)立即清零输出
* 拨弹电流由 shoot_task 写入共享变量,本任务与 Pitch 共帧发送(0x1FF CAN1) * Yaw/Pitch GM6020 采用 0x1FE 电流帧控制;拨弹 M2006 由 shoot_task 独立发 0x200
* ========================================================================== */ * ========================================================================== */
#include "app_tasks.h" #include "app_tasks.h"
#include "cmsis_os.h" #include "cmsis_os.h"
@@ -26,7 +26,7 @@
#define CALIB_GESTURE_L_RL 400 // < 400 即最左 #define CALIB_GESTURE_L_RL 400 // < 400 即最左
#define CALIB_GESTURE_R_RL 1600 // > 1600 即最右 #define CALIB_GESTURE_R_RL 1600 // > 1600 即最右
#define CALIB_GESTURE_MS 500 // 保持时长 #define CALIB_GESTURE_MS 500 // 保持时长
#define CALIB_GESTURE_TICKS (CALIB_GESTURE_MS / 2) // 2ms 任务 #define CALIB_GESTURE_TICKS (CALIB_GESTURE_MS / 2) // 2ms 任务(500Hz)
typedef enum typedef enum
{ {
@@ -64,6 +64,34 @@ void gimbal_task(void const * argument)
if (gesture) if (gesture)
gesture_cnt = 0; gesture_cnt = 0;
#if 0
/* ===== 2026-10-15 台架 Pitch 阶跃发生器(已停用,保留备查) =====
* **有界**测试,不是持续抖动:只跑 GIMBAL_BENCH_PITCH_HALF_COUNT 个半周期,
* 跑完自动归零并锁存,之后云台恢复静止(重新上电/改开关才会再跑一次)。
* 配合 gimbal.c 的台架偏置使用;2ms 任务,半周期 tick 数 = HALF_MS / 2 */
{
static uint32_t bench_phase = 0;
static uint8_t bench_done = 0;
const uint32_t half_ticks = (uint32_t)GIMBAL_BENCH_PITCH_HALF_MS / 2u;
if (!bench_done && half_ticks > 0u)
{
const uint32_t total_ticks = half_ticks * (uint32_t)GIMBAL_BENCH_PITCH_HALF_COUNT;
if (bench_phase >= total_ticks)
{
bench_done = 1;
gimbal_bench_pitch_offset = 0.0f; /* 跑完立即停,不再抖动 */
}
else
{
gimbal_bench_pitch_offset = ((bench_phase / half_ticks) % 2u)
? GIMBAL_BENCH_PITCH_STEP_DEG
: 0.0f;
bench_phase++;
}
}
}
#endif
switch (state) switch (state)
{ {
case YAW_WAIT: case YAW_WAIT:
云台/Application/Task/Src/monitor_task.c
+22 -5
View File
@@ -8,6 +8,8 @@
* 连接音效 = 遥控器上线瞬间两声上扬(M3->H3) * 连接音效 = 遥控器上线瞬间两声上扬(M3->H3)
* CAN 掉线蜂鸣(legacy 语义): 按设备序号 n 响 n 下,2s 一轮,每次只报 * CAN 掉线蜂鸣(legacy 语义): 按设备序号 n 响 n 下,2s 一轮,每次只报
* 最先掉线的设备,设备序号表见 linkcheck_first_offline() * 最先掉线的设备,设备序号表见 linkcheck_first_offline()
* 调试模式(DEBUG_BUZZER_MELODY=1,见 robot_param.h): 上述蜂鸣改为循环
* 播放旋律,避免台架调试时连续滴滴声;红灯常亮等灯语不受影响
* 看门狗: IWDG 约 2.5s;上电 5s 宽限期无条件喂,之后仅当全部任务心跳正常 * 看门狗: IWDG 约 2.5s;上电 5s 宽限期无条件喂,之后仅当全部任务心跳正常
* 才喂——任何任务卡死都会复位,不会整车失控 * 才喂——任何任务卡死都会复位,不会整车失控
* ========================================================================== */ * ========================================================================== */
@@ -83,8 +85,8 @@ static int8_t linkcheck_first_offline(void)
{ {
uint32_t now = HAL_GetTick(); uint32_t now = HAL_GetTick();
#if defined(ROBOT_TYPE_GIMBAL) #if defined(ROBOT_TYPE_GIMBAL)
/* 设备序号:0 摩擦轮L(0x201) 1 摩擦轮R(0x202) 2 Pitch(0x206) /* 设备序号:0 摩擦轮L(CAN1 0x201) 1 摩擦轮R(CAN1 0x202) 2 Pitch(CAN1 0x206)
* 3 Yaw(0x205) 4 拨弹(0x207,安装时) 5 底盘回传(0x019) */ * 3 Yaw(CAN2 0x205) 4 拨弹(CAN2 0x201,安装时) 5 底盘回传(0x019) */
if (LINK_OFFLINE(motor_friction_l.fdb.last_online_tick)) if (LINK_OFFLINE(motor_friction_l.fdb.last_online_tick))
return 0; return 0;
if (LINK_OFFLINE(motor_friction_r.fdb.last_online_tick)) if (LINK_OFFLINE(motor_friction_r.fdb.last_online_tick))
@@ -170,10 +172,24 @@ void LED_Task(void const * argument)
if (can_alarm) if (can_alarm)
{ {
led_set_red(1); 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 窗口 uint8_t w = counter % 20; // 2s 窗口
/* 第 j 声:占 w=[2j,2j+1],响 100ms 停 100ms */ /* 第 j 声:占 w=[2j,2j+1],响 100ms 停 100ms */
if ((w / 2) < n && (w % 2) == 0) if ((w / 2) < n && (w % 2) == 0)
@@ -183,6 +199,7 @@ void LED_Task(void const * argument)
} }
else else
buzzer_off(); buzzer_off();
#endif
} }
else else
{ {
云台/Application/Task/Src/robot_cmd_task.c
+102 -19
View File
@@ -1,6 +1,6 @@
/* ============================================================================= /* =============================================================================
* robot_cmd_task.c - 控制源汇总(2ms),发布 cmd_robot_t(robot_typedef.h) * robot_cmd_task.c - 控制源汇总(2ms),发布 cmd_robot_t(robot_typedef.h)
* 云台板: DR16 遥控器 + 图传键鼠(0x0304,referee 键盘钩子填充 keyboard_data) * 云台板: DR16 遥控器 + 键鼠(0x0304 优先;USART3 的 18 字节帧里同样带键鼠,两路已合并,见 KB_FROM_DBUS_FRAME)
* 键鼠/遥控互斥(与 legacy 一致);摇杆->云台增量与限位门控; * 键鼠/遥控互斥(与 legacy 一致);摇杆->云台增量与限位门控;
* Q 摩擦轮/左键发射/E 退弹/拨轮;首帧触发 gimbal_reset * Q 摩擦轮/左键发射/E 退弹/拨轮;首帧触发 gimbal_reset
* 底盘板: 从 ctoc_remote(0x149/0x189 解析结果)生成: * 底盘板: 从 ctoc_remote(0x149/0x189 解析结果)生成:
@@ -31,6 +31,9 @@ volatile cmd_robot_t cmd_robot;
#if defined(ROBOT_TYPE_GIMBAL) #if defined(ROBOT_TYPE_GIMBAL)
keyboard_data_t keyboard_data; keyboard_data_t keyboard_data;
/* 键鼠来源合并视图(声明见 robot_cmd_task.h):云台控制与 c_comm_task 底盘摇杆合成共用 */
keyboard_data_t keyboard_merged;
uint8_t keyboard_merged_online = 0;
static uint8_t keyboard_online = 0; static uint8_t keyboard_online = 0;
void dr16_frame_callback(const uint8_t * frame, uint16_t len) void dr16_frame_callback(const uint8_t * frame, uint16_t len)
@@ -74,6 +77,15 @@ static void referee_keyboard_hook(const uint8_t data[12])
keyboard_data.last_tick = HAL_GetTick(); keyboard_data.last_tick = HAL_GetTick();
} }
/* ---- 键鼠输入来源(2026-10-16 修) ----
* 背景:客户端键鼠是从 USART3 那 18 字节帧进来的(data[6..15]),dr16_parse 早就
* 解析进 dr16.mouse_rl / dr16.key_w 等字段,但控制逻辑只读 0x0304 填的
* keyboard_data,于是"键鼠控制完全没用"。此开关让两路合并。
* 1 = 除 0x0304 外同时接受帧内键鼠:见到键鼠动作即进入键鼠模式并保持,
* 直到操作手去动云台左摇杆才交还遥控器(右摇杆在底盘,键鼠模式下仍要能用)。
* 0 = 旧行为:只有 0x0304 在线才走键鼠。 */
#define KB_FROM_DBUS_FRAME 1
void robot_cmd_task(void const * argument) void robot_cmd_task(void const * argument)
{ {
referee_set_keyboard_hook(referee_keyboard_hook); referee_set_keyboard_hook(referee_keyboard_hook);
@@ -81,13 +93,80 @@ void robot_cmd_task(void const * argument)
while (!app_ready) while (!app_ready)
osDelay(2); osDelay(2);
/* 键鼠模式状态:帧内键鼠动作置 1,左摇杆动作清 0 */
static uint8_t kb_mode = 0;
for (;;) for (;;)
{ {
keyboard_online = (keyboard_data.last_tick != 0 && keyboard_online = (keyboard_data.last_tick != 0 &&
HAL_GetTick() - keyboard_data.last_tick < DEVICE_OFFLINE_TIME_MS * 10); HAL_GetTick() - keyboard_data.last_tick < DEVICE_OFFLINE_TIME_MS * 10);
#if KB_FROM_DBUS_FRAME
{
uint8_t kb_act = 0, rc_act = 0;
/* 键鼠动作:鼠标移动 / 滚轮 / 左右键 / 任意按键(data[14]或data[15]非 0) */
if (dr16.mouse_rl > 1 || dr16.mouse_rl < -1) kb_act = 1;
if (dr16.mouse_du > 1 || dr16.mouse_du < -1) kb_act = 1;
if (dr16.mouse_wheel != 0) kb_act = 1;
if (dr16.mouse_key_l || dr16.mouse_key_r) kb_act = 1;
if (dr16.key_code_lo || dr16.key_code_hi) kb_act = 1;
/* 遥控器动作:只认云台左摇杆 */
if (dr16.rc_l_UD > 1050 || dr16.rc_l_UD < 1000) rc_act = 1;
if (dr16.rc_l_RL > 1050 || dr16.rc_l_RL < 1000) rc_act = 1;
if (kb_act) kb_mode = 1;
if (rc_act) kb_mode = 0;
}
#endif
/* 是否键鼠在控:0x0304 在线时始终算键鼠(保持原有优先级) */
uint8_t kb_active = (keyboard_online || kb_mode) ? 1 : 0;
/* ---- 合并成共享视图 keyboard_merged ----
* 0x0304 在线 → 整份用 keyboard_data;否则用 USART3 帧里那一套。
* 云台控制、发射机构、以及 c_comm_task 里 WASD→底盘摇杆的合成都读这个
* 视图,避免两处各判一次导致"云台跟鼠标了、底盘却还在收遥控器原始摇杆"。 */
if (keyboard_online)
{
keyboard_merged = keyboard_data;
}
else
{
keyboard_merged.mouse_rl = dr16.mouse_rl;
keyboard_merged.mouse_du = dr16.mouse_du;
keyboard_merged.mouse_wheel = dr16.mouse_wheel;
keyboard_merged.mouse_key_l = dr16.mouse_key_l;
keyboard_merged.mouse_key_r = dr16.mouse_key_r;
keyboard_merged.key_w = dr16.key_w;
keyboard_merged.key_s = dr16.key_s;
keyboard_merged.key_a = dr16.key_a;
keyboard_merged.key_d = dr16.key_d;
keyboard_merged.key_shift = dr16.key_shift;
keyboard_merged.key_ctrl = dr16.key_ctrl;
keyboard_merged.key_q = dr16.key_q;
keyboard_merged.key_e = dr16.key_e;
/* alpha 键:dr16 侧那两位恒为 0(见 dr16.c 的 >>9/>>10),照搬保持语义 */
keyboard_merged.key_alpha1 = dr16.key_alpha1;
keyboard_merged.key_alpha2 = dr16.key_alpha2;
keyboard_merged.key_push_q = dr16.key_push_q;
keyboard_merged.key_push_e = dr16.key_push_e;
keyboard_merged.key_push_shift = dr16.key_push_shift;
keyboard_merged.key_push_ctrl = dr16.key_push_ctrl;
keyboard_merged.last_tick = dr16_last_tick;
}
keyboard_merged_online = kb_active;
/* 控制逻辑用的简写(取自共享视图) */
int16_t kb_mouse_rl = keyboard_merged.mouse_rl;
int16_t kb_mouse_du = keyboard_merged.mouse_du;
uint8_t kb_mouse_l = keyboard_merged.mouse_key_l;
uint8_t kb_push_q = keyboard_merged.key_push_q;
uint8_t kb_push_e = keyboard_merged.key_push_e;
cmd_robot.online = (dr16_status == 1 && !dr16_offline()); cmd_robot.online = (dr16_status == 1 && !dr16_offline());
cmd_robot.source = keyboard_online ? 1 : 0; cmd_robot.source = kb_active ? 1 : 0;
if (cmd_robot.online) if (cmd_robot.online)
{ {
@@ -95,10 +174,10 @@ void robot_cmd_task(void const * argument)
float yaw_delta = 0, pitch_delta = 0; float yaw_delta = 0, pitch_delta = 0;
uint16_t pos = motor_pitch_gimbal.fdb.raw_angle; uint16_t pos = motor_pitch_gimbal.fdb.raw_angle;
if (!keyboard_online) if (!kb_active)
{ {
/* 遥控器:左摇杆,输出与摇杆偏量成比例(legacy 为过死区即全速, /* 遥控器:左摇杆向上(>1050)为抬头(pitch_delta减小,pos向2260减小),门控须 pos > UPPER_LIMIT(2260);
* 体感过灵敏),限位门控与方向保持 legacy */ * 左摇杆向下(<1000)为低头(pitch_delta增大,pos向3950增大),门控须 pos < LOWER_LIMIT(3950) */
if (dr16.rc_l_UD > 1050 && pos > PITCH_GM6020_POSITION_UPPER_LIMIT) if (dr16.rc_l_UD > 1050 && pos > PITCH_GM6020_POSITION_UPPER_LIMIT)
pitch_delta -= GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f * pitch_delta -= GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f *
((float)dr16.rc_l_UD - 1024.0f) / 660.0f; ((float)dr16.rc_l_UD - 1024.0f) / 660.0f;
@@ -113,24 +192,27 @@ void robot_cmd_task(void const * argument)
} }
else else
{ {
/* 键鼠:同改为比例输出 */ /* 键鼠:鼠标前推(>8)为抬头(pitch_delta减小),门控须 pos > UPPER_LIMIT;鼠标后拉(<-8)为低头(pitch_delta增大),门控须 pos < LOWER_LIMIT */
if (keyboard_data.mouse_du < -8 && pos > PITCH_GM6020_POSITION_UPPER_LIMIT) if (kb_mouse_du > 8 && pos > PITCH_GM6020_POSITION_UPPER_LIMIT)
pitch_delta -= GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f * pitch_delta -= GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f *
(-(float)keyboard_data.mouse_du * PC_MOUSE_DU_SENSITIVITY) / 660.0f; ((float)kb_mouse_du * PC_MOUSE_DU_SENSITIVITY) / 660.0f;
else if (keyboard_data.mouse_du > 8 && pos < PITCH_GM6020_POSITION_LOWER_LIMIT) else if (kb_mouse_du < -8 && pos < PITCH_GM6020_POSITION_LOWER_LIMIT)
pitch_delta += GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f * pitch_delta += GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f *
((float)keyboard_data.mouse_du * PC_MOUSE_DU_SENSITIVITY) / 660.0f; (-(float)kb_mouse_du * PC_MOUSE_DU_SENSITIVITY) / 660.0f;
if (keyboard_data.mouse_rl != 0) if (kb_mouse_rl != 0)
yaw_delta -= GIMBAL_LEVER_SPEED_MAP_RATE * GIMBAL_YAW_PITCH_SPEED_RATE * yaw_delta -= GIMBAL_LEVER_SPEED_MAP_RATE * GIMBAL_YAW_PITCH_SPEED_RATE *
GIMBAL_YAW_PITCH_SPEED_RATE * 0.0439453125f * GIMBAL_YAW_PITCH_SPEED_RATE * 0.0439453125f *
((float)keyboard_data.mouse_rl * PC_MOUSE_RL_SENSITIVITY / 660.0f * 2.0f); ((float)kb_mouse_rl * PC_MOUSE_RL_SENSITIVITY / 660.0f * 2.0f);
} }
cmd_robot.gimbal_yaw_delta = yaw_delta; cmd_robot.gimbal_yaw_delta = yaw_delta;
cmd_robot.gimbal_pitch_delta = pitch_delta; cmd_robot.gimbal_pitch_delta = pitch_delta;
/* ---- 发射机构 ---- */ /* ---- 发射机构 ----
if (!keyboard_online) * 键鼠模式用 Q / 鼠标左键 / E(两路来源已合并进 kb_*,0x0304 优先);
* 遥控模式用 S1 与拨轮。kb_push_* 是"按下翻转"的常驻状态,
* 且键鼠模式只在动左摇杆时才退出,所以摩擦轮/E 的状态不会自己掉。 */
if (!kb_active)
{ {
cmd_robot.friction_on = (dr16.s1 == 1); cmd_robot.friction_on = (dr16.s1 == 1);
cmd_robot.fire = (dr16.thumb_wheel < 1000); cmd_robot.fire = (dr16.thumb_wheel < 1000);
@@ -138,9 +220,9 @@ void robot_cmd_task(void const * argument)
} }
else else
{ {
cmd_robot.friction_on = keyboard_data.key_push_q; cmd_robot.friction_on = kb_push_q;
cmd_robot.fire = keyboard_data.mouse_key_l; cmd_robot.fire = kb_mouse_l;
cmd_robot.eject = keyboard_data.key_push_e; cmd_robot.eject = kb_push_e;
} }
cmd_robot.thumb_wheel = dr16.thumb_wheel; cmd_robot.thumb_wheel = dr16.thumb_wheel;
} }
@@ -153,12 +235,13 @@ void robot_cmd_task(void const * argument)
cmd_robot.eject = 0; cmd_robot.eject = 0;
} }
/* 小陀螺使能:Ctrl 沿翻转(仅在遥控源检测,键鼠 TODO); /* 小陀螺使能:Ctrl 沿翻转。键鼠模式下取键鼠那一路的 Ctrl(0x0304 的
* keyboard_data 或帧内 dr16.key_push_ctrl),遥控模式用遥控器 Ctrl。
* 云台侧是数据源头,底盘只收状态位——重连瞬间保持态跳变不会在 * 云台侧是数据源头,底盘只收状态位——重连瞬间保持态跳变不会在
* 底盘误触发沿(修复"重连误开小陀螺疯转") */ * 底盘误触发沿(修复"重连误开小陀螺疯转") */
{ {
static uint8_t spin_flag = 0, last_ctrl = 0; static uint8_t spin_flag = 0, last_ctrl = 0;
uint8_t ctrl_now = (!keyboard_online) ? dr16.key_push_ctrl : 0; uint8_t ctrl_now = keyboard_online ? keyboard_data.key_push_ctrl : dr16.key_push_ctrl;
if (last_ctrl == 0 && ctrl_now == 1) if (last_ctrl == 0 && ctrl_now == 1)
spin_flag = !spin_flag; spin_flag = !spin_flag;
last_ctrl = ctrl_now; last_ctrl = ctrl_now;
云台/Application/Task/Src/shoot_task.c
+2 -2
View File
@@ -1,7 +1,7 @@
/* ============================================================================= /* =============================================================================
* shoot_task.c - 发射机构任务(2ms) * shoot_task.c - 发射机构任务(2ms)
* shoot_control() 计算摩擦轮/拨弹电流 -> shoot_send_cmd() 发 0x200; * shoot_control() 计算摩擦轮/拨弹电流 -> shoot_send_cmd() 发 0x200(CAN1 摩擦轮 / CAN2 拨弹);
* 拨弹电流由云台任务与 Pitch 共帧发送;发射机构上电状态来自底盘 0x019 * 发射机构上电状态来自底盘 0x019
* ========================================================================== */ * ========================================================================== */
#include "app_tasks.h" #include "app_tasks.h"
#include "cmsis_os.h" #include "cmsis_os.h"
云台/Application/robot_param.h
+85 -19
View File
@@ -9,9 +9,11 @@
#define ROBOT_PARAM_H #define ROBOT_PARAM_H
/* ============================ 云台 ============================ */ /* ============================ 云台 ============================ */
#define PITCH_GM6020_POSITION_LOWER_LIMIT 3990 // Pitch 轴编码器值下限(云台抬起方向) #define PITCH_GM6020_POSITION_UPPER_LIMIT 2260 // Pitch 轴抬起极限(最高位,编码器较小值;实测2203撞死限位,留余量设2260)
#define PITCH_GM6020_POSITION_UPPER_LIMIT 2220 // Pitch 轴编码器值上限(云台压低方向) #define PITCH_GM6020_POSITION_LOWER_LIMIT 3950 // Pitch 轴压低极限(最低位,编码器较大值;实测4234撞死限位,留余量设3950)
#define PITCH_GM6020_ENCODER_ZERO 3245 // Pitch 轴编码器零位 #define PITCH_GM6020_ENCODER_ZERO 3200 // Pitch 轴编码器水平零位
#define YAW_GM6020_ENCODER_ZERO 7730 // Yaw 轴编码器零位(机械正前方,0~8191)
#define CHASSIS_SPIN_FEEDFORWARD_K 0.008f // 小陀螺解算时延前馈补偿系数(s)
#define GIMBAL_FRICTION_WHEEL_SPEED 5500.0f // 摩擦轮转速(RPM) #define GIMBAL_FRICTION_WHEEL_SPEED 5500.0f // 摩擦轮转速(RPM)
#define GIMBAL_RAMMER_SPEED -5400.0f// 拨弹盘转速(RPM,负向发射) #define GIMBAL_RAMMER_SPEED -5400.0f// 拨弹盘转速(RPM,负向发射)
@@ -29,35 +31,58 @@
* 这是反复"疯转"的根源,已废弃; * 这是反复"疯转"的根源,已废弃;
* - 现在两环共用陀螺,整条链只有一个装配符号,由上电自检自动测定 * - 现在两环共用陀螺,整条链只有一个装配符号,由上电自检自动测定
* (GIMBAL_YAW_DETECT 见下),无需人工猜; * (GIMBAL_YAW_DETECT 见下),无需人工猜;
* - 单位:角度环 误差° -> 目标角速度°/s;速度环 误差°/s -> 电压。 * - 单位:角度环 误差° -> 目标角速度°/s;速度环 误差°/s -> 电流。
* 调参顺序:先速度环(点动不振荡),再位置环(刚性)——见移植 README §8.1 */ * 调参顺序:先速度环(点动不振荡),再位置环(刚性)——见移植 README §8.1 */
#define GIMBAL_YAW_ANGLE_KP 15.0f // ° -> °/s:10° 误差给 150°/s #define GIMBAL_YAW_ANGLE_KP 20.0f // ° -> °/s:10° 误差给 150°/s
#define GIMBAL_YAW_ANGLE_KI 0.0f #define GIMBAL_YAW_ANGLE_KI 0.0f
#define GIMBAL_YAW_ANGLE_KD 0.0f // 速度环本身即微分,不再叠加 #define GIMBAL_YAW_ANGLE_KD 0.0f // 速度环本身即微分,不再叠加
#define GIMBAL_YAW_ANGLE_EK_DEADBAND 0.05f // 误差死区(°),QEKF 噪声小可用窄死区 #define GIMBAL_YAW_ANGLE_EK_DEADBAND 0.05f // 误差死区(°),QEKF 噪声小可用窄死区
#define GIMBAL_YAW_ANGLE_OUT_LIMIT 360.0f // 目标角速度上限(°/s) #define GIMBAL_YAW_ANGLE_OUT_LIMIT 360.0f // 目标角速度上限(°/s)
#define GIMBAL_YAW_SPEED_KP 60.0f // °/s -> 电压:100°/s 误差给 6000(约 24% 满电压) #define GIMBAL_YAW_SPEED_KP 85.0f // °/s -> 电流:100°/s 误差给 6000(约 36% 满电流)
#define GIMBAL_YAW_SPEED_KI 0.0f // 稳定后可回填 0.5~2 消速度稳态差 #define GIMBAL_YAW_SPEED_KI 0.0f // 稳定后可回填 0.5~2 消速度稳态差
#define GIMBAL_YAW_SPEED_KD 0.0f // 陀螺速率反馈噪声小,先不用 D #define GIMBAL_YAW_SPEED_KD 0.0f // 陀螺速率反馈噪声小,先不用 D
#define GIMBAL_YAW_SPEED_OUT_LIMIT 25000.0f// GM6020 电压限幅 #define GIMBAL_YAW_SPEED_OUT_LIMIT 16384.0f// GM6020 电流限幅(±16384 对应 ±3A)
#define GIMBAL_YAW_SPIN_FEEDFORWARD_K 24.0f // 小陀螺底盘自旋解耦前馈系数(RPM->电流)
/* Yaw 装配符号自动检测:上电自检时给 Yaw 电机一个正向小电压脉冲, /* Yaw 装配符号自动检测:上电自检时给 Yaw 电机一个正向小电流脉冲,
* 看陀螺 z 轴往哪边转——脉冲引起同向转动则符号 +1,反向则 -1。 * 看陀螺 z 轴往哪边转——脉冲引起同向转动则符号 +1,反向则 -1。
* 检测时云台会轻转几度,勿扶云台;失败(被按住/电机离线)时用 FALLBACK */ * 检测时云台会轻转几度,勿扶云台;失败(被按住/电机离线)时用 FALLBACK */
#define GIMBAL_YAW_SIGN_AUTO 1 // 1=上电自动检测 0=直接用 FALLBACK #define GIMBAL_YAW_SIGN_AUTO 1 // 1=上电自动检测 0=直接用 FALLBACK
#define GIMBAL_YAW_SIGN_FALLBACK (1.0f) #define GIMBAL_YAW_SIGN_FALLBACK (1.0f)
#define GIMBAL_YAW_DETECT_VOLTAGE 5000 // 检测脉冲电压(约 20% 满电压) #define GIMBAL_YAW_DETECT_CURRENT 3500 // 检测脉冲电流(约 21% 满电流 16384)
#define GIMBAL_YAW_DETECT_RATE_MIN 20.0f // 判定转动的陀螺速率阈值(°/s) #define GIMBAL_YAW_DETECT_RATE_MIN 20.0f // 判定转动的陀螺速率阈值(°/s)
/* ---- Pitch(位置环陀螺 Roll,速度环转子转速——Pitch 无符号史问题,维持原案) ---- */ /* ---- Pitch(位置环陀螺 Roll,速度环转子转速——Pitch 无符号史问题,维持原案) ---- */
#define GIMBAL_PITCH_ANGLE_EK_DEADBAND 0.3f // 位置环误差死区(°) #define GIMBAL_PITCH_ANGLE_EK_DEADBAND 0.05f // 误差死区(°),收窄死区消除边缘阶跃
#define GIMBAL_PITCH_ANGLE_KP 15.0f #define GIMBAL_PITCH_ANGLE_KP 15.0f
#define GIMBAL_PITCH_ANGLE_KI 0.0f #define GIMBAL_PITCH_ANGLE_KI 0.0f
#define GIMBAL_PITCH_ANGLE_KD 100.0f #define GIMBAL_PITCH_ANGLE_KD 0.0f // 原100.0f是万级电流振颤主因!清零以消除剧烈振荡
#define GIMBAL_PITCH_SPEED_KP 150.0f #define GIMBAL_PITCH_SPEED_KP 40.0f // 速度/电流环Kp
#define GIMBAL_PITCH_SPEED_KI 0.001f #define GIMBAL_PITCH_SPEED_KI 0.0f
#define GIMBAL_PITCH_SPEED_KD 5.0f #define GIMBAL_PITCH_SPEED_KD 0.0f
/* Pitch 重力拟合前馈参数: I_g = K_COS * cos(a) + K_SIN * sin(a) + K_CONST
* 其中 a = (raw_angle - ZERO_ECD) * 360 / 8192 (°) */
#define GIMBAL_PITCH_GRAVITY_ZERO_ECD 3200.0f // 重力补偿拟合基准编码值
#define GIMBAL_PITCH_GRAVITY_K_COS 2856.1f// cos 项系数
#define GIMBAL_PITCH_GRAVITY_K_SIN 4340.3f// sin 项系数
#define GIMBAL_PITCH_GRAVITY_K_CONST 2826.5f // 常数偏置项
/* ---- 2026-10-15 台架前馈实测记录(未采用,保留备查) ----
* 用上位机 203Hz 遥测在编码器 -16.91°/+4.88°/+9.30° 三处长时间静止(编码器
* 零漂移+目标恒定)实测真实保持电流 3597/4498/4613 counts;当时上面这组参数
* 分别高估 699/1543/1733 counts,折合 1.17~2.89° 稳态静差
* (位置环 Kp15×Kp40 = 600 counts/°:静差 = 前馈高估量 / 600)。
* 按"保留原相位与形状(K_COS:K_SIN 不变)、幅值×0.503 并微调常数项"重标定为:
* GIMBAL_PITCH_GRAVITY_K_COS 1435.4f
* GIMBAL_PITCH_GRAVITY_K_SIN 2181.3f
* GIMBAL_PITCH_GRAVITY_K_CONST 2861.6f
* 该组在当次数据上残差 RMS 16 counts(0.03°),看似很准;**但换到另一机械状态后
* 同一编码器角度实测保持电流又差约 1650 counts**,说明被标定对象本身在变
* (见上方"机械滞回 2130 counts"记录),故不采用,前馈保持原值不动。
* 另一并记录:PID 动态特性实测良好(上升时间 0.095~0.14s、超调 0.3~5.2%、
* 16s 稳态段纹波 p-p 0.046°、无振荡),故 GIMBAL_PITCH_*_KP/KI/KD 也未改动。 */
#define GIMBAL_FRICTION_WHEEL_KP 16.0f #define GIMBAL_FRICTION_WHEEL_KP 16.0f
#define GIMBAL_FRICTION_WHEEL_KI 0.0f #define GIMBAL_FRICTION_WHEEL_KI 0.0f
@@ -91,10 +116,10 @@
* err = wrap180(云台yaw - 底盘yaw) * err = wrap180(云台yaw - 底盘yaw)
* 不再依赖底盘侧 GM6020(0x205) 反馈,皮带虚位不在环内,无基准锁定时序坑。 * 不再依赖底盘侧 GM6020(0x205) 反馈,皮带虚位不在环内,无基准锁定时序坑。
* 两板 INS 各自上电零点任意,但差值只随相对转动变化,天然正确 */ * 两板 INS 各自上电零点任意,但差值只随相对转动变化,天然正确 */
#define CHASSIS_FOLLOW_KP 0.4f // w 量纲/°:10° 偏差给 w=4 #define CHASSIS_FOLLOW_KP 0.4678f // w 量纲/°:10° 偏差给 w=4
#define CHASSIS_FOLLOW_KI 0.0f // 调试期 0,同旧教训(积分残留) #define CHASSIS_FOLLOW_KI 0.0f // 调试期 0,同旧教训(积分残留)
#define CHASSIS_FOLLOW_KD 0.0f #define CHASSIS_FOLLOW_KD 24.452f
#define CHASSIS_FOLLOW_OUT_LIMIT 4.0f // 输出限幅(介于慢/快陀螺之间) #define CHASSIS_FOLLOW_OUT_LIMIT 5.5f // 输出限幅(介于慢/快陀螺之间)
#define CHASSIS_FOLLOW_DEADBAND 1.0f // 误差死区(°) #define CHASSIS_FOLLOW_DEADBAND 1.0f // 误差死区(°)
/* 跟随符号全自动综合:云台自检(0x1A9 flags)下发编码器 tick 增向, /* 跟随符号全自动综合:云台自检(0x1A9 flags)下发编码器 tick 增向,
@@ -114,10 +139,11 @@
#define CTOC_SLAVE_ID1 0x149 // 云台->底盘:摇杆数据 #define CTOC_SLAVE_ID1 0x149 // 云台->底盘:摇杆数据
#define CTOC_SLAVE_ID2 0x189 // 云台->底盘:控制状态+故障/陀螺使能位 #define CTOC_SLAVE_ID2 0x189 // 云台->底盘:控制状态+故障/陀螺使能位
#define CTOC_SLAVE_ID3 0x1A9 // 云台->底盘:云台世界系 yaw(float)+att_ready #define CTOC_SLAVE_ID3 0x1A9 // 云台->底盘:云台世界系 yaw(float)+att_ready
#define CTOC_SLAVE_ID4 0x1B9 // 云台->底盘:自定义UI 专用(pitch + 摩擦轮 L/R 转速)
/* ============================ CAN ID ============================ */ /* ============================ CAN ID ============================ */
/* 云台板:CAN1 = Pitch GM6020(0x206)+摩擦轮 M3508(0x201/0x202) /* 云台板:CAN1 = Pitch GM6020(0x206,0x1FE)+摩擦轮 M3508(0x201/0x202,0x200)
* CAN2 = Yaw GM6020(0x205)+底盘回传(0x019) * CAN2 = Yaw GM6020(0x205,0x1FE)+拨弹 M2006(0x201,0x200)+底盘回传(0x019)
* 底盘板:CAN1 = 四轮 M3508(0x201~0x204) * 底盘板:CAN1 = 四轮 M3508(0x201~0x204)
* CAN2 = 云台数据(0x149/0x189/0x1A9) */ * CAN2 = 云台数据(0x149/0x189/0x1A9) */
@@ -125,6 +151,46 @@
#define DEBUG_SHOOT_WITHOUT_REFEREE 1 // 1=无裁判系统时也允许开摩擦轮(台架调试用),比赛请改 0 #define DEBUG_SHOOT_WITHOUT_REFEREE 1 // 1=无裁判系统时也允许开摩擦轮(台架调试用),比赛请改 0
#define GIMBAL_RAMMER_INSTALLED 0 // 1=拨弹 M2006 已安装;0=暂缺,不参与掉线判定 #define GIMBAL_RAMMER_INSTALLED 0 // 1=拨弹 M2006 已安装;0=暂缺,不参与掉线判定
/* ================= 2026-10-15 Pitch 台架 PID 调参改动(已停用,保留备查) =================
* 以下开关是一次台架 Pitch 调参试验所加,已用 #if 0 停用:不参与编译、不影响现有行为。
* 需要复现时把外层 #if 0 改成 #if 1,并同步启用 gimbal.c / gimbal_task.c / gimbal.h
* / debug_task.c 里同名的停用块。
* 启用前务必先确认机械无卡滞:当时实测**同一编码器角度下保持电流可差 2130 counts
* (≈3.55° 当量)**,属机械滞回(线缆拖拽/接触/摩擦),不是控制问题;机械不一致时
* 任何固定前馈都标不准,调参也无意义。 */
#if 0
/* ---- Pitch 台架 PID 调参(仅台架;调完/比赛必须置 0) ----
* 1=在 Pitch 位置环设定值上叠加一个方波阶跃,用于定量观测阶跃响应。
* 安全约束(务必遵守):
* - **有界**:只跑 GIMBAL_BENCH_PITCH_HALF_COUNT 个半周期就自动归零停住,
* 不做长时间连续抖动;
* - **限位内**:轴距上下软限位不足 GIMBAL_BENCH_PITCH_SAFE_MARGIN_DEG 时
* 不施加阶跃,绝不把轴推向限位;
* - 该偏置是纯附加量,不累加进 pitch_target_deg,关掉即完全恢复原行为。 */
#define GIMBAL_BENCH_PITCH_TUNE 0
#define GIMBAL_BENCH_PITCH_STEP_DEG 3.0f // 阶跃幅值(°)
#define GIMBAL_BENCH_PITCH_HALF_MS 800 // 半周期(ms),(跑完自动停,非持续抖动)
#define GIMBAL_BENCH_PITCH_HALF_COUNT 4 // 总半周期数:4×0.8s=3.2s 后自动静止
#define GIMBAL_BENCH_PITCH_SAFE_MARGIN_DEG 5.0f // 距上下限位至少留这么多才允许阶跃
/* Pitch 软限位外的电流限制(方向 + 堵转双层保护):
* - 压向限位的方向一律清零(绝不顶限位);
* - 离开限位的方向允许 GIMBAL_PITCH_LIMIT_CURRENT_RECOVER,让轴能自行归位
* (烧录/上电会短暂失力矩,轴会掉到限位,必须能自己爬起来);
* - 但限位外若连续 GIMBAL_PITCH_LIMIT_STALL_MS 编码器一动不动,判为堵转/
* 撞死,允许电流降到 GIMBAL_PITCH_LIMIT_CURRENT,避免长时间过载发热。
* 轴回到软限位内后闭环自动恢复正常输出。 */
#define GIMBAL_PITCH_LIMIT_CURRENT_RECOVER 12000.0f // 限位外抬升方向允许电流(≈2.2A,额定3A的73%)
#define GIMBAL_PITCH_LIMIT_CURRENT 1500.0f // 限位外判定堵转后的涓流上限(≈0.27A)
#define GIMBAL_PITCH_LIMIT_STALL_MS 300 // 限位外不动多久判定堵转(高位电流只持续这么久)
#endif
/* ================= 以上为停用块,以下为原有代码 ================= */
/* 1=把 CAN 掉线警告音替换成循环播放的旋律《你》(RM动画片尾曲),报警时播放;
* 旋律音符表见 modules/dev/buzzer.c 的 buzzer_melody[] */
#define DEBUG_BUZZER_MELODY 0
#define BUZZER_MELODY_SPEED_DIV 1 // 旋律降速分频(默认1:每步100ms精准匹配原版带吐音断音节拍)
/* ============================ 掉线检测 ============================ */ /* ============================ 掉线检测 ============================ */
#define DEVICE_OFFLINE_TIME_MS 100 // CAN 设备/裁判系统掉线判定时间 #define DEVICE_OFFLINE_TIME_MS 100 // CAN 设备/裁判系统掉线判定时间
#define CTOC_OFFLINE_TIME_MS 100 // 板间通讯掉线判定时间 #define CTOC_OFFLINE_TIME_MS 100 // 板间通讯掉线判定时间
云台/Core/Src/gpio.c
+10 -2
View File
@@ -102,9 +102,17 @@ void MX_GPIO_Init(void)
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(CS1_ACCEL_GPIO_Port, &GPIO_InitStruct); 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.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; GPIO_InitStruct.Pull = GPIO_PULLUP;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
云台/MDK-ARM/Simba-ELCT-FrameWork.uvoptx
+24 -19
View File
@@ -114,9 +114,14 @@
<tDlgDll></tDlgDll> <tDlgDll></tDlgDll>
<tDlgPa></tDlgPa> <tDlgPa></tDlgPa>
<tIfile></tIfile> <tIfile></tIfile>
<pMon>BIN\CMSIS_AGDI.dll</pMon> <pMon>Segger\JL2CM3.dll</pMon>
</DebugOpt> </DebugOpt>
<TargetDriverDllRegistry> <TargetDriverDllRegistry>
<SetRegEntry>
<Number>0</Number>
<Key>JL2CM3</Key>
<Name>-U20090928 -O78 -S2 -ZTIFSpeedSel5000 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST0 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -TB1 -TFE0 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
</SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>ARMRTXEVENTFLAGS</Key> <Key>ARMRTXEVENTFLAGS</Key>
@@ -140,7 +145,7 @@
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
<Key>CMSIS_AGDI</Key> <Key>CMSIS_AGDI</Key>
<Name>-X"" -O206 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name> <Name>-X"Any" -UAny -O206 -S8 -C0 -P00000000 -N00("") -D00(00000000) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
</SetRegEntry> </SetRegEntry>
<SetRegEntry> <SetRegEntry>
<Number>0</Number> <Number>0</Number>
@@ -1040,7 +1045,7 @@
<Group> <Group>
<GroupName>Application/Task</GroupName> <GroupName>Application/Task</GroupName>
<tvExp>1</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
@@ -1215,7 +1220,7 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\PID\pid.c</PathWithFileName> <PathWithFileName>..\modules\algorithm\pid.c</PathWithFileName>
<FilenameWithoutPath>pid.c</FilenameWithoutPath> <FilenameWithoutPath>pid.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
@@ -1227,7 +1232,7 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\filter\filter.c</PathWithFileName> <PathWithFileName>..\modules\algorithm\filter.c</PathWithFileName>
<FilenameWithoutPath>filter.c</FilenameWithoutPath> <FilenameWithoutPath>filter.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
@@ -1251,7 +1256,7 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\DR16\dr16.c</PathWithFileName> <PathWithFileName>..\modules\comms\dr16.c</PathWithFileName>
<FilenameWithoutPath>dr16.c</FilenameWithoutPath> <FilenameWithoutPath>dr16.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
@@ -1263,7 +1268,7 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\referee\referee.c</PathWithFileName> <PathWithFileName>..\modules\comms\referee.c</PathWithFileName>
<FilenameWithoutPath>referee.c</FilenameWithoutPath> <FilenameWithoutPath>referee.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
@@ -1275,7 +1280,7 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\LED\led.c</PathWithFileName> <PathWithFileName>..\modules\dev\led.c</PathWithFileName>
<FilenameWithoutPath>led.c</FilenameWithoutPath> <FilenameWithoutPath>led.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
@@ -1287,7 +1292,7 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\BUZZER\buzzer.c</PathWithFileName> <PathWithFileName>..\modules\dev\buzzer.c</PathWithFileName>
<FilenameWithoutPath>buzzer.c</FilenameWithoutPath> <FilenameWithoutPath>buzzer.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
@@ -1299,7 +1304,7 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\c_comm\c_comm.c</PathWithFileName> <PathWithFileName>..\modules\comms\c_comm.c</PathWithFileName>
<FilenameWithoutPath>c_comm.c</FilenameWithoutPath> <FilenameWithoutPath>c_comm.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
@@ -1311,7 +1316,7 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\BMI088\bmi088.c</PathWithFileName> <PathWithFileName>..\modules\sensor\bmi088.c</PathWithFileName>
<FilenameWithoutPath>bmi088.c</FilenameWithoutPath> <FilenameWithoutPath>bmi088.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
@@ -1323,7 +1328,7 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\IST8310\ist8310.c</PathWithFileName> <PathWithFileName>..\modules\sensor\ist8310.c</PathWithFileName>
<FilenameWithoutPath>ist8310.c</FilenameWithoutPath> <FilenameWithoutPath>ist8310.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
@@ -1335,7 +1340,7 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\attitude\attitude.c</PathWithFileName> <PathWithFileName>..\modules\imu\attitude.c</PathWithFileName>
<FilenameWithoutPath>attitude.c</FilenameWithoutPath> <FilenameWithoutPath>attitude.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
@@ -1347,7 +1352,7 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\ins\kalman_filter.c</PathWithFileName> <PathWithFileName>..\modules\imu\qekf\kalman_filter.c</PathWithFileName>
<FilenameWithoutPath>kalman_filter.c</FilenameWithoutPath> <FilenameWithoutPath>kalman_filter.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
@@ -1359,7 +1364,7 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\ins\QuaternionEKF.c</PathWithFileName> <PathWithFileName>..\modules\imu\qekf\QuaternionEKF.c</PathWithFileName>
<FilenameWithoutPath>QuaternionEKF.c</FilenameWithoutPath> <FilenameWithoutPath>QuaternionEKF.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
@@ -1371,7 +1376,7 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\ins\ins.c</PathWithFileName> <PathWithFileName>..\modules\imu\ins.c</PathWithFileName>
<FilenameWithoutPath>ins.c</FilenameWithoutPath> <FilenameWithoutPath>ins.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
@@ -1383,7 +1388,7 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\gimbal\gimbal.c</PathWithFileName> <PathWithFileName>..\modules\control\gimbal.c</PathWithFileName>
<FilenameWithoutPath>gimbal.c</FilenameWithoutPath> <FilenameWithoutPath>gimbal.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
@@ -1395,7 +1400,7 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\shoot\shoot.c</PathWithFileName> <PathWithFileName>..\modules\control\shoot.c</PathWithFileName>
<FilenameWithoutPath>shoot.c</FilenameWithoutPath> <FilenameWithoutPath>shoot.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
@@ -1466,7 +1471,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>12</GroupNumber> <GroupNumber>12</GroupNumber>
<FileNumber>100</FileNumber> <FileNumber>98</FileNumber>
<FileType>4</FileType> <FileType>4</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
云台/MDK-ARM/Simba-ELCT-FrameWork.uvprojx
+17 -17
View File
@@ -341,7 +341,7 @@
<MiscControls></MiscControls> <MiscControls></MiscControls>
<Define>USE_HAL_DRIVER,STM32F407xx,ARM_MATH_CM4,ROBOT_TYPE_GIMBAL</Define> <Define>USE_HAL_DRIVER,STM32F407xx,ARM_MATH_CM4,ROBOT_TYPE_GIMBAL</Define>
<Undefine></Undefine> <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> </VariousControls>
</Cads> </Cads>
<Aads> <Aads>
@@ -359,7 +359,7 @@
<MiscControls></MiscControls> <MiscControls></MiscControls>
<Define></Define> <Define></Define>
<Undefine></Undefine> <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> </VariousControls>
</Aads> </Aads>
<LDads> <LDads>
@@ -2402,12 +2402,12 @@
<File> <File>
<FileName>pid.c</FileName> <FileName>pid.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\PID\pid.c</FilePath> <FilePath>..\modules\algorithm\pid.c</FilePath>
</File> </File>
<File> <File>
<FileName>filter.c</FileName> <FileName>filter.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\filter\filter.c</FilePath> <FilePath>..\modules\algorithm\filter.c</FilePath>
</File> </File>
<File> <File>
<FileName>dji_motor.c</FileName> <FileName>dji_motor.c</FileName>
@@ -2417,67 +2417,67 @@
<File> <File>
<FileName>dr16.c</FileName> <FileName>dr16.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\DR16\dr16.c</FilePath> <FilePath>..\modules\comms\dr16.c</FilePath>
</File> </File>
<File> <File>
<FileName>referee.c</FileName> <FileName>referee.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\referee\referee.c</FilePath> <FilePath>..\modules\comms\referee.c</FilePath>
</File> </File>
<File> <File>
<FileName>led.c</FileName> <FileName>led.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\LED\led.c</FilePath> <FilePath>..\modules\dev\led.c</FilePath>
</File> </File>
<File> <File>
<FileName>buzzer.c</FileName> <FileName>buzzer.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\BUZZER\buzzer.c</FilePath> <FilePath>..\modules\dev\buzzer.c</FilePath>
</File> </File>
<File> <File>
<FileName>c_comm.c</FileName> <FileName>c_comm.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\c_comm\c_comm.c</FilePath> <FilePath>..\modules\comms\c_comm.c</FilePath>
</File> </File>
<File> <File>
<FileName>bmi088.c</FileName> <FileName>bmi088.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\BMI088\bmi088.c</FilePath> <FilePath>..\modules\sensor\bmi088.c</FilePath>
</File> </File>
<File> <File>
<FileName>ist8310.c</FileName> <FileName>ist8310.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\IST8310\ist8310.c</FilePath> <FilePath>..\modules\sensor\ist8310.c</FilePath>
</File> </File>
<File> <File>
<FileName>attitude.c</FileName> <FileName>attitude.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\attitude\attitude.c</FilePath> <FilePath>..\modules\imu\attitude.c</FilePath>
</File> </File>
<File> <File>
<FileName>kalman_filter.c</FileName> <FileName>kalman_filter.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\ins\kalman_filter.c</FilePath> <FilePath>..\modules\imu\qekf\kalman_filter.c</FilePath>
</File> </File>
<File> <File>
<FileName>QuaternionEKF.c</FileName> <FileName>QuaternionEKF.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\ins\QuaternionEKF.c</FilePath> <FilePath>..\modules\imu\qekf\QuaternionEKF.c</FilePath>
</File> </File>
<File> <File>
<FileName>ins.c</FileName> <FileName>ins.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\ins\ins.c</FilePath> <FilePath>..\modules\imu\ins.c</FilePath>
</File> </File>
<File> <File>
<FileName>gimbal.c</FileName> <FileName>gimbal.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\gimbal\gimbal.c</FilePath> <FilePath>..\modules\control\gimbal.c</FilePath>
</File> </File>
<File> <File>
<FileName>shoot.c</FileName> <FileName>shoot.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\shoot\shoot.c</FilePath> <FilePath>..\modules\control\shoot.c</FilePath>
</File> </File>
</Files> </Files>
</Group> </Group>
云台/MDK-ARM/startup_stm32f407xx.s
+8 -1
View File
@@ -40,7 +40,14 @@ __initial_sp
; <o> Heap Size (in Bytes) <0x0-0xFFFFFFFF:8> ; <o> Heap Size (in Bytes) <0x0-0xFFFFFFFF:8>
; </h> ; </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 AREA HEAP, NOINIT, READWRITE, ALIGN=3
__heap_base __heap_base
云台/Middlewares/ST/ARM/DSP/Lib/arm_cortexM4lf_math.lib
BIN
View File
Binary file not shown.
云台/README.md
+40 -114
View File
@@ -1,133 +1,59 @@
# Simba-ELCT-FrameWork # 云台板固件(Simba-ELCT-FrameWork)
> 步兵机器人**云台板**工程 · STM32F407IGH6 · Keil + FreeRTOS + CMSIS-DSP
<table> 项目总览、代码地图、任务表、调试与注意事项请先读 **[根 README](../README.md)** 与 **[已知问题与排障](../doc/已知问题与排障.md)**。本文只讲这块板特有的东西。
<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>
[![Author](https://img.shields.io/badge/Author-LSMushui-orange.svg)](https://gitee.com/LSMushui) ---
![language](https://img.shields.io/badge/language-C-blue.svg)
![license](https://img.shields.io/badge/license-GPLv3-green.svg)
## 这块板负责什么
## 简介 - **云台控制**:Yaw(GM6020 `0x205`,CAN2 `0x1FE`电流控制)/ Pitch(GM6020 `0x206`,CAN1 `0x1FE`电流控制)双环
- **发射机构**:摩擦轮 M3508 `0x201`/`0x202`(CAN1 `0x200`) + 拨弹 M2006 `0x201`(CAN2 `0x200`)(装与不装由 `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 掉线报警按设备序号响 `n` 下:
机器人控制模块：
- [ ] 大疆电机
- [ ] 达妙电机
- [ ] 瓴控电机
- [ ] 小米电机
- [ ] 超级电容控制板
- [ ] 板间通信(CAN)
- [ ] PWM 控制舵机
- [ ] PWM 控制气泵
- **Chassis**\
底盘模块：
- [ ] 麦轮底盘
- [ ] 全向轮底盘
- [ ] 舵轮底盘
- [ ] 平衡底盘
- **Gimbal**\
云台模块：
- [ ] yaw-pitch 直连云台
- [ ] yaw直连-pitch丝杆 云台
- [ ] yaw-pitch 丝杆云台
- **Shoot**\
发射机构模块：
- [ ] 摩擦轮+拨弹盘
- [ ] 摩擦轮+弹鼓+推杆
- **Communication**\
通信模块：
- [ ] 板间通信(CAN2)
- [ ] 上下位机通信
- **Music**\
音乐模块：
- [ ] 开机声
- [ ] 报错音
- [ ] 裁判系统接入提示音
- [ ] 《卡农》
- [ ] 《天空之城》
- [ ] 《恭喜发财》
- [ ] 《好运来》
- [ ] 《See You Again》
- [ ] 《Unity》
- [ ] 《你》
## 开发工具 ```
0 = 摩擦轮L(CAN1 0x201) 1 = 摩擦轮R(CAN1 0x202) 2 = Pitch(CAN1 0x206)
3 = Yaw(CAN2 0x205) 4 = 拨弹(CAN2 0x201) 5 = 底盘回传(0x019)
```
本项目使用HAL库编写，通过CubeMX生成项目，使用FreeRTOS **响三下 = Pitch 电机掉线。** 台架调试嫌吵时把 `robot_param.h` 的 `DEBUG_BUZZER_MELODY` 置 1(只换声音不换灯语)。
使用VSCode作为代码编辑器，Keil5作为IDE ## 构建
Keil5编译器使用AC6版本 用 Keil 打开 `MDK-ARM/Simba-ELCT-FrameWork.uvprojx`,确认 Define 里是 `ROBOT_TYPE_GIMBAL`,Rebuild → Download。
> 虽然keil5老，但在嵌入式调试以及环境配置方面是非常简单易上手的。**千万不要用keil5来编辑代码！！！**
\
\
> VSCode作为现代化代码编辑器 ，通过各种插件即可实现各种提高效率的开发方式，**尤其是Github Copilot**
\
\
> AC6编译器相比古早AC5编译器效率更高，AC5编译空项目就要足足1min47s，非常不中，相比之下AC6只需要6s，遥遥领先
\
\
> 但是CubeMX开启FreeRTOS无法直接使用AC6编译器
\
> 强烈建议通过此[教程](https://bbs.robomaster.com/article/18268)(From COD战队GrassFan Wang)配置好自己的环境
> ⚠️ 本工程与底盘工程源码高度同构,改一处通常要同步改另一处。动手前请读根 README 的 [§11 已知陷阱](../README.md#11-已知陷阱与注意事项)。
## 欢迎贡献
如果你发现了本项目中的问题和可优化的点，可用创建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
云台/Simba-ELCT-FrameWork.ioc
+4 -2
View File
@@ -361,12 +361,14 @@ PC2.GPIOParameters=GPIO_Label
PC2.GPIO_Label=HW2 PC2.GPIO_Label=HW2
PC2.Locked=true PC2.Locked=true
PC2.Signal=GPIO_Input 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_Label=INT1_ACCEL
PC4.GPIO_PuPd=GPIO_PULLUP PC4.GPIO_PuPd=GPIO_PULLUP
PC4.Locked=true PC4.Locked=true
PC4.Signal=GPXTI4 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_Label=INT1_GYRO
PC5.GPIO_PuPd=GPIO_PULLUP PC5.GPIO_PuPd=GPIO_PULLUP
PC5.Locked=true PC5.Locked=true
云台/modules/BUZZER/buzzer.c
-44
View File
@@ -1,44 +0,0 @@
/* =============================================================================
* buzzer.c - C 板无源蜂鸣器(TIM4_CH3, PD14)
* 说明: 音调表与 legacy 完全一致(ARR 固定 10 分频,改 PSC 变调,
* 装载用 HAL_TIM_GenerateEvent 立即生效);buzzer_init 必须在
* CAN_Task 中调用一次启动 PWM 输出,否则所有报警/音效无声
* ========================================================================== */
#include "buzzer.h"
#include "tim.h"
extern TIM_HandleTypeDef htim4;
/* 蜂鸣器音调频率表(ARR 固定 10 分频下的 PSC 值,与 legacy 一致) */
static const uint16_t buzzer_tone_freq[37] = {
0,
32107, 30305, 28604, 26999, 25483, 24053, 22703, 21429, 20226, 19091, 18019, 17008,
16053, 15152, 14302, 13499, 12742, 12026, 11352, 10714, 10113, 9545, 9010, 8504,
8027, 7576, 7151, 6750, 6371, 6013, 5676, 5357, 5056, 4773, 4505, 4252,
};
void buzzer_init(void)
{
/* 与 legacy 相同:ARR=10 分频,CubeMX 的 ARR=65535 改为 9 */
__HAL_TIM_SET_AUTORELOAD(&htim4, 10 - 1);
__HAL_TIM_SET_PRESCALER(&htim4, 19091);
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 0);
HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_3);
}
void buzzer_on(buzzer_tone_e tone)
{
if (tone != BUZZER_P)
{
__HAL_TIM_SET_PRESCALER(&htim4, buzzer_tone_freq[tone]);
HAL_TIM_GenerateEvent(&htim4, TIM_EVENTSOURCE_UPDATE); // 立即装载 PSC
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 5); // 占空比 50%
}
else
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 0);
}
void buzzer_off(void)
{
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 0);
}
云台/modules/filter/filter.c → 云台/modules/algorithm/filter.c
View File
云台/modules/filter/filter.h → 云台/modules/algorithm/filter.h
View File
云台/modules/PID/pid.c → 云台/modules/algorithm/pid.c
View File
云台/modules/PID/pid.h → 云台/modules/algorithm/pid.h
View File
底盘/modules/c_comm/c_comm.c → 云台/modules/comms/c_comm.c
+20
View File
@@ -136,6 +136,26 @@ void c_comm_master_send(uint8_t keyboard_online, const uint8_t kb[12], uint8_t m
c_comm_send_frame(&hcan2, CTOC_SLAVE_ID2, data); c_comm_send_frame(&hcan2, CTOC_SLAVE_ID2, data);
} }
/* 0x1B9 云台->底盘:自定义UI 专用量(底盘本地没有,见 referee_ui)
* [0..1] pitch(0.01°) [2..3] 左摩擦轮(rpm/100) [4..5] 右摩擦轮(rpm/100)
* [6] bit0=小陀螺使能 */
void c_comm_master_send_ui(float pitch_deg, float fric_l_rpm, float fric_r_rpm, uint8_t spin_on)
{
uint8_t data[8] = {0};
int16_t p = (int16_t)(pitch_deg * 100.0f);
int16_t fl = (int16_t)(fric_l_rpm / 100.0f);
int16_t fr = (int16_t)(fric_r_rpm / 100.0f);
data[0] = (uint8_t)(p & 0xFF);
data[1] = (uint8_t)((uint16_t)p >> 8);
data[2] = (uint8_t)(fl & 0xFF);
data[3] = (uint8_t)((uint16_t)fl >> 8);
data[4] = (uint8_t)(fr & 0xFF);
data[5] = (uint8_t)((uint16_t)fr >> 8);
data[6] = spin_on ? 0x01 : 0;
c_comm_send_frame(&hcan2, CTOC_SLAVE_ID4, data);
}
#elif defined(ROBOT_TYPE_CHASSIS) #elif defined(ROBOT_TYPE_CHASSIS)
/* ============================ 底盘板(从机) ============================ */ /* ============================ 底盘板(从机) ============================ */
底盘/modules/c_comm/c_comm.h → 云台/modules/comms/c_comm.h
+7 -3
View File
@@ -12,11 +12,10 @@
* 底盘误判沿会误开小陀螺,勿再使用) * 底盘误判沿会误开小陀螺,勿再使用)
* Data[6] bit0=键1 bit1=键2 bit2=小陀螺使能(本工程扩展) * Data[6] bit0=键1 bit1=键2 bit2=小陀螺使能(本工程扩展)
* bit7=云台电机故障(本工程扩展) * bit7=云台电机故障(本工程扩展)
* 0x1A9 云台->底盘:Data[0..3]=云台世界系 yaw(float,LE) Data[4]=att_ready
* (双 INS 跟随方案的数据源,2026-09 新增)
* 0x1A9 云台->底盘:Data[0..3]=云台世界系 yaw(float,LE) * 0x1A9 云台->底盘:Data[0..3]=云台世界系 yaw(float,LE)
* Data[4] flags:bit0=att_ready bit1=编码器tick增向为CCW * Data[4] flags:bit0=att_ready bit1=编码器tick增向为CCW
* bit2=该位有效(自检完成) [跟随符号自动综合的数据源] * bit2=该位有效(自检完成)
* (双 INS 跟随方案 + 跟随符号自动综合的数据源,2026-09)
* 0x019 底盘->云台:Data[0]发射机构上电状态 Data[1] bit0=底盘电机故障(本工程扩展) * 0x019 底盘->云台:Data[0]发射机构上电状态 Data[1] bit0=底盘电机故障(本工程扩展)
* *
* 故障位语义:任一侧电机掉线即置位,对侧收到后必须停止自己的输出(整车安全) */ * 故障位语义:任一侧电机掉线即置位,对侧收到后必须停止自己的输出(整车安全) */
@@ -64,6 +63,11 @@ extern volatile int8_t ctoc_encoder_ccw;
/* 底盘板:0x1A9 帧是否新鲜(掉线检测) */ /* 底盘板:0x1A9 帧是否新鲜(掉线检测) */
extern uint8_t ctoc_att_fresh(void); extern uint8_t ctoc_att_fresh(void);
/* ---- 0x1B9 云台->底盘:自定义UI 专用数据 ---------------------------------
* 底盘本地没有、但裁判系统自定义UI 需要的量(见 底盘/modules/comms/referee_ui):
* pitch 角度与左右摩擦轮转速。接收侧解析见底盘 c_comm.c(c_comm_rx_0x1b9)。 */
extern void c_comm_master_send_ui(float pitch_deg, float fric_l_rpm, float fric_r_rpm, uint8_t spin_on);
/* 注册 CAN 接收回调(can_start() 之后调用) */ /* 注册 CAN 接收回调(can_start() 之后调用) */
extern void c_comm_init(void); extern void c_comm_init(void);
底盘/modules/DR16/dr16.c → 云台/modules/comms/dr16.c
+6
View File
@@ -48,6 +48,12 @@ void dr16_parse(const uint8_t * data)
dr16.key_alpha1 = (data[14] >> 9) & 0x01; dr16.key_alpha1 = (data[14] >> 9) & 0x01;
dr16.key_alpha2 = (data[14] >> 10) & 0x01; dr16.key_alpha2 = (data[14] >> 10) & 0x01;
/* 原始按键字节。注意 data[14] 是 uint8_t:上面两条 >>9/>>10 恒为 0
* (整型提升后 byte 值 <256,右移 9 位必然为 0),即 alpha 键从未被真正解析。
* 官方 16 位 key_code 的高字节在 data[15],这里先原样存下来供诊断/后续解析。 */
dr16.key_code_lo = data[14];
dr16.key_code_hi = data[15];
dr16.thumb_wheel = (int16_t)(((uint16_t)data[17] << 8) | data[16]); // B[120:135] dr16.thumb_wheel = (int16_t)(((uint16_t)data[17] << 8) | data[16]); // B[120:135]
/* 按键边沿检测(legacy 语义:Q/E/Shift/Ctrl 按下翻转,鼠标右键按下瞬间置 1) */ /* 按键边沿检测(legacy 语义:Q/E/Shift/Ctrl 按下翻转,鼠标右键按下瞬间置 1) */
云台/modules/DR16/dr16.h → 云台/modules/comms/dr16.h
+7
View File
@@ -37,6 +37,13 @@ typedef struct
uint8_t key_alpha1; // 图传扩展键 uint8_t key_alpha1; // 图传扩展键
uint8_t key_alpha2; uint8_t key_alpha2;
/* 原始按键字节(诊断用,不做位拆分) —— 用来判断"帧里到底有没有键鼠数据":
* 官方 dbus.h 的 key_code 是 16 位:bit0..7 = W/S/A/D/Shift/Ctrl/Q/E(=data[14]),
* bit8..15 = R/F/G/Z/X/C/V/B(=data[15])。本工程此前只读了 data[14]。
* 拨键盘时这两个字节若非 0 说明数据确实到了;全 0 说明帧里就是空的。 */
uint8_t key_code_lo; // data[14]
uint8_t key_code_hi; // data[15]
int16_t thumb_wheel; // 遥控器拨轮 int16_t thumb_wheel; // 遥控器拨轮
/* 按键边沿检测(legacy 语义) */ /* 按键边沿检测(legacy 语义) */
云台/modules/referee/referee.c → 云台/modules/comms/referee.c
View File
云台/modules/referee/referee.h → 云台/modules/comms/referee.h
View File
云台/modules/control/gimbal.c
+333
View File
@@ -0,0 +1,333 @@
/* =============================================================================
* gimbal.c - 云台双轴控制(串级 PID)
* Yaw(2026-09 重写):双陀螺环,见 gimbal.h 与 robot_param.h 说明
* 位置环:err(°) = 目标角 - SIGN*att_deg_yaw -> 目标角速度(°/s,限幅)
* 速度环:err(°/s) = 目标角速度 - SIGN*gyro_z -> 电流(限幅)
* 稳定性条件:SIGN*G>0,G 为"电流->陀螺 z 角速度"的装配增益,
* 由 gimbal_detect_yaw_sign() 上电实测,彻底消除双传感器符号打架
* Pitch:位置环用 QEKF Roll(安装方向),速度环转子转速(legacy 案,无符号史问题)
* 发送:CAN2 0x1FE 槽1=Yaw 电流;CAN1 0x1FE 槽2=Pitch 电流
* ========================================================================== */
#include "gimbal.h"
#include "shoot.h"
#include "pid.h"
#include "attitude.h"
#include "bmi088.h"
#include "can.h"
#include "robot_param.h"
#include "filter.h"
#include "robot_typedef.h"
#include "dr16.h"
#include <math.h>
#ifndef RAD_TO_ANGLE
#define RAD_TO_ANGLE 57.295779513082320876798154814105f
#endif
#ifndef ANGLE_TO_RAD
#define ANGLE_TO_RAD 0.01745329251994329576923690768489f
#endif
dji_motor_t motor_yaw_gimbal;
dji_motor_t motor_pitch_gimbal;
dji_motor_t motor_friction_l;
dji_motor_t motor_friction_r;
dji_motor_t motor_rammer;
static PID_PositionTypeDef yaw_angle_pid, yaw_speed_pid;
static PID_PositionTypeDef pitch_angle_pid, pitch_speed_pid;
static float yaw_target_deg; // 世界系目标角(°)
static float pitch_target_deg; // 目标角(°)
static float yaw_current;
float pitch_pid = 0.0f;
float pitch_g = 0.0f;
float pitch_current = 0.0f;
/* ===== 2026-10-15 台架调参临时量(已停用,保留备查) ===== */
#if 0
/* 台架调参用 Pitch 目标角附加偏置(°)。由 gimbal_task 的方波发生器写入,
* 只叠加到位置环设定值上,不累加进 pitch_target_deg,默认 0 无副作用 */
float gimbal_bench_pitch_offset = 0.0f;
/* 实际施加的偏置(经限位安全门控后),仅用于遥测显示真实设定值 */
static float pitch_applied_offset = 0.0f;
#endif
/* 装配符号:0=未检测,±1=已测定;未检测前 gimbal_control 不应被调用 */
static float yaw_sign_f = 0.0f;
static uint8_t yaw_sign_valid = 0;
void gimbal_motor_init(void)
{
dji_motor_init(&motor_yaw_gimbal, &hcan2, 0x205, DJI_MOTOR_6020);
dji_motor_init(&motor_pitch_gimbal, &hcan1, 0x206, DJI_MOTOR_6020);
dji_motor_init(&motor_friction_l, &hcan1, 0x201, DJI_MOTOR_3508);
dji_motor_init(&motor_friction_r, &hcan1, 0x202, DJI_MOTOR_3508);
dji_motor_init(&motor_rammer, &hcan2, 0x201, DJI_MOTOR_2006);
/* Yaw 位置环:° -> °/s */
PID_PositionStructureInit(&yaw_angle_pid, 0);
PID_PositionSetParameter(&yaw_angle_pid, GIMBAL_YAW_ANGLE_KP, GIMBAL_YAW_ANGLE_KI, GIMBAL_YAW_ANGLE_KD);
PID_PositionSetEkRange(&yaw_angle_pid, -GIMBAL_YAW_ANGLE_EK_DEADBAND, GIMBAL_YAW_ANGLE_EK_DEADBAND);
PID_PositionSetOUTRange(&yaw_angle_pid, -GIMBAL_YAW_ANGLE_OUT_LIMIT, GIMBAL_YAW_ANGLE_OUT_LIMIT);
/* Yaw 速度环:°/s -> 电流 */
PID_PositionStructureInit(&yaw_speed_pid, 0);
PID_PositionSetParameter(&yaw_speed_pid, GIMBAL_YAW_SPEED_KP, GIMBAL_YAW_SPEED_KI, GIMBAL_YAW_SPEED_KD);
PID_PositionSetOUTRange(&yaw_speed_pid, -GIMBAL_YAW_SPEED_OUT_LIMIT, GIMBAL_YAW_SPEED_OUT_LIMIT);
/* Pitch:与 legacy 相同的两环(位置环反馈为 Roll) */
PID_PositionStructureInit(&pitch_angle_pid, 0);
PID_PositionSetParameter(&pitch_angle_pid, GIMBAL_PITCH_ANGLE_KP, GIMBAL_PITCH_ANGLE_KI, GIMBAL_PITCH_ANGLE_KD);
PID_PositionSetEkRange(&pitch_angle_pid, -GIMBAL_PITCH_ANGLE_EK_DEADBAND, GIMBAL_PITCH_ANGLE_EK_DEADBAND);
PID_PositionSetOUTRange(&pitch_angle_pid, -200, 200);
PID_PositionStructureInit(&pitch_speed_pid, 300);
PID_PositionSetParameter(&pitch_speed_pid, GIMBAL_PITCH_SPEED_KP, GIMBAL_PITCH_SPEED_KI, GIMBAL_PITCH_SPEED_KD);
PID_PositionSetEkRange(&pitch_speed_pid, 0, 0);
PID_PositionSetOUTRange(&pitch_speed_pid, -16384, 16384);
}
uint8_t gimbal_detect_yaw_sign(void)
{
if (gimbal_motor_offline())
return 0;
/* 静默期:输出清零等待机械/陀螺稳定 */
dji_motor_send_gm6020_current_low(&hcan2, 0, 0, 0, 0);
HAL_Delay(50);
/* 正向电流脉冲 60ms,期间平均陀螺 z 角速度 */
float sum = 0.0f;
dji_motor_send_gm6020_current_low(&hcan2, GIMBAL_YAW_DETECT_CURRENT, 0, 0, 0);
for (uint16_t i = 0; i < 60; i++)
{
sum += bmi088_gyro[2]; // rad/s(z 轴向上为正)
HAL_Delay(1);
}
dji_motor_send_gm6020_current_low(&hcan2, 0, 0, 0, 0);
HAL_Delay(80); // 回稳,随后 gimbal_task 的首次控制会同步目标角
const float rate_dps = sum / 60.0f * RAD_TO_ANGLE;
if (rate_dps > GIMBAL_YAW_DETECT_RATE_MIN)
{
yaw_sign_f = 1.0f; // 正电流->陀螺正转,装配增益 G>0
yaw_sign_valid = 1;
}
else if (rate_dps < -GIMBAL_YAW_DETECT_RATE_MIN)
{
yaw_sign_f = -1.0f; // 皮带交叉等反向装配
yaw_sign_valid = 1;
}
else
return 0; // 被按住/没转:不给结论,由调用方决定重试或回退
return 1;
}
float gimbal_yaw_sign(void)
{
return yaw_sign_valid ? yaw_sign_f : GIMBAL_YAW_SIGN_FALLBACK;
}
int8_t gimbal_encoder_ccw(void)
{
/* 自检:电流正脉冲 -> 陀螺 z 正转(云台 CCW),而电流正方向即编码器
* tick 增大方向,故 tick 增方向 = sign(gimbal 旋转) */
if (!yaw_sign_valid)
return 0;
return (yaw_sign_f > 0.0f) ? 1 : -1;
}
void gimbal_control(float yaw_delta, float pitch_delta, uint8_t reset_flag)
{
/* 双环共用装配符号 */
const float sign = gimbal_yaw_sign();
const float yaw_meas = sign * att_deg_yaw; // °
const float yaw_rate_meas = sign * (bmi088_gyro[2] * RAD_TO_ANGLE); // °/s
/* 首次进入控制或复位:将目标角设定为设定的编码器零位朝向(类似 Pitch 上电归位) */
static uint8_t first_run = 1;
if (first_run || reset_flag)
{
first_run = 0;
/* 计算当前编码器距离设定零位的最短差值(-4096 ~ +4096) */
int16_t ecd_diff = (int16_t)motor_yaw_gimbal.fdb.raw_angle - (int16_t)YAW_GM6020_ENCODER_ZERO;
if (ecd_diff > 4096)
ecd_diff -= 8192;
else if (ecd_diff < -4096)
ecd_diff += 8192;
/* 将编码器 tick 差折算为机械角度偏差(°)，补偿给世界系目标角 */
const float ecd_err_deg = (float)ecd_diff * (360.0f / 8192.0f);
yaw_target_deg = yaw_meas - ecd_err_deg;
/* Pitch: 计算当前编码器距离机械零位的差值，折算为角度偏差补偿给目标角(上电自动归位到零位) */
int16_t pitch_ecd_diff = (int16_t)motor_pitch_gimbal.fdb.raw_angle - (int16_t)PITCH_GM6020_ENCODER_ZERO;
const float pitch_ecd_err_deg = (float)pitch_ecd_diff * (360.0f / 8192.0f);
pitch_target_deg = -att_deg_roll - pitch_ecd_err_deg;
if (reset_flag)
{
PID_PositionClean(&yaw_angle_pid);
PID_PositionClean(&yaw_speed_pid);
PID_PositionClean(&pitch_angle_pid);
PID_PositionClean(&pitch_speed_pid);
}
}
yaw_target_deg += yaw_delta;
pitch_target_deg += pitch_delta;
/* Yaw 位置环:° -> °/s(反馈为带符号姿态角) */
yaw_angle_pid.Need_Value = yaw_target_deg;
PID_PositionCalc(&yaw_angle_pid, yaw_meas);
/* Yaw 速度环:°/s -> 电流(反馈为带符号陀螺角速度,双环符号自洽) */
yaw_speed_pid.Need_Value = yaw_angle_pid.OUT;
PID_PositionCalc(&yaw_speed_pid, yaw_rate_meas);
/* 小陀螺底盘自旋解耦前馈:根据底盘相对转速补偿动摩擦与反电动势拖拽，消除同向/反向手感不对称 */
float spin_feedforward = 0.0f;
if (cmd_robot.spin_flag || dr16.s2 == 1 || dr16.s2 == 2)
{
spin_feedforward = GIMBAL_YAW_SPIN_FEEDFORWARD_K * (float)motor_yaw_gimbal.fdb.rotor_speed;
}
yaw_current = yaw_speed_pid.OUT + spin_feedforward;
if (yaw_current > GIMBAL_YAW_SPEED_OUT_LIMIT)
yaw_current = GIMBAL_YAW_SPEED_OUT_LIMIT;
else if (yaw_current < -GIMBAL_YAW_SPEED_OUT_LIMIT)
yaw_current = -GIMBAL_YAW_SPEED_OUT_LIMIT;
/* Pitch 串级闭环,位置环反馈为 AHRS Roll */
#if 0
/* ===== 2026-10-15 台架调参临时改动(已停用,保留备查) =====
* 启用时:先算编码器角与上下软限位,做台架偏置的限位安全门控(轴距限位
* 不足 GIMBAL_BENCH_PITCH_SAFE_MARGIN_DEG 时不施加阶跃),并把下面一行
* 改为 pitch_angle_pid.Need_Value = pitch_target_deg + pitch_applied_offset; */
const float pitch_ecd_deg = ((float)motor_pitch_gimbal.fdb.raw_angle -
(float)PITCH_GM6020_ENCODER_ZERO) * (360.0f / 8192.0f);
const float pitch_lim_up_deg = ((float)PITCH_GM6020_POSITION_UPPER_LIMIT -
(float)PITCH_GM6020_ENCODER_ZERO) * (360.0f / 8192.0f);
const float pitch_lim_dn_deg = ((float)PITCH_GM6020_POSITION_LOWER_LIMIT -
(float)PITCH_GM6020_ENCODER_ZERO) * (360.0f / 8192.0f);
pitch_applied_offset = gimbal_bench_pitch_offset;
if (pitch_ecd_deg < pitch_lim_up_deg + GIMBAL_BENCH_PITCH_SAFE_MARGIN_DEG ||
pitch_ecd_deg > pitch_lim_dn_deg - GIMBAL_BENCH_PITCH_SAFE_MARGIN_DEG)
pitch_applied_offset = 0.0f;
#endif
pitch_angle_pid.Need_Value = pitch_target_deg;
PID_PositionCalc(&pitch_angle_pid, -att_deg_roll);
pitch_speed_pid.Need_Value = pitch_angle_pid.OUT;
PID_PositionCalc(&pitch_speed_pid, motor_pitch_gimbal.fdb.rotor_speed);
pitch_pid = pitch_speed_pid.OUT;
/* Pitch 重力拟合前馈补偿:
* a 为当前物理编码值减去 3200*360/8192 的数值(折算为弧度送入三角函数)
* I = 3105.88 * cos(a) + 3565.50 * sin(a) + 64.31
* 最终电流输出 pitch_out = pitch_pid + pitch_g (速度环不代入 pitch_g) */
const float a_deg = ((float)motor_pitch_gimbal.fdb.raw_angle - GIMBAL_PITCH_GRAVITY_ZERO_ECD) * (360.0f / 8192.0f);
const float a_rad = a_deg * ANGLE_TO_RAD;
pitch_g = GIMBAL_PITCH_GRAVITY_K_COS * cosf(a_rad) +
GIMBAL_PITCH_GRAVITY_K_SIN * sinf(a_rad) +
GIMBAL_PITCH_GRAVITY_K_CONST;
pitch_current = pitch_pid + pitch_g;
if (pitch_current > 16384.0f)
pitch_current = 16384.0f;
else if (pitch_current < -16384.0f)
pitch_current = -16384.0f;
#if 0
/* ===== 2026-10-15 台架调参临时改动:Pitch 软限位/堵转保护(已停用,保留备查) =====
* 背景:原设计的上下限位只作用于遥控输入路径(robot_cmd_task),控制环自身的
* 目标角/前馈没有任何限位约束。台架烧录会失力矩十几秒~几分钟,轴会掉到限位,
* 若不加保护,原逻辑会持续输出大电流顶着限位(GM6020 直驱堵转 = 持续过载发热)。
* 做法:越限位时只允许"离开限位"方向的电流(正电流 = 抬头/编码器减小,故下限位
* 只留正向、上限位只留负向),反方向清零;限位外连续 GIMBAL_PITCH_LIMIT_STALL_MS
* 编码器不动则判堵转,电流降到涓流上限。轴回到软限位内后闭环自动恢复正常。 */
if (pitch_ecd_deg >= pitch_lim_dn_deg || pitch_ecd_deg <= pitch_lim_up_deg)
{
/* 堵转计时:相对基准编码值移动超过 10 个刻度(0.44°)才算"在动"。
* 阈值取 10 而不是 2:陀螺/结构微振会让编码器抖 2~3 个刻度,阈值太小会把
* 静止误判成在动,保护永远不生效(实测踩过);慢速抬升 5°/s 在 300ms 内
* 也能走 34 个刻度,不会被误判。 */
static uint16_t lim_stall = 0;
static uint16_t lim_ref_ecd = 0;
const int16_t d_ecd = (int16_t)((int16_t)motor_pitch_gimbal.fdb.raw_angle -
(int16_t)lim_ref_ecd);
if (d_ecd > 10 || d_ecd < -10)
{
lim_ref_ecd = motor_pitch_gimbal.fdb.raw_angle;
lim_stall = 0;
}
else if (lim_stall < 60000u)
lim_stall++;
const float cap = (lim_stall * 2u >= (uint16_t)GIMBAL_PITCH_LIMIT_STALL_MS)
? GIMBAL_PITCH_LIMIT_CURRENT
: GIMBAL_PITCH_LIMIT_CURRENT_RECOVER;
if (pitch_ecd_deg >= pitch_lim_dn_deg) /* 轴过低:只许抬升 */
{
if (pitch_current < 0.0f)
pitch_current = 0.0f;
else if (pitch_current > cap)
pitch_current = cap;
}
else /* 轴过高:只许下压 */
{
if (pitch_current > 0.0f)
pitch_current = 0.0f;
else if (pitch_current < -cap)
pitch_current = -cap;
}
PID_PositionClean(&pitch_speed_pid);
}
#endif
/* ===== 停用块结束 ===== */
/* 发送:CAN2 0x1FE 槽1 = Yaw 电流;CAN1 0x1FE 槽2 = Pitch 电流 */
dji_motor_send_gm6020_current_low(&hcan2, (int16_t)yaw_current, 0, 0, 0);
dji_motor_send_gm6020_current_low(&hcan1, 0, (int16_t)pitch_current, 0, 0);
}
#if 0
/* ===== 2026-10-15 台架调参临时接口(已停用,保留备查) ===== */
/* Pitch 位置环实际设定值(°) = 真实目标角 + 经限位门控后的台架偏置 */
float gimbal_pitch_setpoint_deg(void)
{
return pitch_target_deg + pitch_applied_offset;
}
/* Pitch 位置环输出 = 速度环目标(°/s),用于判断内环是否被限幅 */
float gimbal_pitch_speed_ref(void)
{
return pitch_angle_pid.OUT;
}
#endif
uint8_t gimbal_motor_offline(void)
{
uint32_t now = HAL_GetTick();
uint8_t rammer_off = 0;
#if GIMBAL_RAMMER_INSTALLED
rammer_off = (motor_rammer.fdb.last_online_tick == 0 ||
now - motor_rammer.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS);
#endif
return (motor_yaw_gimbal.fdb.last_online_tick == 0 || now - motor_yaw_gimbal.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
motor_pitch_gimbal.fdb.last_online_tick == 0 || now - motor_pitch_gimbal.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
motor_friction_l.fdb.last_online_tick == 0 || now - motor_friction_l.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
motor_friction_r.fdb.last_online_tick == 0 || now - motor_friction_r.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
rammer_off);
}
void gimbal_output_zero(void)
{
dji_motor_send_gm6020_current_low(&hcan2, 0, 0, 0, 0);
dji_motor_send_gm6020_current_low(&hcan1, 0, 0, 0, 0);
dji_motor_send_current_low(&hcan1, 0, 0, 0, 0);
dji_motor_send_current_low(&hcan2, 0, 0, 0, 0);
}
云台/modules/gimbal/gimbal.h → 云台/modules/control/gimbal.h
+20 -8
View File
@@ -4,13 +4,13 @@
/* 云台双轴控制(串级 PID) /* 云台双轴控制(串级 PID)
* Yaw (2026-09 重写):双环都以陀螺仪为反馈—— * Yaw (2026-09 重写):双环都以陀螺仪为反馈——
* 位置环:QEKF 多圈 yaw(°) -> 目标角速度(°/s) * 位置环:QEKF 多圈 yaw(°) -> 目标角速度(°/s)
* 速度环:陀螺 z 轴角速度(°/s) -> GM6020 电压 * 速度环:陀螺 z 轴角速度(°/s) -> GM6020 电流
* 装配符号由上电自检自动测定(gimbal_detect_yaw_sign), * 装配符号由上电自检自动测定(gimbal_detect_yaw_sign),
* 两环共用同一符号,不可能再出现"两传感器符号打架" * 两环共用同一符号,不可能再出现"两传感器符号打架"
* Pitch:位置环用 QEKF 的 Roll(安装方向所致,勿改),速度环转子转速 * Pitch:位置环用 QEKF 的 Roll(安装方向所致,勿改),速度环转子转速
* 电机: Yaw=GM6020 CAN2 0x205(电压,0x1FF CAN2 槽1) * 电机: Yaw=GM6020 CAN2 0x205(电流,0x1FE CAN2 槽1)
* Pitch=GM6020 CAN1 0x206(电压,与拨弹共帧 0x1FF CAN1 槽2/3,legacy 方案) * Pitch=GM6020 CAN1 0x206(电流,0x1FE CAN1 槽2)
* 摩擦轮 M3508 0x201/0x202 与拨弹 M2006 0x207 的实例也在此初始化 */ * 摩擦轮 M3508 0x201/0x202(CAN1) 与拨弹 M2006 0x201(CAN2) 的实例也在此初始化 */
#include "dji_motor.h" #include "dji_motor.h"
@@ -19,12 +19,15 @@ extern dji_motor_t motor_yaw_gimbal; // Yaw GM6020,CAN2 0x205
extern dji_motor_t motor_pitch_gimbal; // Pitch GM6020,CAN1 0x206 extern dji_motor_t motor_pitch_gimbal; // Pitch GM6020,CAN1 0x206
extern dji_motor_t motor_friction_l; // 左摩擦轮 M3508,CAN1 0x201 extern dji_motor_t motor_friction_l; // 左摩擦轮 M3508,CAN1 0x201
extern dji_motor_t motor_friction_r; // 右摩擦轮 M3508,CAN1 0x202 extern dji_motor_t motor_friction_r; // 右摩擦轮 M3508,CAN1 0x202
extern dji_motor_t motor_rammer; // 拨弹 M2006,CAN1 0x207 extern dji_motor_t motor_rammer; // 拨弹 M2006,CAN2 0x201
extern float pitch_pid; // Pitch PID 输出分量(仅速度环反馈计算)
extern float pitch_g; // Pitch 重力拟合前馈分量
extern float pitch_current; // Pitch GM6020 最终电流控制输出(pitch_pid + pitch_g, ±16384)
/* 初始化电机实例与 PID(can_start 之后调用) */ /* 初始化电机实例与 PID(can_start 之后调用) */
extern void gimbal_motor_init(void); extern void gimbal_motor_init(void);
/* Yaw 装配符号自检:给 Yaw 电机正向小电压脉冲,按陀螺 z 轴响应方向 /* Yaw 装配符号自检:给 Yaw 电机正向小电流脉冲,按陀螺 z 轴响应方向
* 测定符号。阻塞约 200ms(HAL_Delay),检测期间云台会轻转几度。 * 测定符号。阻塞约 200ms(HAL_Delay),检测期间云台会轻转几度。
* 返回 1-成功(gimbal_yaw_sign() 可用) 0-失败(被按住/电机离线/无响应) */ * 返回 1-成功(gimbal_yaw_sign() 可用) 0-失败(被按住/电机离线/无响应) */
extern uint8_t gimbal_detect_yaw_sign(void); extern uint8_t gimbal_detect_yaw_sign(void);
@@ -36,13 +39,22 @@ extern float gimbal_yaw_sign(void);
* 0=尚未自检。随 0x1A9 flags bit1 下发,供底盘跟随符号自动综合 */ * 0=尚未自检。随 0x1A9 flags bit1 下发,供底盘跟随符号自动综合 */
extern int8_t gimbal_encoder_ccw(void); extern int8_t gimbal_encoder_ccw(void);
/* 云台轴控制:根据 cmd 增量更新目标角并计算电压输出(2ms 调用) /* 云台轴控制:根据 cmd 增量更新目标角并计算电流输出(2ms 调用)
* 内部发送 CAN2 0x1FF(Yaw)与 CAN1 0x1FF(Pitch+拨弹) */ * 内部发送 CAN2 0x1FE(Yaw)与 CAN1 0x1FE(Pitch) */
extern void gimbal_control(float yaw_delta, float pitch_delta, uint8_t reset_flag); extern void gimbal_control(float yaw_delta, float pitch_delta, uint8_t reset_flag);
/* 云台任一电机掉线返回 1 */ /* 云台任一电机掉线返回 1 */
extern uint8_t gimbal_motor_offline(void); extern uint8_t gimbal_motor_offline(void);
/* ===== 2026-10-15 台架调参临时接口(已停用,保留备查) =====
* 启用时把 #if 0 改 #if 1,并同步启用 gimbal.c / gimbal_task.c / robot_param.h
* / debug_task.c 里的同名停用块 */
#if 0
extern float gimbal_bench_pitch_offset; // 位置环设定值附加偏置(°),台架方波发生器写入
extern float gimbal_pitch_setpoint_deg(void); // 位置环实际设定值(°),供调参遥测读取
extern float gimbal_pitch_speed_ref(void); // 位置环输出=速度环目标(°/s),判断内环是否限幅
#endif
/* 云台输出清零(停车) */ /* 云台输出清零(停车) */
extern void gimbal_output_zero(void); extern void gimbal_output_zero(void);
云台/modules/shoot/shoot.c → 云台/modules/control/shoot.c
+8 -7
View File
@@ -1,10 +1,10 @@
/* ============================================================================= /* =============================================================================
* shoot.c - 发射机构:摩擦轮 2xM3508(0x200 CAN1 槽1/2)+ 拨弹 M2006 * shoot.c - 发射机构:摩擦轮 2xM3508(0x200 CAN1 槽1/2)+ 拨弹 M2006(0x200 CAN2 槽1)
* 逻辑(与 legacy 一致): * 逻辑(与 legacy 一致):
* 摩擦轮开关受发射机构上电状态门控(可由 DEBUG_SHOOT_WITHOUT_REFEREE 绕过), * 摩擦轮开关受发射机构上电状态门控(可由 DEBUG_SHOOT_WITHOUT_REFEREE 绕过),
* 开启时联动激光(PC8=TIM3_CH3 PWM,占空比 0/满 对应关/开) * 开启时联动激光(PC8=TIM3_CH3 PWM,占空比 0/满 对应关/开)
* 拨弹仅在摩擦轮开启后允许;fire=GIMBAL_RAMMER_SPEED(-5400),eject 反向 * 拨弹仅在摩擦轮开启后允许;fire=GIMBAL_RAMMER_SPEED(-5400),eject 反向
* 拨弹电流经 shoot_share_rammer_current() 交给云台共帧发送(0x1FF CAN1) * 拨弹电流经 shoot_send_cmd() 经 CAN2 0x200 帧直接发送(ID 1)
* ========================================================================== */ * ========================================================================== */
#include "shoot.h" #include "shoot.h"
#include "gimbal.h" #include "gimbal.h"
@@ -43,7 +43,7 @@ void shoot_init(void)
PID_PositionStructureInit(&rammer_pid, GIMBAL_RAMMER_SPEED); PID_PositionStructureInit(&rammer_pid, GIMBAL_RAMMER_SPEED);
PID_PositionSetParameter(&rammer_pid, GIMBAL_RAMMER_KP, GIMBAL_RAMMER_KI, GIMBAL_RAMMER_KD); PID_PositionSetParameter(&rammer_pid, GIMBAL_RAMMER_KP, GIMBAL_RAMMER_KI, GIMBAL_RAMMER_KD);
PID_PositionSetEkRange(&rammer_pid, -20, 20); PID_PositionSetEkRange(&rammer_pid, -20, 20);
PID_PositionSetOUTRange(&rammer_pid, -30000, 30000); PID_PositionSetOUTRange(&rammer_pid, -10000, 10000);
laser_init(); laser_init();
} }
@@ -67,7 +67,7 @@ void shoot_control(uint8_t friction_on, uint8_t fire, uint8_t eject, uint8_t sho
shoot_friction_flag = 0; shoot_friction_flag = 0;
} }
/* 拨弹:仅在摩擦轮开启时允许 */ /* 拨弹:仅在摩擦轮开启后允许 */
if (shoot_friction_flag) if (shoot_friction_flag)
{ {
if (fire) if (fire)
@@ -83,19 +83,20 @@ void shoot_control(uint8_t friction_on, uint8_t fire, uint8_t eject, uint8_t sho
PID_PositionCalc(&friction_l_pid, motor_friction_l.fdb.rotor_speed); PID_PositionCalc(&friction_l_pid, motor_friction_l.fdb.rotor_speed);
PID_PositionCalc(&friction_r_pid, motor_friction_r.fdb.rotor_speed); PID_PositionCalc(&friction_r_pid, motor_friction_r.fdb.rotor_speed);
PID_PositionCalc(&rammer_pid, motor_rammer.fdb.rotor_speed); PID_PositionCalc(&rammer_pid, motor_rammer.fdb.rotor_speed);
/* 拨弹电流交给云台控制帧统一发送(0x1FF CAN1 共帧) */
shoot_share_rammer_current((int16_t)rammer_pid.OUT);
} }
void shoot_send_cmd(void) void shoot_send_cmd(void)
{ {
/* 摩擦轮 2×M3508:CAN1 0x200 槽1/2 */
dji_motor_send_current_low(&hcan1, (int16_t)friction_l_pid.OUT, (int16_t)friction_r_pid.OUT, 0, 0); dji_motor_send_current_low(&hcan1, (int16_t)friction_l_pid.OUT, (int16_t)friction_r_pid.OUT, 0, 0);
/* 拨弹盘 M2006(ID 1):CAN2 0x200 槽1 */
dji_motor_send_current_low(&hcan2, (int16_t)rammer_pid.OUT, 0, 0, 0);
} }
void shoot_output_zero(void) void shoot_output_zero(void)
{ {
dji_motor_send_current_low(&hcan1, 0, 0, 0, 0); dji_motor_send_current_low(&hcan1, 0, 0, 0, 0);
dji_motor_send_current_low(&hcan2, 0, 0, 0, 0);
LASER_OFF(); LASER_OFF();
shoot_friction_flag = 0; shoot_friction_flag = 0;
} }
云台/modules/shoot/shoot.h → 云台/modules/control/shoot.h
+2 -4
View File
@@ -3,7 +3,7 @@
#include <stdint.h> #include <stdint.h>
/* 发射机构:摩擦轮 2×M3508(0x200 CAN1 槽1/2)+ 拨弹 M2006(电流由云台共帧发送) /* 发射机构:摩擦轮 2×M3508(0x200 CAN1 槽1/2)+ 拨弹 M2006(0x200 CAN2 槽1)
* 摩擦轮开启时联动激光(PC8,经 TIM3 CH3 PWM) */ * 摩擦轮开启时联动激光(PC8,经 TIM3 CH3 PWM) */
/* 初始化摩擦轮/拨弹 PID(can_start 之后调用) */ /* 初始化摩擦轮/拨弹 PID(can_start 之后调用) */
@@ -14,7 +14,7 @@ extern void shoot_init(void);
* shooter_power_on:发射机构上电状态(来自底盘 0x019) */ * shooter_power_on:发射机构上电状态(来自底盘 0x019) */
extern void shoot_control(uint8_t friction_on, uint8_t fire, uint8_t eject, uint8_t shooter_power_on); extern void shoot_control(uint8_t friction_on, uint8_t fire, uint8_t eject, uint8_t shooter_power_on);
/* 发送摩擦轮电流帧(0x200 CAN1) */ /* 发送摩擦轮(0x200 CAN1)与拨弹(0x200 CAN2)电流帧 */
extern void shoot_send_cmd(void); extern void shoot_send_cmd(void);
/* 摩擦轮输出清零 */ /* 摩擦轮输出清零 */
@@ -23,6 +23,4 @@ extern void shoot_output_zero(void);
/* 摩擦轮当前开关状态(供激光联动与调试) */ /* 摩擦轮当前开关状态(供激光联动与调试) */
extern uint8_t shoot_friction_flag; extern uint8_t shoot_friction_flag;
/* 拨弹电流写入共享变量(由 shoot 模块调用,云台共帧发送) */
extern void shoot_share_rammer_current(int16_t current);
#endif #endif
云台/modules/dev/buzzer.c
+117
View File
@@ -0,0 +1,117 @@
/* =============================================================================
* buzzer.c - C 板无源蜂鸣器(TIM4_CH3, PD14)
* 说明: 音调表与 legacy 完全一致(ARR 固定 10 分频,改 PSC 变调,
* 装载用 HAL_TIM_GenerateEvent 立即生效);buzzer_init 必须在
* CAN_Task 中调用一次启动 PWM 输出,否则所有报警/音效无声
* ========================================================================== */
#include "buzzer.h"
#include "tim.h"
#include "robot_param.h"
extern TIM_HandleTypeDef htim4;
/* 蜂鸣器音调频率表(ARR 固定 10 分频下的 PSC 值,与 legacy 一致) */
static const uint16_t buzzer_tone_freq[37] = {
0,
32107, 30305, 28604, 26999, 25483, 24053, 22703, 21429, 20226, 19091, 18019, 17008,
16053, 15152, 14302, 13499, 12742, 12026, 11352, 10714, 10113, 9545, 9010, 8504,
8027, 7576, 7151, 6750, 6371, 6013, 5676, 5357, 5056, 4773, 4505, 4252,
};
void buzzer_init(void)
{
/* 与 legacy 相同:ARR=10 分频,CubeMX 的 ARR=65535 改为 9 */
__HAL_TIM_SET_AUTORELOAD(&htim4, 10 - 1);
__HAL_TIM_SET_PRESCALER(&htim4, 19091);
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 0);
HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_3);
}
void buzzer_on(buzzer_tone_e tone)
{
if (tone != BUZZER_P)
{
__HAL_TIM_SET_PRESCALER(&htim4, buzzer_tone_freq[tone]);
HAL_TIM_GenerateEvent(&htim4, TIM_EVENTSOURCE_UPDATE); // 立即装载 PSC
/* 占空比 30%:抑制高次尖锐谐波,使吉他拨弦音色圆润纯净 */
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 3);
}
else
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 0);
}
void buzzer_off(void)
{
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 0);
}
/* ==========================================================================
* 周杰伦《晴天》- 标志性吉他前奏伴奏 Solo (纯享循环版)
* 调式: 1=C,处于 C 板蜂鸣器最佳频响区间(500~1000Hz)
* 结构: 完整还原原曲前奏 4 小节经典木吉他单音分解
* [小节1: Em] 3 2 1 1 2 1 5_ 1 2
* [小节2: C ] 3 2 1 1 2 1 6_ 1 2
* [小节3: G ] 3 2 1 1 2 1 5_ 1 2
* [小节4: D ] 3 2 1 1 2 1 2 3 5 3 2 1 2 -
* 吐音设计: 每个音符保留 BUZZER_P 间隔,模拟吉他真实的清脆拨弦与泛音
* ========================================================================== */
static const buzzer_tone_e buzzer_melody[] = {
/* ---- 小节 1 (Em): 3 2 1 1 2 1 | 5_ 1 2 ---- */
BUZZER_M3, BUZZER_P, // 3 (Mi)
BUZZER_M2, BUZZER_P, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_M1, // 1 (Do)
BUZZER_M2, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_L5, BUZZER_P, // 低音 5 (Sol)
BUZZER_M1, // 1 (Do)
BUZZER_M2, // 2 (Re)
/* ---- 小节 2 (C): 3 2 1 1 2 1 | 6_ 1 2 ---- */
BUZZER_M3, BUZZER_P, // 3 (Mi)
BUZZER_M2, BUZZER_P, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_M1, // 1 (Do)
BUZZER_M2, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_L6, BUZZER_P, // 低音 6 (La)
BUZZER_M1, // 1 (Do)
BUZZER_M2, // 2 (Re)
/* ---- 小节 3 (G): 3 2 1 1 2 1 | 5_ 1 2 ---- */
BUZZER_M3, BUZZER_P, // 3 (Mi)
BUZZER_M2, BUZZER_P, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_M1, // 1 (Do)
BUZZER_M2, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_L5, BUZZER_P, // 低音 5 (Sol)
BUZZER_M1, // 1 (Do)
BUZZER_M2, // 2 (Re)
/* ---- 小节 4 (D): 3 2 1 1 2 1 | 2 3 5 3 2 1 2 - ---- */
BUZZER_M3, BUZZER_P, // 3 (Mi)
BUZZER_M2, BUZZER_P, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_M1, // 1 (Do)
BUZZER_M2, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_M2, BUZZER_P, // 2 (Re)
BUZZER_M3, // 3 (Mi)
BUZZER_M5, // 5 (Sol)
BUZZER_M3, // 3 (Mi)
BUZZER_M2, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_M2, BUZZER_M2, BUZZER_M2, BUZZER_M2, BUZZER_M2, BUZZER_P, // 2 (Re 长音结音)
BUZZER_P, BUZZER_P, BUZZER_P, BUZZER_P, // 尾部休止 400ms 后循环
};
void buzzer_play_melody(uint16_t step)
{
const uint16_t steps =
(uint16_t)(sizeof(buzzer_melody) / sizeof(buzzer_melody[0]));
#ifndef BUZZER_MELODY_SPEED_DIV
#define BUZZER_MELODY_SPEED_DIV 1
#endif
buzzer_on(buzzer_melody[(step / BUZZER_MELODY_SPEED_DIV) % steps]);
}
底盘/modules/BUZZER/buzzer.h → 云台/modules/dev/buzzer.h
+6
View File
@@ -16,4 +16,10 @@ typedef enum
extern void buzzer_init(void); extern void buzzer_init(void);
extern void buzzer_on(buzzer_tone_e tone); extern void buzzer_on(buzzer_tone_e tone);
extern void buzzer_off(void); extern void buzzer_off(void);
/* ---- 调试用旋律 ----
* 按固定 100ms/步 推进的短旋律(音符表见 buzzer.c),用于把掉线警告音换成
* 音乐。step 由调用方每 100ms 递增 1,内部对旋律总步数取模自动循环;
* 旋律中的空拍 BUZZER_P 由 buzzer_on() 解释为静音,用来断音。 */
extern void buzzer_play_melody(uint16_t step);
#endif #endif
云台/modules/LED/led.c → 云台/modules/dev/led.c
View File
云台/modules/LED/led.h → 云台/modules/dev/led.h
View File
云台/modules/gimbal/gimbal.c
-205
View File
@@ -1,205 +0,0 @@
/* =============================================================================
* gimbal.c - 云台双轴控制(串级 PID)
* Yaw(2026-09 重写):双陀螺环,见 gimbal.h 与 robot_param.h 说明
* 位置环:err(°) = 目标角 - SIGN*att_deg_yaw -> 目标角速度(°/s,限幅)
* 速度环:err(°/s) = 目标角速度 - SIGN*gyro_z -> 电压(限幅)
* 稳定性条件:SIGN*G>0,G 为"电压->陀螺 z 角速度"的装配增益,
* 由 gimbal_detect_yaw_sign() 上电实测,彻底消除双传感器符号打架
* Pitch:位置环用 QEKF Roll(安装方向),速度环转子转速(legacy 案,无符号史问题)
* 发送:CAN2 0x1FF 槽1=Yaw 电压;CAN1 0x1FF 槽2=Pitch 电压,槽3=拨弹电流
* ========================================================================== */
#include "gimbal.h"
#include "shoot.h"
#include "pid.h"
#include "attitude.h"
#include "bmi088.h"
#include "can.h"
#include "robot_param.h"
#include "filter.h"
#ifndef RAD_TO_ANGLE
#define RAD_TO_ANGLE 57.295779513082320876798154814105f
#endif
dji_motor_t motor_yaw_gimbal;
dji_motor_t motor_pitch_gimbal;
dji_motor_t motor_friction_l;
dji_motor_t motor_friction_r;
dji_motor_t motor_rammer;
static PID_PositionTypeDef yaw_angle_pid, yaw_speed_pid;
static PID_PositionTypeDef pitch_angle_pid, pitch_speed_pid;
static float yaw_target_deg; // 世界系目标角(°)
static float pitch_target_deg; // 目标角(°)
static float yaw_voltage;
static float pitch_voltage;
/* 装配符号:0=未检测,±1=已测定;未检测前 gimbal_control 不应被调用 */
static float yaw_sign_f = 0.0f;
static uint8_t yaw_sign_valid = 0;
/* 拨弹电流由 shoot 模块写入,云台与拨弹共帧(0x1FF CAN1)发送 */
static int16_t rammer_current_shared = 0;
void shoot_share_rammer_current(int16_t current);
void gimbal_motor_init(void)
{
dji_motor_init(&motor_yaw_gimbal, &hcan2, 0x205, DJI_MOTOR_6020);
dji_motor_init(&motor_pitch_gimbal, &hcan1, 0x206, DJI_MOTOR_6020);
dji_motor_init(&motor_friction_l, &hcan1, 0x201, DJI_MOTOR_3508);
dji_motor_init(&motor_friction_r, &hcan1, 0x202, DJI_MOTOR_3508);
dji_motor_init(&motor_rammer, &hcan1, 0x207, DJI_MOTOR_2006);
/* Yaw 位置环:° -> °/s */
PID_PositionStructureInit(&yaw_angle_pid, 0);
PID_PositionSetParameter(&yaw_angle_pid, GIMBAL_YAW_ANGLE_KP, GIMBAL_YAW_ANGLE_KI, GIMBAL_YAW_ANGLE_KD);
PID_PositionSetEkRange(&yaw_angle_pid, -GIMBAL_YAW_ANGLE_EK_DEADBAND, GIMBAL_YAW_ANGLE_EK_DEADBAND);
PID_PositionSetOUTRange(&yaw_angle_pid, -GIMBAL_YAW_ANGLE_OUT_LIMIT, GIMBAL_YAW_ANGLE_OUT_LIMIT);
/* Yaw 速度环:°/s -> 电压 */
PID_PositionStructureInit(&yaw_speed_pid, 0);
PID_PositionSetParameter(&yaw_speed_pid, GIMBAL_YAW_SPEED_KP, GIMBAL_YAW_SPEED_KI, GIMBAL_YAW_SPEED_KD);
PID_PositionSetOUTRange(&yaw_speed_pid, -GIMBAL_YAW_SPEED_OUT_LIMIT, GIMBAL_YAW_SPEED_OUT_LIMIT);
/* Pitch:与 legacy 相同的两环(位置环反馈为 Roll) */
PID_PositionStructureInit(&pitch_angle_pid, 0);
PID_PositionSetParameter(&pitch_angle_pid, GIMBAL_PITCH_ANGLE_KP, GIMBAL_PITCH_ANGLE_KI, GIMBAL_PITCH_ANGLE_KD);
PID_PositionSetEkRange(&pitch_angle_pid, -GIMBAL_PITCH_ANGLE_EK_DEADBAND, GIMBAL_PITCH_ANGLE_EK_DEADBAND);
PID_PositionSetOUTRange(&pitch_angle_pid, -200, 200);
PID_PositionStructureInit(&pitch_speed_pid, 300);
PID_PositionSetParameter(&pitch_speed_pid, GIMBAL_PITCH_SPEED_KP, GIMBAL_PITCH_SPEED_KI, GIMBAL_PITCH_SPEED_KD);
PID_PositionSetEkRange(&pitch_speed_pid, -5, 5);
PID_PositionSetOUTRange(&pitch_speed_pid, -30000, 30000);
}
uint8_t gimbal_detect_yaw_sign(void)
{
if (gimbal_motor_offline())
return 0;
/* 静默期:输出清零等待机械/陀螺稳定 */
dji_motor_send_current_high(&hcan2, 0, 0, 0, 0);
HAL_Delay(50);
/* 正向电压脉冲 60ms,期间平均陀螺 z 角速度 */
float sum = 0.0f;
dji_motor_send_current_high(&hcan2, GIMBAL_YAW_DETECT_VOLTAGE, 0, 0, 0);
for (uint16_t i = 0; i < 60; i++)
{
sum += bmi088_gyro[2]; // rad/s(z 轴向上为正)
HAL_Delay(1);
}
dji_motor_send_current_high(&hcan2, 0, 0, 0, 0);
HAL_Delay(80); // 回稳,随后 gimbal_task 的首次控制会同步目标角
const float rate_dps = sum / 60.0f * RAD_TO_ANGLE;
if (rate_dps > GIMBAL_YAW_DETECT_RATE_MIN)
{
yaw_sign_f = 1.0f; // 正电压->陀螺正转,装配增益 G>0
yaw_sign_valid = 1;
}
else if (rate_dps < -GIMBAL_YAW_DETECT_RATE_MIN)
{
yaw_sign_f = -1.0f; // 皮带交叉等反向装配
yaw_sign_valid = 1;
}
else
return 0; // 被按住/没转:不给结论,由调用方决定重试或回退
return 1;
}
float gimbal_yaw_sign(void)
{
return yaw_sign_valid ? yaw_sign_f : GIMBAL_YAW_SIGN_FALLBACK;
}
int8_t gimbal_encoder_ccw(void)
{
/* 自检:电压正脉冲 -> 陀螺 z 正转(云台 CCW),而电压正方向即编码器
* tick 增大方向,故 tick 增方向 = sign(gimbal 旋转) */
if (!yaw_sign_valid)
return 0;
return (yaw_sign_f > 0.0f) ? 1 : -1;
}
void gimbal_control(float yaw_delta, float pitch_delta, uint8_t reset_flag)
{
/* 双环共用装配符号 */
const float sign = gimbal_yaw_sign();
const float yaw_meas = sign * att_deg_yaw; // °
const float yaw_rate_meas = sign * (bmi088_gyro[2] * RAD_TO_ANGLE); // °/s
/* 首次进入控制:目标角同步为当前姿态(否则 Yaw 目标仍为初始 0°,
* 一闭环就会朝 0° 猛转) */
static uint8_t first_run = 1;
if (first_run)
{
first_run = 0;
yaw_target_deg = yaw_meas;
pitch_target_deg = 0;
}
if (reset_flag)
{
yaw_target_deg = yaw_meas; // Yaw 复位为当前角度
pitch_target_deg = 0; // Pitch 复位为上电位置
PID_PositionClean(&yaw_angle_pid);
PID_PositionClean(&yaw_speed_pid);
PID_PositionClean(&pitch_angle_pid);
PID_PositionClean(&pitch_speed_pid);
}
yaw_target_deg += yaw_delta;
pitch_target_deg += pitch_delta;
/* Yaw 位置环:° -> °/s(反馈为带符号姿态角) */
yaw_angle_pid.Need_Value = yaw_target_deg;
PID_PositionCalc(&yaw_angle_pid, yaw_meas);
/* Yaw 速度环:°/s -> 电压(反馈为带符号陀螺角速度,双环符号自洽) */
yaw_speed_pid.Need_Value = yaw_angle_pid.OUT;
PID_PositionCalc(&yaw_speed_pid, yaw_rate_meas);
yaw_voltage = yaw_speed_pid.OUT;
/* Pitch 串级闭环,位置环反馈为 AHRS Roll(legacy 语义) */
pitch_angle_pid.Need_Value = pitch_target_deg;
PID_PositionCalc(&pitch_angle_pid, att_deg_roll);
pitch_speed_pid.Need_Value = pitch_angle_pid.OUT;
PID_PositionCalc(&pitch_speed_pid, motor_pitch_gimbal.fdb.rotor_speed);
pitch_voltage = pitch_speed_pid.OUT;
/* 发送:CAN2 0x1FF 槽1 = Yaw 电压;CAN1 0x1FF 槽2 = Pitch 电压,槽3 = 拨弹电流 */
dji_motor_send_current_high(&hcan2, (int16_t)yaw_voltage, 0, 0, 0);
dji_motor_send_current_high(&hcan1, 0, (int16_t)pitch_voltage, rammer_current_shared, 0);
}
void shoot_share_rammer_current(int16_t current)
{
rammer_current_shared = current;
}
uint8_t gimbal_motor_offline(void)
{
uint32_t now = HAL_GetTick();
uint8_t rammer_off = 0;
#if GIMBAL_RAMMER_INSTALLED
rammer_off = (motor_rammer.fdb.last_online_tick == 0 ||
now - motor_rammer.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS);
#endif
return (motor_yaw_gimbal.fdb.last_online_tick == 0 || now - motor_yaw_gimbal.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
motor_pitch_gimbal.fdb.last_online_tick == 0 || now - motor_pitch_gimbal.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
motor_friction_l.fdb.last_online_tick == 0 || now - motor_friction_l.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
motor_friction_r.fdb.last_online_tick == 0 || now - motor_friction_r.fdb.last_online_tick > DEVICE_OFFLINE_TIME_MS ||
rammer_off);
}
void gimbal_output_zero(void)
{
dji_motor_send_current_high(&hcan2, 0, 0, 0, 0);
dji_motor_send_current_high(&hcan1, 0, 0, 0, 0);
dji_motor_send_current_low(&hcan1, 0, 0, 0, 0);
}
云台/modules/attitude/attitude.c → 云台/modules/imu/attitude.c
+4
View File
@@ -34,6 +34,10 @@ void attitude_init(void)
void attitude_update(void) void attitude_update(void)
{ {
/* 先做数据路健康检查:IMU 数据一旦冻结,姿态会安静地"保持不动"甚至
* 匀速漂移(见 bmi088.c 的 bmi088_health_check 说明) */
bmi088_health_check();
ins_update(0.001f); // 1kHz 固定节拍 ins_update(0.001f); // 1kHz 固定节拍
att_deg_yaw = INS.YawTotalAngle; // 多圈连续角,云台 Yaw 环直接使用 att_deg_yaw = INS.YawTotalAngle; // 多圈连续角,云台 Yaw 环直接使用
云台/modules/attitude/attitude.h → 云台/modules/imu/attitude.h
View File
底盘/modules/ins/ins.c → 云台/modules/imu/ins.c
+52 -6
View File
@@ -16,11 +16,33 @@
#include "main.h" #include "main.h"
#include <string.h> #include <string.h>
/* z 轴静止零偏标定参数:QEKF 无法观测 z 偏置(见 ins.h),上电采 1s */ /* ---- z 轴静止零偏标定 ----
#define INS_Z_CAL_SAMPLES 1000 * 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 float gyro_z_offset = 0;
static uint8_t z_cal_done = 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; INS_t INS;
static const float xb[3] = {1, 0, 0}; static const float xb[3] = {1, 0, 0};
@@ -31,15 +53,39 @@ void ins_init(void)
{ {
memset(&INS, 0, sizeof(INS)); memset(&INS, 0, sizeof(INS));
/* z 轴静止零偏标定:上电静止采 1s 取平均(x/y 偏置由 EKF 在线估计, /* 等 SPI 采样路稳定,避免把未就绪/残留的样本算进零偏 */
* 无需校准;此期间 ins_update() 不输出,控制任务由 ins_ready() 门控) */ 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++)
{
float sum = 0; float sum = 0;
uint16_t valid = 0;
for (uint16_t i = 0; i < INS_Z_CAL_SAMPLES; i++) for (uint16_t i = 0; i < INS_Z_CAL_SAMPLES; i++)
{ {
sum += bmi088_gyro[2]; 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); HAL_Delay(1);
} }
gyro_z_offset = sum / INS_Z_CAL_SAMPLES;
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;
}
/* 否则认为上电时车体在动,整轮作废、再来一轮 */
}
ins_gyro_z_offset = gyro_z_offset;
z_cal_done = 1; z_cal_done = 1;
INS.Initialized = 1; INS.Initialized = 1;
云台/modules/ins/ins.h → 云台/modules/imu/ins.h
+7
View File
@@ -48,6 +48,13 @@ extern void ins_update(float dt);
/* 1-标定完成且已有有效姿态输出(控制任务以此为门控) */ /* 1-标定完成且已有有效姿态输出(控制任务以此为门控) */
extern uint8_t ins_ready(void); 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 BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q);
extern void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q); extern void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q);
云台/modules/ins/QuaternionEKF.c → 云台/modules/imu/qekf/QuaternionEKF.c
View File
云台/modules/ins/QuaternionEKF.h → 云台/modules/imu/qekf/QuaternionEKF.h
View File
云台/modules/ins/kalman_filter.c → 云台/modules/imu/qekf/kalman_filter.c
View File
云台/modules/ins/kalman_filter.h → 云台/modules/imu/qekf/kalman_filter.h
View File
云台/modules/motor/dji_motor.c
+31 -9
View File
@@ -7,8 +7,7 @@
* 记录 last_online_tick 供掉线检测 * 记录 last_online_tick 供掉线检测
* 控制路径(任务上下文): * 控制路径(任务上下文):
* dji_motor_send_current_low/high() 0x200 / 0x1FF 电流帧(+/-16384 = 20A/10A) * dji_motor_send_current_low/high() 0x200 / 0x1FF 电流帧(+/-16384 = 20A/10A)
* dji_motor_send_voltage_high() 0x2FF 电压帧(GM6020 ID5~7) * dji_motor_send_gm6020_current_low/high() 0x1FE / 0x2FE GM6020 电流帧(+/-16384 = 3A)
* 注意 0x1FF 帧为 M3508/M2006 高位电流与 GM6020 低位电压共用,按槽位区分
* ========================================================================== */ * ========================================================================== */
#include "dji_motor.h" #include "dji_motor.h"
@@ -20,7 +19,8 @@
static dji_motor_t * dji_motor_list[DJI_MOTOR_MAX]; static dji_motor_t * dji_motor_list[DJI_MOTOR_MAX];
static uint8_t dji_motor_count = 0; static uint8_t dji_motor_count = 0;
static void dji_motor_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc); static void dji_motor_can1_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc);
static void dji_motor_can2_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc);
/* 解析一帧反馈,多圈累计(无 ±1 圈限制,底盘小陀螺时 Yaw 电机可连续旋转) */ /* 解析一帧反馈,多圈累计(无 ±1 圈限制,底盘小陀螺时 Yaw 电机可连续旋转) */
static void dji_motor_parse(dji_motor_t * motor, const uint8_t data[8]) static void dji_motor_parse(dji_motor_t * motor, const uint8_t data[8])
@@ -71,16 +71,32 @@ void dji_motor_init(dji_motor_t * motor, hcan_t * hcan, uint16_t rx_id, dji_moto
if (dji_motor_count < DJI_MOTOR_MAX) if (dji_motor_count < DJI_MOTOR_MAX)
{ {
dji_motor_list[dji_motor_count++] = motor; dji_motor_list[dji_motor_count++] = motor;
can_rx_register(hcan, rx_id, dji_motor_rx_callback); if (hcan == &hcan1)
can_rx_register(hcan, rx_id, dji_motor_can1_rx_callback);
else
can_rx_register(hcan, rx_id, dji_motor_can2_rx_callback);
} }
} }
static void dji_motor_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc) static void dji_motor_can1_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
{ {
(void)dlc; (void)dlc;
for (uint8_t i = 0; i < dji_motor_count; i++) for (uint8_t i = 0; i < dji_motor_count; i++)
{ {
if (dji_motor_list[i]->rx_id == std_id) if (dji_motor_list[i]->hcan == &hcan1 && dji_motor_list[i]->rx_id == std_id)
{
dji_motor_parse(dji_motor_list[i], rx_data);
return;
}
}
}
static void dji_motor_can2_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
{
(void)dlc;
for (uint8_t i = 0; i < dji_motor_count; i++)
{
if (dji_motor_list[i]->hcan == &hcan2 && dji_motor_list[i]->rx_id == std_id)
{ {
dji_motor_parse(dji_motor_list[i], rx_data); dji_motor_parse(dji_motor_list[i], rx_data);
return; return;
@@ -119,8 +135,14 @@ void dji_motor_send_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t
dji_motor_send_int16x4(hcan, 0x1FF, v); dji_motor_send_int16x4(hcan, 0x1FF, v);
} }
void dji_motor_send_voltage_high(hcan_t * hcan, int16_t v5, int16_t v6, int16_t v7) void dji_motor_send_gm6020_current_low(hcan_t * hcan, int16_t i1, int16_t i2, int16_t i3, int16_t i4)
{ {
const int16_t v[4] = {v5, v6, v7, 0}; const int16_t v[4] = {i1, i2, i3, i4};
dji_motor_send_int16x4(hcan, 0x2FF, v); dji_motor_send_int16x4(hcan, 0x1FE, v);
}
void dji_motor_send_gm6020_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t i7, int16_t i8)
{
const int16_t v[4] = {i5, i6, i7, i8};
dji_motor_send_int16x4(hcan, 0x2FE, v);
} }
云台/modules/motor/dji_motor.h
+7 -6
View File
@@ -5,13 +5,13 @@
/* DJI 电机(M3508/M2006/GM6020)统一驱动 /* DJI 电机(M3508/M2006/GM6020)统一驱动
* - 反馈:can_start() 后通过 can_rx_register 按 rx_id 注册,接收中断内解析(多圈累计) * - 反馈:can_start() 后通过 can_rx_register 按 rx_id 注册,接收中断内解析(多圈累计)
* - 控制:组装 0x200/0x1FF/0x2FF 控制帧,经 CAN_SendTxMessage 发送 */ * - 控制:组装 0x200/0x1FF/0x1FE/0x2FE 控制帧,经 CAN_SendTxMessage 发送 */
typedef enum typedef enum
{ {
DJI_MOTOR_3508, // 减速比 3591:187(≈19:1), 转矩常数 0.3N·m/A DJI_MOTOR_3508, // 减速比 3591:187(≈19:1), 转矩常数 0.3N·m/A
DJI_MOTOR_2006, // 减速比 36:1, 转矩常数 0.18N·m/A DJI_MOTOR_2006, // 减速比 36:1, 转矩常数 0.18N·m/A
DJI_MOTOR_6020, // 直驱, 电压控制 DJI_MOTOR_6020, // 直驱, 电流控制(ID1~4 为 0x1FE 帧, ±16384 对应 ±3A)
} dji_motor_type_e; } dji_motor_type_e;
typedef struct typedef struct
@@ -20,7 +20,7 @@ typedef struct
int64_t total_tick; // 累计角度位置原始数据(8192*圈数+raw,无 ±1 圈限制) int64_t total_tick; // 累计角度位置原始数据(8192*圈数+raw,无 ±1 圈限制)
float rotor_angle_deg; // 累计转子机械角(°) float rotor_angle_deg; // 累计转子机械角(°)
int16_t rotor_speed; // 转子转速(RPM) int16_t rotor_speed; // 转子转速(RPM)
int16_t raw_torque; // 转矩电流/转矩电压原始数据 int16_t raw_torque; // 转矩电流原始数据
float torque_current; // 转矩电流(A) float torque_current; // 转矩电流(A)
uint8_t temperature; // 电机温度(℃) uint8_t temperature; // 电机温度(℃)
uint32_t last_online_tick; // 最近一次收到反馈的 tick(HAL_GetTick) uint32_t last_online_tick; // 最近一次收到反馈的 tick(HAL_GetTick)
@@ -39,11 +39,12 @@ extern void dji_motor_init(dji_motor_t * motor, hcan_t * hcan, uint16_t rx_id, d
/* ---- 控制帧发送 ---- /* ---- 控制帧发送 ----
* M3508/M2006 ID1~4:0x200 电流帧(±16384 对应 ±20A/±10A) * M3508/M2006 ID1~4:0x200 电流帧(±16384 对应 ±20A/±10A)
* M3508/M2006 ID5~8:0x1FF 电流帧;GM6020 ID1~4:0x1FF 电压帧(±25000) * M3508/M2006 ID5~8:0x1FF 电流帧(±16384)
* GM6020 ID5~7:0x2FF 电压帧 */ * GM6020 电流控制:ID1~4 为 0x1FE 电流帧(±16384 对应 ±3A),ID5~8 为 0x2FE 电流帧(±16384) */
extern void dji_motor_send_current_low(hcan_t * hcan, int16_t i1, int16_t i2, int16_t i3, int16_t i4); extern void dji_motor_send_current_low(hcan_t * hcan, int16_t i1, int16_t i2, int16_t i3, int16_t i4);
extern void dji_motor_send_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t i7, int16_t i8); extern void dji_motor_send_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t i7, int16_t i8);
extern void dji_motor_send_voltage_high(hcan_t * hcan, int16_t v5, int16_t v6, int16_t v7); extern void dji_motor_send_gm6020_current_low(hcan_t * hcan, int16_t i1, int16_t i2, int16_t i3, int16_t i4);
extern void dji_motor_send_gm6020_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t i7, int16_t i8);
/* 通用 int16x4 发送(板间通讯等复用) */ /* 通用 int16x4 发送(板间通讯等复用) */
extern void dji_motor_send_int16x4(hcan_t * hcan, uint32_t std_id, const int16_t val[4]); extern void dji_motor_send_int16x4(hcan_t * hcan, uint32_t std_id, const int16_t val[4]);
云台/modules/BMI088/bmi088.c → 云台/modules/sensor/bmi088.c
+168 -1
View File
@@ -49,6 +49,8 @@ float bmi088_temperature;
uint8_t bmi088_online = 0; uint8_t bmi088_online = 0;
uint8_t bmi088_last_acc_id = 0; uint8_t bmi088_last_acc_id = 0;
uint8_t bmi088_last_gyro_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_tx_buf[8];
static uint8_t spi_rx_buf[8]; static uint8_t spi_rx_buf[8];
@@ -63,6 +65,17 @@ typedef enum
} bmi088_phase_e; } bmi088_phase_e;
static volatile bmi088_phase_e read_phase = BMI088_PHASE_NONE; 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; 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[0] = raw[0] * 0.0008974358974f; // (raw/32768*3)*9.8
bmi088_accel[1] = raw[1] * 0.0008974358974f; bmi088_accel[1] = raw[1] * 0.0008974358974f;
bmi088_accel[2] = raw[2] * 0.0008974358974f; bmi088_accel[2] = raw[2] * 0.0008974358974f;
bmi088_accel_cnt++;
} }
static void process_temperature(void) 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[0] = raw[0] * 0.0010652644360316952f; // (raw/32768*2000)/180*π
bmi088_gyro[1] = raw[1] * 0.0010652644360316952f; bmi088_gyro[1] = raw[1] * 0.0010652644360316952f;
bmi088_gyro[2] = raw[2] * 0.0010652644360316952f; bmi088_gyro[2] = raw[2] * 0.0010652644360316952f;
bmi088_gyro_cnt++;
} }
/* 启动一次 SPI DMA 读(若 SPI 忙则丢弃本次,等下一个数据就绪沿) */ /* 启动一次 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) if (HAL_SPI_GetState(&hspi1) != HAL_SPI_STATE_READY)
return; return;
read_phase = phase; read_phase = phase;
read_phase_tick = HAL_GetTick();
spi_tx_buf[0] = (uint8_t)(reg | 0x80); spi_tx_buf[0] = (uint8_t)(reg | 0x80);
for (uint8_t i = 1; i < len; i++) for (uint8_t i = 1; i < len; i++)
spi_tx_buf[i] = 0x00; 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 else
BMI088_CS_GYRO(0); 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) if (!bmi088_online)
return; // 初始化完成前不启动 DMA 读,避免占用 SPI 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 (GPIO_Pin == INT1_ACCEL_Pin)
{ {
if (read_phase == BMI088_PHASE_NONE) if (read_phase == BMI088_PHASE_NONE)
@@ -303,3 +337,136 @@ void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef * hspi)
break; 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++;
}
云台/modules/BMI088/bmi088.h → 云台/modules/sensor/bmi088.h
+16
View File
@@ -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_acc_id; // 最近一次读到的加速度计 ID(诊断用,正常 0x1E)
extern uint8_t bmi088_last_gyro_id; // 最近一次读到的陀螺仪 ID(诊断用,正常 0x0F) 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 检测循环;失败时绿灯报警并重试) */ /* 初始化(任务上下文调用,含阻塞式寄存器配置与 ID 检测循环;失败时绿灯报警并重试) */
extern void bmi088_init(void); extern void bmi088_init(void);
云台/modules/IST8310/ist8310.c → 云台/modules/sensor/ist8310.c
View File
云台/modules/IST8310/ist8310.h → 云台/modules/sensor/ist8310.h
View File
底盘/Application/Task/Inc/debug_task.h
+3 -2
View File
@@ -1,8 +1,9 @@
/* debug_task.h - USB 虚拟串口诊断任务声明(仅诊断用) */ /* debug_task.h - USB 虚拟串口(CDC)欧拉角输出任务声明 */
#ifndef DEBUG_TASK_H #ifndef DEBUG_TASK_H
#define 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); extern void debug_task(void const * argument);
#endif #endif
底盘/Application/Task/Src/chassis_task.c
+10 -1
View File
@@ -17,6 +17,7 @@
#include "robot_typedef.h" #include "robot_typedef.h"
#include "chassis.h" #include "chassis.h"
#include "attitude.h" #include "attitude.h"
#include "bmi088.h"
#include "referee.h" #include "referee.h"
#include "c_comm.h" #include "c_comm.h"
#include "robot_param.h" #include "robot_param.h"
@@ -120,6 +121,14 @@ void chassis_task(void const * argument)
float vx = cmd_robot.vx * boost; float vx = cmd_robot.vx * boost;
float vy = cmd_robot.vy * boost; float vy = cmd_robot.vy * boost;
/* 坐标系旋转投影:将以云台枪口为准的平移速度转换到底盘坐标系，并叠加 kw 前馈 */
float chassis_vx = vx;
float chassis_vy = vy;
/* 跟随模式下无持续自旋，前馈量为0；小陀螺模式下代入实际自旋角速度 w */
float w_feedforward = (cmd_robot.chassis_mode == CHASSIS_MODE_FOLLOW) ? 0.0f : bmi088_gyro[2];
chassis_vector_to_body(vx, vy, w_feedforward, &chassis_vx, &chassis_vy);
/* 简易功率限制:缓冲能量线性映射为缩放系数 */ /* 简易功率限制:缓冲能量线性映射为缩放系数 */
float scale = 1.0f; float scale = 1.0f;
if (referee.buffer_energy < POWER_LIMIT_BUFFER_LOW) if (referee.buffer_energy < POWER_LIMIT_BUFFER_LOW)
@@ -130,7 +139,7 @@ void chassis_task(void const * argument)
else if (scale > 1.0f) else if (scale > 1.0f)
scale = 1.0f; scale = 1.0f;
chassis_control(vx, vy, w, scale); chassis_control(chassis_vx, chassis_vy, w, scale);
} }
else else
{ {
底盘/Application/Task/Src/debug_task.c
+113 -59
View File
@@ -1,82 +1,136 @@
/* ============================================================================= /* =============================================================================
* debug_task.c - USB 虚拟串口(CDC)诊断输出(仅诊断用,稳定后可整组移除) * debug_task.c - USB 虚拟串口(CDC)姿态输出(欧拉角 + 姿态链路诊断 + 控制量/模式)
* 每 500ms 一行状态:各芯片实测 ID/DR16 帧计数/USART 空闲中断计数/任务循环 * 每 DEBUG_OUT_PERIOD_MS 输出一行。单位: 角度 ° / 加速度 m/s² / 角速度 rad/s
* 计数等;板子插电脑出现虚拟 COM 口,串口助手打开即可(波特率随意)。 *
* 字段含义见重构说明与编译指南.md 的诊断章节 * ---- 欧拉角(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_CMD_OUT=1 时追加) ----
* mode 底盘模式 0=OFF 1=MANUAL 2=FOLLOW 3=SPIN_SLOW 4=SPIN_FAST
* (由本板依据收到的 s2/spin 推出;模式逻辑见 robot_cmd_task.c)
* online 1=板间通讯(CToC)在线 —— 本板唯一的"控制源在线"判据
* vx/vy 底盘平移速度指令(世界系映射值) wz [已废弃,恒 0]
* spin **收到的**小陀螺使能(0x189 bit2,云台侧做 Ctrl 沿检测)
* s2 **收到的**遥控 S2 档位(小陀螺为真时它被覆盖)
* reset 跟随朝向重锁请求(跟随模式重连时置位)
* 注意:本板拿不到"遥控器还是键鼠"这一信息——0x149/0x189 不带该位,
* robot_cmd_task 里 cmd_robot.source 恒被置 0,故这里不输出假字段。
* 与姿态同源(cmd_robot/ctoc_remote),一起看才能判断"模式对不对、量给没给"。
*
* 开关: DEBUG_ATT_HEALTH=0 只输出三个欧拉角;DEBUG_ATT_DIAG=0 去掉诊断位;
* DEBUG_CMD_OUT=1 追加控制量/控制模式。
* ========================================================================== */ * ========================================================================== */
#include "debug_task.h" #include "debug_task.h"
#include "app_tasks.h"
#include "cmsis_os.h" #include "cmsis_os.h"
#include "FreeRTOS.h" #include "FreeRTOS.h"
#include "task.h" #include "task.h"
#include "usb_device.h"
#include "usbd_cdc_if.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 "attitude.h"
#include "gimbal.h"
#elif defined(ROBOT_TYPE_CHASSIS)
#include "chassis.h"
#include "bmi088.h"
#include "ins.h" #include "ins.h"
#include "bmi088.h"
#include "robot_typedef.h" #include "robot_typedef.h"
#include "c_comm.h"
#include <stdio.h>
#include <math.h>
/* 1-输出健康位(cvg/rdy/imu/an/gn); 0-只输出三个欧拉角 */
#define DEBUG_ATT_HEALTH 1
/* 1-再附加原始陀螺/零偏/采样计数(排障用); 0-只留上面的通用健康位 */
#define DEBUG_ATT_DIAG 1
/* 1-追加控制量/控制模式(见文件头); 0-保持原姿态输出不变 */
#define DEBUG_CMD_OUT 1
/* 输出周期(ms)。姿态观测建议 100ms(10Hz) */
#define DEBUG_OUT_PERIOD_MS 100
#if DEBUG_CMD_OUT
/* 底盘模式名(便于人读;数值定义见 robot_typedef.h 的 chassis_mode_e) */
static const char * cmd_mode_name(chassis_mode_e m)
{
switch (m)
{
case CHASSIS_MODE_OFF: return "OFF";
case CHASSIS_MODE_MANUAL: return "MANUAL";
case CHASSIS_MODE_FOLLOW: return "FOLLOW";
case CHASSIS_MODE_SPIN_SLOW: return "SPIN_SLOW";
case CHASSIS_MODE_SPIN_FAST: return "SPIN_FAST";
default: return "?";
}
}
#endif #endif
/* debug_task:USB 虚拟串口(CDC)诊断输出,每 500ms 一行
* 板子插上电脑会出现虚拟 COM 口,用串口助手打开即可看到(波特率随意)
* 输出为纯整数,用于远程定位传感器/通讯初始化问题 */
/* 各模块导出的诊断变量(定义在 imu_task.c / can_task.c) */
extern volatile uint32_t dbg_imu_loop;
extern volatile uint32_t dbg_can_loop;
void debug_task(void const * argument) void debug_task(void const * argument)
{ {
char line[128]; char line[448];
char tail[112];
for (;;) for (;;)
{ {
int len = 0; const float an = sqrtf(bmi088_accel[0] * bmi088_accel[0] +
#if defined(ROBOT_TYPE_GIMBAL) bmi088_accel[1] * bmi088_accel[1] +
len = snprintf(line, sizeof(line), bmi088_accel[2] * bmi088_accel[2]);
"app=%u can=%u imu=%u acc=%02X gyro=%02X bmi=%u ist=%u who=%02X " const float gn = sqrtf(bmi088_gyro[0] * bmi088_gyro[0] +
"cvg=%u ys=%d yaw=%.1f u3g=%02X i3=%u l3=%u d16=%u ref=%u/%u mot=%u\r\n", bmi088_gyro[1] * bmi088_gyro[1] +
app_ready, dbg_can_loop, dbg_imu_loop, bmi088_gyro[2] * bmi088_gyro[2]);
bmi088_last_acc_id, bmi088_last_gyro_id, bmi088_online,
ist8310_online, ist8310_last_who, tail[0] = '\0';
INS.ConvergeFlag, #if DEBUG_ATT_HEALTH && DEBUG_ATT_DIAG
(int)gimbal_yaw_sign(), att_deg_yaw, snprintf(tail, sizeof(tail),
(unsigned)(huart3.gState), dbg_usart3_idle, dbg_usart3_last_len, " gx=%.4f gy=%.4f gz=%.4f zo=%.4f ck=%u ca=%lu cg=%lu se=%lu st=%u",
dr16_frame_count, bmi088_gyro[0], bmi088_gyro[1], bmi088_gyro[2],
referee.link_status, (unsigned)(HAL_GetTick() - referee.last_update_tick), ins_gyro_z_offset, (unsigned)ins_z_cal_ok,
gimbal_motor_offline()); (unsigned long)bmi088_accel_cnt, (unsigned long)bmi088_gyro_cnt,
#elif defined(ROBOT_TYPE_CHASSIS) (unsigned long)bmi088_spi_errors, (unsigned)bmi088_spi_state());
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);
#endif #endif
#if DEBUG_CMD_OUT
char ctl[192];
snprintf(ctl, sizeof(ctl),
" mode=%s online=%u vx=%.3f vy=%.3f wz=%.3f spin=%u s2=%u reset=%u",
cmd_mode_name(cmd_robot.chassis_mode),
(unsigned)cmd_robot.online,
cmd_robot.vx, cmd_robot.vy, cmd_robot.wz_manual,
(unsigned)ctoc_remote.spin_flag,
(unsigned)ctoc_remote.s2,
(unsigned)cmd_robot.gimbal_reset);
#else
const char * ctl = "";
#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%s\r\n",
att_deg_yaw, att_deg_pitch, att_deg_roll,
(unsigned)INS.ConvergeFlag, (unsigned)att_ready,
(unsigned)bmi088_online, an, gn, tail, ctl);
#else
int len = snprintf(line, sizeof(line), "yaw=%.2f pitch=%.2f roll=%.2f%s\r\n",
att_deg_yaw, att_deg_pitch, att_deg_roll, ctl);
#endif
if (len > 0) if (len > 0)
CDC_Transmit_FS((uint8_t *)line, (uint16_t)len); CDC_Transmit_FS((uint8_t *)line, (uint16_t)len);
osDelay(500); osDelay(DEBUG_OUT_PERIOD_MS);
} }
} }
底盘/Application/Task/Src/monitor_task.c
+22 -5
View File
@@ -8,6 +8,8 @@
* 连接音效 = 遥控器上线瞬间两声上扬(M3->H3) * 连接音效 = 遥控器上线瞬间两声上扬(M3->H3)
* CAN 掉线蜂鸣(legacy 语义): 按设备序号 n 响 n 下,2s 一轮,每次只报 * CAN 掉线蜂鸣(legacy 语义): 按设备序号 n 响 n 下,2s 一轮,每次只报
* 最先掉线的设备,设备序号表见 linkcheck_first_offline() * 最先掉线的设备,设备序号表见 linkcheck_first_offline()
* 调试模式(DEBUG_BUZZER_MELODY=1,见 robot_param.h): 上述蜂鸣改为循环
* 播放旋律,避免台架调试时连续滴滴声;红灯常亮等灯语不受影响
* 看门狗: IWDG 约 2.5s;上电 5s 宽限期无条件喂,之后仅当全部任务心跳正常 * 看门狗: IWDG 约 2.5s;上电 5s 宽限期无条件喂,之后仅当全部任务心跳正常
* 才喂——任何任务卡死都会复位,不会整车失控 * 才喂——任何任务卡死都会复位,不会整车失控
* ========================================================================== */ * ========================================================================== */
@@ -83,8 +85,8 @@ static int8_t linkcheck_first_offline(void)
{ {
uint32_t now = HAL_GetTick(); uint32_t now = HAL_GetTick();
#if defined(ROBOT_TYPE_GIMBAL) #if defined(ROBOT_TYPE_GIMBAL)
/* 设备序号:0 摩擦轮L(0x201) 1 摩擦轮R(0x202) 2 Pitch(0x206) /* 设备序号:0 摩擦轮L(CAN1 0x201) 1 摩擦轮R(CAN1 0x202) 2 Pitch(CAN1 0x206)
* 3 Yaw(0x205) 4 拨弹(0x207,安装时) 5 底盘回传(0x019) */ * 3 Yaw(CAN2 0x205) 4 拨弹(CAN2 0x201,安装时) 5 底盘回传(0x019) */
if (LINK_OFFLINE(motor_friction_l.fdb.last_online_tick)) if (LINK_OFFLINE(motor_friction_l.fdb.last_online_tick))
return 0; return 0;
if (LINK_OFFLINE(motor_friction_r.fdb.last_online_tick)) if (LINK_OFFLINE(motor_friction_r.fdb.last_online_tick))
@@ -170,10 +172,24 @@ void LED_Task(void const * argument)
if (can_alarm) if (can_alarm)
{ {
led_set_red(1); 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 窗口 uint8_t w = counter % 20; // 2s 窗口
/* 第 j 声:占 w=[2j,2j+1],响 100ms 停 100ms */ /* 第 j 声:占 w=[2j,2j+1],响 100ms 停 100ms */
if ((w / 2) < n && (w % 2) == 0) if ((w / 2) < n && (w % 2) == 0)
@@ -183,6 +199,7 @@ void LED_Task(void const * argument)
} }
else else
buzzer_off(); buzzer_off();
#endif
} }
else else
{ {
底盘/Application/Task/Src/referee_task.c
+34
View File
@@ -7,6 +7,9 @@
#include "cmsis_os.h" #include "cmsis_os.h"
#include "bsp_usart.h" #include "bsp_usart.h"
#include "referee.h" #include "referee.h"
#include "referee_ui.h"
#include "c_comm.h"
#include "attitude.h"
#include "robot_param.h" #include "robot_param.h"
/* referee_task:裁判系统链路监测 /* referee_task:裁判系统链路监测
@@ -29,6 +32,37 @@ void referee_task(void const * argument)
HAL_GetTick() - referee.last_update_tick > DEVICE_OFFLINE_TIME_MS * 10) HAL_GetTick() - referee.last_update_tick > DEVICE_OFFLINE_TIME_MS * 10)
referee.link_status = 0; referee.link_status = 0;
/* ---- 自定义UI:机器人 -> 裁判系统 -> 选手端 ------------------------
* 本任务 100ms 一轮,内部按 UI_SEND_HZ 分频(5Hz:约 40 图形/帧,
* 115200 链路上约 3.8KB/s)。数据来源:
* 底盘本地 = att_deg_yaw(朝向) + referee(功率/电容)
* 云台 CToC = pitch + 摩擦轮 L/R 转速(0x1B9) + 云台 yaw(0x1A9) + 陀螺位(0x189)
* ------------------------------------------------------------------ */
{
static uint8_t ui_div = 0;
if (++ui_div >= (uint8_t)(10 / UI_SEND_HZ))
{
ui_div = 0;
ui_data_t ui;
ui.pitch_deg = ctoc_ui_pitch_deg;
ui.yaw_deg = ctoc_gimbal_yaw;
ui.hdg_deg = att_deg_yaw;
ui.pwr_w = referee.chassis_power;
ui.pwr_limit_w = (float)referee.chassis_power_limit;
ui.buf_j = referee.buffer_energy;
ui.cap_pct = (uint8_t)(referee.buffer_energy >= 60.0f
? 100
: (referee.buffer_energy * 100.0f / 60.0f));
ui.fric_l_rpm = ctoc_ui_fric_l_rpm;
ui.fric_r_rpm = ctoc_ui_fric_r_rpm;
ui.spin_on = ctoc_remote.spin_flag;
ui.ref_online = referee.link_status;
referee_ui_update(&ui);
}
}
monitor_heartbeat(HB_REFEREE); monitor_heartbeat(HB_REFEREE);
osDelay(100); osDelay(100);
} }
底盘/Application/Task/Src/robot_cmd_task.c
+7 -7
View File
@@ -97,8 +97,8 @@ void robot_cmd_task(void const * argument)
if (!keyboard_online) if (!keyboard_online)
{ {
/* 遥控器:左摇杆,输出与摇杆偏量成比例(legacy 为过死区即全速, /* 遥控器:左摇杆向上(>1050)为抬头(pitch_delta减小,pos向2260减小),门控须 pos > UPPER_LIMIT(2260);
* 体感过灵敏),限位门控与方向保持 legacy */ * 左摇杆向下(<1000)为低头(pitch_delta增大,pos向3950增大),门控须 pos < LOWER_LIMIT(3950) */
if (dr16.rc_l_UD > 1050 && pos > PITCH_GM6020_POSITION_UPPER_LIMIT) if (dr16.rc_l_UD > 1050 && pos > PITCH_GM6020_POSITION_UPPER_LIMIT)
pitch_delta -= GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f * pitch_delta -= GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f *
((float)dr16.rc_l_UD - 1024.0f) / 660.0f; ((float)dr16.rc_l_UD - 1024.0f) / 660.0f;
@@ -113,13 +113,13 @@ void robot_cmd_task(void const * argument)
} }
else else
{ {
/* 键鼠:同改为比例输出 */ /* 键鼠:鼠标前推(>8)为抬头(pitch_delta减小),门控须 pos > UPPER_LIMIT;鼠标后拉(<-8)为低头(pitch_delta增大),门控须 pos < LOWER_LIMIT */
if (keyboard_data.mouse_du < -8 && pos > PITCH_GM6020_POSITION_UPPER_LIMIT) if (keyboard_data.mouse_du > 8 && pos > PITCH_GM6020_POSITION_UPPER_LIMIT)
pitch_delta -= GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f * pitch_delta -= GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f *
(-(float)keyboard_data.mouse_du * PC_MOUSE_DU_SENSITIVITY) / 660.0f;
else if (keyboard_data.mouse_du > 8 && pos < PITCH_GM6020_POSITION_LOWER_LIMIT)
pitch_delta += GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f *
((float)keyboard_data.mouse_du * PC_MOUSE_DU_SENSITIVITY) / 660.0f; ((float)keyboard_data.mouse_du * PC_MOUSE_DU_SENSITIVITY) / 660.0f;
else if (keyboard_data.mouse_du < -8 && pos < PITCH_GM6020_POSITION_LOWER_LIMIT)
pitch_delta += GIMBAL_PITCH_LEVER_RATE / 8192.0f * 360.0f *
(-(float)keyboard_data.mouse_du * PC_MOUSE_DU_SENSITIVITY) / 660.0f;
if (keyboard_data.mouse_rl != 0) if (keyboard_data.mouse_rl != 0)
yaw_delta -= GIMBAL_LEVER_SPEED_MAP_RATE * GIMBAL_YAW_PITCH_SPEED_RATE * yaw_delta -= GIMBAL_LEVER_SPEED_MAP_RATE * GIMBAL_YAW_PITCH_SPEED_RATE *
底盘/Application/robot_param.h
+36 -20
View File
@@ -9,9 +9,11 @@
#define ROBOT_PARAM_H #define ROBOT_PARAM_H
/* ============================ 云台 ============================ */ /* ============================ 云台 ============================ */
#define PITCH_GM6020_POSITION_LOWER_LIMIT 3990 // Pitch 轴编码器值下限(云台抬起方向) #define PITCH_GM6020_POSITION_UPPER_LIMIT 2260 // Pitch 轴抬起极限(最高位,编码器较小值;实测2203撞死限位,留余量设2260)
#define PITCH_GM6020_POSITION_UPPER_LIMIT 2220 // Pitch 轴编码器值上限(云台压低方向) #define PITCH_GM6020_POSITION_LOWER_LIMIT 3950 // Pitch 轴压低极限(最低位,编码器较大值;实测4234撞死限位,留余量设3950)
#define PITCH_GM6020_ENCODER_ZERO 3245 // Pitch 轴编码器零位 #define PITCH_GM6020_ENCODER_ZERO 3200 // Pitch 轴编码器水平零位
#define YAW_GM6020_ENCODER_ZERO 7730 // Yaw 轴编码器零位(机械正前方,0~8191)
#define CHASSIS_SPIN_FEEDFORWARD_K 0.008f // 小陀螺解算时延前馈补偿系数(s)
#define GIMBAL_FRICTION_WHEEL_SPEED 5500.0f // 摩擦轮转速(RPM) #define GIMBAL_FRICTION_WHEEL_SPEED 5500.0f // 摩擦轮转速(RPM)
#define GIMBAL_RAMMER_SPEED -5400.0f// 拨弹盘转速(RPM,负向发射) #define GIMBAL_RAMMER_SPEED -5400.0f// 拨弹盘转速(RPM,负向发射)
@@ -29,35 +31,43 @@
* 这是反复"疯转"的根源,已废弃; * 这是反复"疯转"的根源,已废弃;
* - 现在两环共用陀螺,整条链只有一个装配符号,由上电自检自动测定 * - 现在两环共用陀螺,整条链只有一个装配符号,由上电自检自动测定
* (GIMBAL_YAW_DETECT 见下),无需人工猜; * (GIMBAL_YAW_DETECT 见下),无需人工猜;
* - 单位:角度环 误差° -> 目标角速度°/s;速度环 误差°/s -> 电压。 * - 单位:角度环 误差° -> 目标角速度°/s;速度环 误差°/s -> 电流。
* 调参顺序:先速度环(点动不振荡),再位置环(刚性)——见移植 README §8.1 */ * 调参顺序:先速度环(点动不振荡),再位置环(刚性)——见移植 README §8.1 */
#define GIMBAL_YAW_ANGLE_KP 15.0f // ° -> °/s:10° 误差给 150°/s #define GIMBAL_YAW_ANGLE_KP 20.0f // ° -> °/s:10° 误差给 150°/s
#define GIMBAL_YAW_ANGLE_KI 0.0f #define GIMBAL_YAW_ANGLE_KI 0.0f
#define GIMBAL_YAW_ANGLE_KD 0.0f // 速度环本身即微分,不再叠加 #define GIMBAL_YAW_ANGLE_KD 0.0f // 速度环本身即微分,不再叠加
#define GIMBAL_YAW_ANGLE_EK_DEADBAND 0.05f // 误差死区(°),QEKF 噪声小可用窄死区 #define GIMBAL_YAW_ANGLE_EK_DEADBAND 0.05f // 误差死区(°),QEKF 噪声小可用窄死区
#define GIMBAL_YAW_ANGLE_OUT_LIMIT 360.0f // 目标角速度上限(°/s) #define GIMBAL_YAW_ANGLE_OUT_LIMIT 360.0f // 目标角速度上限(°/s)
#define GIMBAL_YAW_SPEED_KP 60.0f // °/s -> 电压:100°/s 误差给 6000(约 24% 满电压) #define GIMBAL_YAW_SPEED_KP 85.0f // °/s -> 电流:100°/s 误差给 6000(约 36% 满电流)
#define GIMBAL_YAW_SPEED_KI 0.0f // 稳定后可回填 0.5~2 消速度稳态差 #define GIMBAL_YAW_SPEED_KI 0.0f // 稳定后可回填 0.5~2 消速度稳态差
#define GIMBAL_YAW_SPEED_KD 0.0f // 陀螺速率反馈噪声小,先不用 D #define GIMBAL_YAW_SPEED_KD 0.0f // 陀螺速率反馈噪声小,先不用 D
#define GIMBAL_YAW_SPEED_OUT_LIMIT 25000.0f// GM6020 电压限幅 #define GIMBAL_YAW_SPEED_OUT_LIMIT 16384.0f// GM6020 电流限幅(±16384 对应 ±3A)
#define GIMBAL_YAW_SPIN_FEEDFORWARD_K 24.0f // 小陀螺底盘自旋解耦前馈系数(RPM->电流)
/* Yaw 装配符号自动检测:上电自检时给 Yaw 电机一个正向小电压脉冲, /* Yaw 装配符号自动检测:上电自检时给 Yaw 电机一个正向小电流脉冲,
* 看陀螺 z 轴往哪边转——脉冲引起同向转动则符号 +1,反向则 -1。 * 看陀螺 z 轴往哪边转——脉冲引起同向转动则符号 +1,反向则 -1。
* 检测时云台会轻转几度,勿扶云台;失败(被按住/电机离线)时用 FALLBACK */ * 检测时云台会轻转几度,勿扶云台;失败(被按住/电机离线)时用 FALLBACK */
#define GIMBAL_YAW_SIGN_AUTO 1 // 1=上电自动检测 0=直接用 FALLBACK #define GIMBAL_YAW_SIGN_AUTO 1 // 1=上电自动检测 0=直接用 FALLBACK
#define GIMBAL_YAW_SIGN_FALLBACK (1.0f) #define GIMBAL_YAW_SIGN_FALLBACK (1.0f)
#define GIMBAL_YAW_DETECT_VOLTAGE 5000 // 检测脉冲电压(约 20% 满电压) #define GIMBAL_YAW_DETECT_CURRENT 3500 // 检测脉冲电流(约 21% 满电流 16384)
#define GIMBAL_YAW_DETECT_RATE_MIN 20.0f // 判定转动的陀螺速率阈值(°/s) #define GIMBAL_YAW_DETECT_RATE_MIN 20.0f // 判定转动的陀螺速率阈值(°/s)
/* ---- Pitch(位置环陀螺 Roll,速度环转子转速——Pitch 无符号史问题,维持原案) ---- */ /* ---- Pitch(位置环陀螺 Roll,速度环转子转速——Pitch 无符号史问题,维持原案) ---- */
#define GIMBAL_PITCH_ANGLE_EK_DEADBAND 0.3f // 位置环误差死区(°) #define GIMBAL_PITCH_ANGLE_EK_DEADBAND 0.05f // 误差死区(°),收窄死区消除边缘阶跃
#define GIMBAL_PITCH_ANGLE_KP 15.0f #define GIMBAL_PITCH_ANGLE_KP 40.0f
#define GIMBAL_PITCH_ANGLE_KI 0.0f #define GIMBAL_PITCH_ANGLE_KI 0.0f
#define GIMBAL_PITCH_ANGLE_KD 100.0f #define GIMBAL_PITCH_ANGLE_KD 0.0f // 原100.0f是万级电流振颤主因!清零以消除剧烈振荡
#define GIMBAL_PITCH_SPEED_KP 150.0f #define GIMBAL_PITCH_SPEED_KP 100.0f // 速度/电流环Kp
#define GIMBAL_PITCH_SPEED_KI 0.001f #define GIMBAL_PITCH_SPEED_KI 0.0f
#define GIMBAL_PITCH_SPEED_KD 5.0f #define GIMBAL_PITCH_SPEED_KD 0.0f
/* Pitch 重力拟合前馈参数: I_g = K_COS * cos(a) + K_SIN * sin(a) + K_CONST
* 其中 a = (raw_angle - ZERO_ECD) * 360 / 8192 (°) */
#define GIMBAL_PITCH_GRAVITY_ZERO_ECD 3200.0f // 重力补偿拟合基准编码值
#define GIMBAL_PITCH_GRAVITY_K_COS 2856.1f// cos 项系数
#define GIMBAL_PITCH_GRAVITY_K_SIN 4340.3f// sin 项系数
#define GIMBAL_PITCH_GRAVITY_K_CONST 2826.5f // 常数偏置项
#define GIMBAL_FRICTION_WHEEL_KP 16.0f #define GIMBAL_FRICTION_WHEEL_KP 16.0f
#define GIMBAL_FRICTION_WHEEL_KI 0.0f #define GIMBAL_FRICTION_WHEEL_KI 0.0f
@@ -91,10 +101,10 @@
* err = wrap180(云台yaw - 底盘yaw) * err = wrap180(云台yaw - 底盘yaw)
* 不再依赖底盘侧 GM6020(0x205) 反馈,皮带虚位不在环内,无基准锁定时序坑。 * 不再依赖底盘侧 GM6020(0x205) 反馈,皮带虚位不在环内,无基准锁定时序坑。
* 两板 INS 各自上电零点任意,但差值只随相对转动变化,天然正确 */ * 两板 INS 各自上电零点任意,但差值只随相对转动变化,天然正确 */
#define CHASSIS_FOLLOW_KP 0.4f // w 量纲/°:10° 偏差给 w=4 #define CHASSIS_FOLLOW_KP 0.4678f // w 量纲/°:10° 偏差给 w=4
#define CHASSIS_FOLLOW_KI 0.0f // 调试期 0,同旧教训(积分残留) #define CHASSIS_FOLLOW_KI 0.0f // 调试期 0,同旧教训(积分残留)
#define CHASSIS_FOLLOW_KD 0.0f #define CHASSIS_FOLLOW_KD 24.452f
#define CHASSIS_FOLLOW_OUT_LIMIT 4.0f // 输出限幅(介于慢/快陀螺之间) #define CHASSIS_FOLLOW_OUT_LIMIT 5.5f // 输出限幅(介于慢/快陀螺之间)
#define CHASSIS_FOLLOW_DEADBAND 1.0f // 误差死区(°) #define CHASSIS_FOLLOW_DEADBAND 1.0f // 误差死区(°)
/* 跟随符号全自动综合:云台自检(0x1A9 flags)下发编码器 tick 增向, /* 跟随符号全自动综合:云台自检(0x1A9 flags)下发编码器 tick 增向,
@@ -114,10 +124,11 @@
#define CTOC_SLAVE_ID1 0x149 // 云台->底盘:摇杆数据 #define CTOC_SLAVE_ID1 0x149 // 云台->底盘:摇杆数据
#define CTOC_SLAVE_ID2 0x189 // 云台->底盘:控制状态+故障/陀螺使能位 #define CTOC_SLAVE_ID2 0x189 // 云台->底盘:控制状态+故障/陀螺使能位
#define CTOC_SLAVE_ID3 0x1A9 // 云台->底盘:云台世界系 yaw(float)+att_ready #define CTOC_SLAVE_ID3 0x1A9 // 云台->底盘:云台世界系 yaw(float)+att_ready
#define CTOC_SLAVE_ID4 0x1B9 // 云台->底盘:UI 数据(pitch + 摩擦轮 L/R 转速)
/* ============================ CAN ID ============================ */ /* ============================ CAN ID ============================ */
/* 云台板:CAN1 = Pitch GM6020(0x206)+摩擦轮 M3508(0x201/0x202) /* 云台板:CAN1 = Pitch GM6020(0x206,0x1FE)+摩擦轮 M3508(0x201/0x202,0x200)
* CAN2 = Yaw GM6020(0x205)+底盘回传(0x019) * CAN2 = Yaw GM6020(0x205,0x1FE)+拨弹 M2006(0x201,0x200)+底盘回传(0x019)
* 底盘板:CAN1 = 四轮 M3508(0x201~0x204) * 底盘板:CAN1 = 四轮 M3508(0x201~0x204)
* CAN2 = 云台数据(0x149/0x189/0x1A9) */ * CAN2 = 云台数据(0x149/0x189/0x1A9) */
@@ -125,6 +136,11 @@
#define DEBUG_SHOOT_WITHOUT_REFEREE 1 // 1=无裁判系统时也允许开摩擦轮(台架调试用),比赛请改 0 #define DEBUG_SHOOT_WITHOUT_REFEREE 1 // 1=无裁判系统时也允许开摩擦轮(台架调试用),比赛请改 0
#define GIMBAL_RAMMER_INSTALLED 0 // 1=拨弹 M2006 已安装;0=暂缺,不参与掉线判定 #define GIMBAL_RAMMER_INSTALLED 0 // 1=拨弹 M2006 已安装;0=暂缺,不参与掉线判定
/* 1=把 CAN 掉线警告音替换成循环播放的旋律《你》(RM动画片尾曲),报警时播放;
* 旋律音符表见 modules/dev/buzzer.c 的 buzzer_melody[] */
#define DEBUG_BUZZER_MELODY 0
#define BUZZER_MELODY_SPEED_DIV 1 // 旋律降速分频(默认1:每步100ms精准匹配原版带吐音断音节拍)
/* ============================ 掉线检测 ============================ */ /* ============================ 掉线检测 ============================ */
#define DEVICE_OFFLINE_TIME_MS 100 // CAN 设备/裁判系统掉线判定时间 #define DEVICE_OFFLINE_TIME_MS 100 // CAN 设备/裁判系统掉线判定时间
#define CTOC_OFFLINE_TIME_MS 100 // 板间通讯掉线判定时间 #define CTOC_OFFLINE_TIME_MS 100 // 板间通讯掉线判定时间
底盘/Core/Src/gpio.c
+10 -2
View File
@@ -102,9 +102,17 @@ void MX_GPIO_Init(void)
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
HAL_GPIO_Init(CS1_ACCEL_GPIO_Port, &GPIO_InitStruct); 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.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; GPIO_InitStruct.Pull = GPIO_PULLUP;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
底盘/MDK-ARM/Simba-ELCT-FrameWork.uvoptx
+42 -30
View File
@@ -103,7 +103,7 @@
<bEvRecOn>1</bEvRecOn> <bEvRecOn>1</bEvRecOn>
<bSchkAxf>0</bSchkAxf> <bSchkAxf>0</bSchkAxf>
<bTchkAxf>0</bTchkAxf> <bTchkAxf>0</bTchkAxf>
<nTsel>3</nTsel> <nTsel>2</nTsel>
<sDll></sDll> <sDll></sDll>
<sDllPa></sDllPa> <sDllPa></sDllPa>
<sDlgDll></sDlgDll> <sDlgDll></sDlgDll>
@@ -1186,7 +1186,7 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\PID\pid.c</PathWithFileName> <PathWithFileName>..\modules\algorithm\pid.c</PathWithFileName>
<FilenameWithoutPath>pid.c</FilenameWithoutPath> <FilenameWithoutPath>pid.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
@@ -1198,7 +1198,7 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\filter\filter.c</PathWithFileName> <PathWithFileName>..\modules\algorithm\filter.c</PathWithFileName>
<FilenameWithoutPath>filter.c</FilenameWithoutPath> <FilenameWithoutPath>filter.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
@@ -1222,7 +1222,7 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\DR16\dr16.c</PathWithFileName> <PathWithFileName>..\modules\comms\dr16.c</PathWithFileName>
<FilenameWithoutPath>dr16.c</FilenameWithoutPath> <FilenameWithoutPath>dr16.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
@@ -1234,7 +1234,7 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\referee\referee.c</PathWithFileName> <PathWithFileName>..\modules\comms\referee.c</PathWithFileName>
<FilenameWithoutPath>referee.c</FilenameWithoutPath> <FilenameWithoutPath>referee.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
@@ -1246,8 +1246,8 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\LED\led.c</PathWithFileName> <PathWithFileName>..\modules\comms\referee_ui.c</PathWithFileName>
<FilenameWithoutPath>led.c</FilenameWithoutPath> <FilenameWithoutPath>referee_ui.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
</File> </File>
@@ -1258,8 +1258,8 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\BUZZER\buzzer.c</PathWithFileName> <PathWithFileName>..\modules\dev\led.c</PathWithFileName>
<FilenameWithoutPath>buzzer.c</FilenameWithoutPath> <FilenameWithoutPath>led.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
</File> </File>
@@ -1270,8 +1270,8 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\c_comm\c_comm.c</PathWithFileName> <PathWithFileName>..\modules\dev\buzzer.c</PathWithFileName>
<FilenameWithoutPath>c_comm.c</FilenameWithoutPath> <FilenameWithoutPath>buzzer.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
</File> </File>
@@ -1282,8 +1282,8 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\chassis\chassis.c</PathWithFileName> <PathWithFileName>..\modules\comms\c_comm.c</PathWithFileName>
<FilenameWithoutPath>chassis.c</FilenameWithoutPath> <FilenameWithoutPath>c_comm.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
</File> </File>
@@ -1294,8 +1294,8 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\BMI088\bmi088.c</PathWithFileName> <PathWithFileName>..\modules\control\chassis.c</PathWithFileName>
<FilenameWithoutPath>bmi088.c</FilenameWithoutPath> <FilenameWithoutPath>chassis.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
</File> </File>
@@ -1306,8 +1306,8 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\IST8310\ist8310.c</PathWithFileName> <PathWithFileName>..\modules\sensor\bmi088.c</PathWithFileName>
<FilenameWithoutPath>ist8310.c</FilenameWithoutPath> <FilenameWithoutPath>bmi088.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
</File> </File>
@@ -1318,8 +1318,8 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\attitude\attitude.c</PathWithFileName> <PathWithFileName>..\modules\sensor\ist8310.c</PathWithFileName>
<FilenameWithoutPath>attitude.c</FilenameWithoutPath> <FilenameWithoutPath>ist8310.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
</File> </File>
@@ -1330,8 +1330,8 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\ins\kalman_filter.c</PathWithFileName> <PathWithFileName>..\modules\imu\attitude.c</PathWithFileName>
<FilenameWithoutPath>kalman_filter.c</FilenameWithoutPath> <FilenameWithoutPath>attitude.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
</File> </File>
@@ -1342,8 +1342,8 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\ins\QuaternionEKF.c</PathWithFileName> <PathWithFileName>..\modules\imu\qekf\kalman_filter.c</PathWithFileName>
<FilenameWithoutPath>QuaternionEKF.c</FilenameWithoutPath> <FilenameWithoutPath>kalman_filter.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
</File> </File>
@@ -1354,7 +1354,19 @@
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2> <bDave2>0</bDave2>
<PathWithFileName>..\modules\ins\ins.c</PathWithFileName> <PathWithFileName>..\modules\imu\qekf\QuaternionEKF.c</PathWithFileName>
<FilenameWithoutPath>QuaternionEKF.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>91</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\modules\imu\ins.c</PathWithFileName>
<FilenameWithoutPath>ins.c</FilenameWithoutPath> <FilenameWithoutPath>ins.c</FilenameWithoutPath>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<bShared>0</bShared> <bShared>0</bShared>
@@ -1369,7 +1381,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>11</GroupNumber> <GroupNumber>11</GroupNumber>
<FileNumber>91</FileNumber> <FileNumber>92</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@@ -1381,7 +1393,7 @@
</File> </File>
<File> <File>
<GroupNumber>11</GroupNumber> <GroupNumber>11</GroupNumber>
<FileNumber>92</FileNumber> <FileNumber>93</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@@ -1393,7 +1405,7 @@
</File> </File>
<File> <File>
<GroupNumber>11</GroupNumber> <GroupNumber>11</GroupNumber>
<FileNumber>93</FileNumber> <FileNumber>94</FileNumber>
<FileType>5</FileType> <FileType>5</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@@ -1405,7 +1417,7 @@
</File> </File>
<File> <File>
<GroupNumber>11</GroupNumber> <GroupNumber>11</GroupNumber>
<FileNumber>94</FileNumber> <FileNumber>95</FileNumber>
<FileType>5</FileType> <FileType>5</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@@ -1419,13 +1431,13 @@
<Group> <Group>
<GroupName>Middlewares/Library/DSP Library/DSP Library</GroupName> <GroupName>Middlewares/Library/DSP Library/DSP Library</GroupName>
<tvExp>0</tvExp> <tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>12</GroupNumber> <GroupNumber>12</GroupNumber>
<FileNumber>97</FileNumber> <FileNumber>96</FileNumber>
<FileType>4</FileType> <FileType>4</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
底盘/MDK-ARM/Simba-ELCT-FrameWork.uvprojx
+21 -16
View File
@@ -341,7 +341,7 @@
<MiscControls></MiscControls> <MiscControls></MiscControls>
<Define>USE_HAL_DRIVER,STM32F407xx,ARM_MATH_CM4,ROBOT_TYPE_CHASSIS</Define> <Define>USE_HAL_DRIVER,STM32F407xx,ARM_MATH_CM4,ROBOT_TYPE_CHASSIS</Define>
<Undefine></Undefine> <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> </VariousControls>
</Cads> </Cads>
<Aads> <Aads>
@@ -359,7 +359,7 @@
<MiscControls></MiscControls> <MiscControls></MiscControls>
<Define></Define> <Define></Define>
<Undefine></Undefine> <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> </VariousControls>
</Aads> </Aads>
<LDads> <LDads>
@@ -2341,12 +2341,12 @@
<File> <File>
<FileName>pid.c</FileName> <FileName>pid.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\PID\pid.c</FilePath> <FilePath>..\modules\algorithm\pid.c</FilePath>
</File> </File>
<File> <File>
<FileName>filter.c</FileName> <FileName>filter.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\filter\filter.c</FilePath> <FilePath>..\modules\algorithm\filter.c</FilePath>
</File> </File>
<File> <File>
<FileName>dji_motor.c</FileName> <FileName>dji_motor.c</FileName>
@@ -2356,62 +2356,67 @@
<File> <File>
<FileName>dr16.c</FileName> <FileName>dr16.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\DR16\dr16.c</FilePath> <FilePath>..\modules\comms\dr16.c</FilePath>
</File> </File>
<File> <File>
<FileName>referee.c</FileName> <FileName>referee.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\referee\referee.c</FilePath> <FilePath>..\modules\comms\referee.c</FilePath>
</File>
<File>
<FileName>referee_ui.c</FileName>
<FileType>1</FileType>
<FilePath>..\modules\comms\referee_ui.c</FilePath>
</File> </File>
<File> <File>
<FileName>led.c</FileName> <FileName>led.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\LED\led.c</FilePath> <FilePath>..\modules\dev\led.c</FilePath>
</File> </File>
<File> <File>
<FileName>buzzer.c</FileName> <FileName>buzzer.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\BUZZER\buzzer.c</FilePath> <FilePath>..\modules\dev\buzzer.c</FilePath>
</File> </File>
<File> <File>
<FileName>c_comm.c</FileName> <FileName>c_comm.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\c_comm\c_comm.c</FilePath> <FilePath>..\modules\comms\c_comm.c</FilePath>
</File> </File>
<File> <File>
<FileName>chassis.c</FileName> <FileName>chassis.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\chassis\chassis.c</FilePath> <FilePath>..\modules\control\chassis.c</FilePath>
</File> </File>
<File> <File>
<FileName>bmi088.c</FileName> <FileName>bmi088.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\BMI088\bmi088.c</FilePath> <FilePath>..\modules\sensor\bmi088.c</FilePath>
</File> </File>
<File> <File>
<FileName>ist8310.c</FileName> <FileName>ist8310.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\IST8310\ist8310.c</FilePath> <FilePath>..\modules\sensor\ist8310.c</FilePath>
</File> </File>
<File> <File>
<FileName>attitude.c</FileName> <FileName>attitude.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\attitude\attitude.c</FilePath> <FilePath>..\modules\imu\attitude.c</FilePath>
</File> </File>
<File> <File>
<FileName>kalman_filter.c</FileName> <FileName>kalman_filter.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\ins\kalman_filter.c</FilePath> <FilePath>..\modules\imu\qekf\kalman_filter.c</FilePath>
</File> </File>
<File> <File>
<FileName>QuaternionEKF.c</FileName> <FileName>QuaternionEKF.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\ins\QuaternionEKF.c</FilePath> <FilePath>..\modules\imu\qekf\QuaternionEKF.c</FilePath>
</File> </File>
<File> <File>
<FileName>ins.c</FileName> <FileName>ins.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\modules\ins\ins.c</FilePath> <FilePath>..\modules\imu\ins.c</FilePath>
</File> </File>
</Files> </Files>
</Group> </Group>
底盘/MDK-ARM/startup_stm32f407xx.s
+8 -1
View File
@@ -40,7 +40,14 @@ __initial_sp
; <o> Heap Size (in Bytes) <0x0-0xFFFFFFFF:8> ; <o> Heap Size (in Bytes) <0x0-0xFFFFFFFF:8>
; </h> ; </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 AREA HEAP, NOINIT, READWRITE, ALIGN=3
__heap_base __heap_base
底盘/Middlewares/ST/ARM/DSP/Lib/arm_cortexM4lf_math.lib
BIN
View File
Binary file not shown.
底盘/README.md
+36 -113
View File
@@ -1,133 +1,56 @@
# Simba-ELCT-FrameWork # 底盘板固件(Simba-ELCT-FrameWork)
> 步兵机器人**底盘板**工程 · STM32F407IGH6 · Keil + FreeRTOS + CMSIS-DSP
<table> 项目总览、代码地图、任务表、调试与注意事项请先读 **[根 README](../README.md)** 与 **[已知问题与排障](../doc/已知问题与排障.md)**。本文只讲这块板特有的东西。
<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>
[![Author](https://img.shields.io/badge/Author-LSMushui-orange.svg)](https://gitee.com/LSMushui) ---
![language](https://img.shields.io/badge/language-C-blue.svg)
![license](https://img.shields.io/badge/license-GPLv3-green.svg)
## 这块板负责什么
## 简介 - **麦轮底盘**:四轮 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。
> ⚠️ 本工程与云台工程源码高度同构,改一处通常要同步改另一处。动手前请读根 README 的 [§11 已知陷阱](../README.md#11-已知陷阱与注意事项)。
## 欢迎贡献
如果你发现了本项目中的问题和可优化的点，可用创建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
底盘/Simba-ELCT-FrameWork.ioc
+4 -2
View File
@@ -362,12 +362,14 @@ PC2.GPIOParameters=GPIO_Label
PC2.GPIO_Label=HW2 PC2.GPIO_Label=HW2
PC2.Locked=true PC2.Locked=true
PC2.Signal=GPIO_Input 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_Label=INT1_ACCEL
PC4.GPIO_PuPd=GPIO_PULLUP PC4.GPIO_PuPd=GPIO_PULLUP
PC4.Locked=true PC4.Locked=true
PC4.Signal=GPXTI4 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_Label=INT1_GYRO
PC5.GPIO_PuPd=GPIO_PULLUP PC5.GPIO_PuPd=GPIO_PULLUP
PC5.Locked=true PC5.Locked=true
底盘/modules/BUZZER/buzzer.c
-44
View File
@@ -1,44 +0,0 @@
/* =============================================================================
* buzzer.c - C 板无源蜂鸣器(TIM4_CH3, PD14)
* 说明: 音调表与 legacy 完全一致(ARR 固定 10 分频,改 PSC 变调,
* 装载用 HAL_TIM_GenerateEvent 立即生效);buzzer_init 必须在
* CAN_Task 中调用一次启动 PWM 输出,否则所有报警/音效无声
* ========================================================================== */
#include "buzzer.h"
#include "tim.h"
extern TIM_HandleTypeDef htim4;
/* 蜂鸣器音调频率表(ARR 固定 10 分频下的 PSC 值,与 legacy 一致) */
static const uint16_t buzzer_tone_freq[37] = {
0,
32107, 30305, 28604, 26999, 25483, 24053, 22703, 21429, 20226, 19091, 18019, 17008,
16053, 15152, 14302, 13499, 12742, 12026, 11352, 10714, 10113, 9545, 9010, 8504,
8027, 7576, 7151, 6750, 6371, 6013, 5676, 5357, 5056, 4773, 4505, 4252,
};
void buzzer_init(void)
{
/* 与 legacy 相同:ARR=10 分频,CubeMX 的 ARR=65535 改为 9 */
__HAL_TIM_SET_AUTORELOAD(&htim4, 10 - 1);
__HAL_TIM_SET_PRESCALER(&htim4, 19091);
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 0);
HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_3);
}
void buzzer_on(buzzer_tone_e tone)
{
if (tone != BUZZER_P)
{
__HAL_TIM_SET_PRESCALER(&htim4, buzzer_tone_freq[tone]);
HAL_TIM_GenerateEvent(&htim4, TIM_EVENTSOURCE_UPDATE); // 立即装载 PSC
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 5); // 占空比 50%
}
else
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 0);
}
void buzzer_off(void)
{
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 0);
}
底盘/modules/filter/filter.c → 底盘/modules/algorithm/filter.c
View File
底盘/modules/filter/filter.h → 底盘/modules/algorithm/filter.h
View File
底盘/modules/PID/pid.c → 底盘/modules/algorithm/pid.c
View File
底盘/modules/PID/pid.h → 底盘/modules/algorithm/pid.h
View File
云台/modules/c_comm/c_comm.c → 底盘/modules/comms/c_comm.c
+22
View File
@@ -23,6 +23,13 @@ static volatile uint32_t ctoc_att_last_tick = 0;
/* 云台自检下发的编码器方向:+1=tick增向CCW -1=CW 0=未检测(0x1A9 flags) */ /* 云台自检下发的编码器方向:+1=tick增向CCW -1=CW 0=未检测(0x1A9 flags) */
volatile int8_t ctoc_encoder_ccw = 0; volatile int8_t ctoc_encoder_ccw = 0;
/* ---- 0x1B9 自定义UI 专用量(云台提供,底盘本地没有) ---- */
volatile float ctoc_ui_pitch_deg = 0.0f;
volatile float ctoc_ui_fric_l_rpm = 0.0f;
volatile float ctoc_ui_fric_r_rpm = 0.0f;
volatile uint8_t ctoc_ui_spin_flag = 0;
volatile uint32_t ctoc_ui_last_tick = 0;
/* 0x1A9 帧新鲜判定(两板公共:底盘跟随用它门控,云台留作诊断) */ /* 0x1A9 帧新鲜判定(两板公共:底盘跟随用它门控,云台留作诊断) */
uint8_t ctoc_att_fresh(void) uint8_t ctoc_att_fresh(void)
{ {
@@ -179,11 +186,26 @@ static void c_comm_rx_0x1a9(uint32_t std_id, const uint8_t rx_data[8], uint32_t
ctoc_att_last_tick = HAL_GetTick(); ctoc_att_last_tick = HAL_GetTick();
} }
/* 0x1B9 云台->底盘:自定义UI 需要而底盘本地没有的量
* [0..1] pitch(0.01°) [2..3] 左摩擦轮(rpm/100) [4..5] 右摩擦轮(rpm/100)
* [6] bit0=小陀螺使能 */
static void c_comm_rx_0x1b9(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
{
(void)std_id;
(void)dlc;
ctoc_ui_pitch_deg = (float)(int16_t)(((uint16_t)rx_data[1] << 8) | rx_data[0]) * 0.01f;
ctoc_ui_fric_l_rpm = (float)(int16_t)(((uint16_t)rx_data[3] << 8) | rx_data[2]) * 100.0f;
ctoc_ui_fric_r_rpm = (float)(int16_t)(((uint16_t)rx_data[5] << 8) | rx_data[4]) * 100.0f;
ctoc_ui_spin_flag = rx_data[6] & 0x01;
ctoc_ui_last_tick = HAL_GetTick();
}
void c_comm_init(void) void c_comm_init(void)
{ {
can_rx_register(&hcan2, CTOC_SLAVE_ID1, c_comm_rx_0x149); can_rx_register(&hcan2, CTOC_SLAVE_ID1, c_comm_rx_0x149);
can_rx_register(&hcan2, CTOC_SLAVE_ID2, c_comm_rx_0x189); can_rx_register(&hcan2, CTOC_SLAVE_ID2, c_comm_rx_0x189);
can_rx_register(&hcan2, CTOC_SLAVE_ID3, c_comm_rx_0x1a9); can_rx_register(&hcan2, CTOC_SLAVE_ID3, c_comm_rx_0x1a9);
can_rx_register(&hcan2, CTOC_SLAVE_ID4, c_comm_rx_0x1b9);
} }
void c_comm_slave_send(uint8_t motor_fault) void c_comm_slave_send(uint8_t motor_fault)
云台/modules/c_comm/c_comm.h → 底盘/modules/comms/c_comm.h
+14 -3
View File
@@ -12,11 +12,10 @@
* 底盘误判沿会误开小陀螺,勿再使用) * 底盘误判沿会误开小陀螺,勿再使用)
* Data[6] bit0=键1 bit1=键2 bit2=小陀螺使能(本工程扩展) * Data[6] bit0=键1 bit1=键2 bit2=小陀螺使能(本工程扩展)
* bit7=云台电机故障(本工程扩展) * bit7=云台电机故障(本工程扩展)
* 0x1A9 云台->底盘:Data[0..3]=云台世界系 yaw(float,LE) Data[4]=att_ready
* (双 INS 跟随方案的数据源,2026-09 新增)
* 0x1A9 云台->底盘:Data[0..3]=云台世界系 yaw(float,LE) * 0x1A9 云台->底盘:Data[0..3]=云台世界系 yaw(float,LE)
* Data[4] flags:bit0=att_ready bit1=编码器tick增向为CCW * Data[4] flags:bit0=att_ready bit1=编码器tick增向为CCW
* bit2=该位有效(自检完成) [跟随符号自动综合的数据源] * bit2=该位有效(自检完成)
* (双 INS 跟随方案 + 跟随符号自动综合的数据源,2026-09)
* 0x019 底盘->云台:Data[0]发射机构上电状态 Data[1] bit0=底盘电机故障(本工程扩展) * 0x019 底盘->云台:Data[0]发射机构上电状态 Data[1] bit0=底盘电机故障(本工程扩展)
* *
* 故障位语义:任一侧电机掉线即置位,对侧收到后必须停止自己的输出(整车安全) */ * 故障位语义:任一侧电机掉线即置位,对侧收到后必须停止自己的输出(整车安全) */
@@ -64,6 +63,18 @@ extern volatile int8_t ctoc_encoder_ccw;
/* 底盘板:0x1A9 帧是否新鲜(掉线检测) */ /* 底盘板:0x1A9 帧是否新鲜(掉线检测) */
extern uint8_t ctoc_att_fresh(void); extern uint8_t ctoc_att_fresh(void);
/* ---- 0x1B9 云台->底盘:自定义UI 专用数据 ---------------------------------
* 底盘本地没有、但 UI 要显示的量(见 referee_ui):pitch 角度与左右摩擦轮转速。
* 单位:0.01° / rpm(×100 压缩成 int16 传输) */
extern volatile float ctoc_ui_pitch_deg;
extern volatile float ctoc_ui_fric_l_rpm;
extern volatile float ctoc_ui_fric_r_rpm;
extern volatile uint8_t ctoc_ui_spin_flag;
extern volatile uint32_t ctoc_ui_last_tick;
/* 云台板:发送 0x1B9 UI 数据帧(2ms 周期或按需,c_comm_task 调用) */
extern void c_comm_master_send_ui(float pitch_deg, float fric_l_rpm, float fric_r_rpm, uint8_t spin_on);
/* 注册 CAN 接收回调(can_start() 之后调用) */ /* 注册 CAN 接收回调(can_start() 之后调用) */
extern void c_comm_init(void); extern void c_comm_init(void);
云台/modules/DR16/dr16.c → 底盘/modules/comms/dr16.c
View File
底盘/modules/DR16/dr16.h → 底盘/modules/comms/dr16.h
View File
底盘/modules/referee/referee.c → 底盘/modules/comms/referee.c
+45
View File
@@ -216,3 +216,48 @@ void referee_parse_bytes(const uint8_t * data, uint16_t len)
while (len--) while (len--)
referee_parse_byte(*data++); referee_parse_byte(*data++);
} }
/* ============================================================================
* 发送:裁判系统帧(机器人 -> 裁判系统 -> 选手端)
* 帧: A5 | data_length(2,LE,=payload 字节数,不含 cmd_id) | seq | CRC8
* | cmd_id(2,LE) | data | CRC16(2,LE,低字节先)
* CRC8 覆盖前 4 字节;CRC16 覆盖除末 2 字节外全部。
* 自定义UI 走 cmd 0x0301(payload 首 6 字节为 data_cmd_id/sender_ID/receiver_ID)。
* USART6 TX DMA:静态缓冲,发送期间不得被覆盖;UI 刷新率 5Hz 远低于链路能力。
* ==========================================================================*/
#include "bsp_usart.h"
static uint8_t ref_tx_seq = 0;
void referee_send_frame(uint16_t cmd_id, const uint8_t * data, uint16_t len)
{
static uint8_t frame[REF_FRAME_MAX_LEN];
uint16_t i = 0;
if (len > (uint16_t)(REF_FRAME_MAX_LEN - 9) || data == NULL)
return;
frame[i++] = 0xA5;
frame[i++] = (uint8_t)(len & 0xFF);
frame[i++] = (uint8_t)(len >> 8);
frame[i++] = ref_tx_seq++;
uint8_t crc8 = 0xFF;
for (uint16_t k = 0; k < 4; k++)
crc8 = crc8_update(crc8, frame[k]);
frame[i++] = crc8;
frame[i++] = (uint8_t)(cmd_id & 0xFF);
frame[i++] = (uint8_t)(cmd_id >> 8);
for (uint16_t k = 0; k < len; k++)
frame[i++] = data[k];
uint16_t crc16 = 0xFFFF;
for (uint16_t k = 0; k < i; k++)
crc16 = crc16_update(crc16, frame[k]);
frame[i++] = (uint8_t)(crc16 & 0xFF);
frame[i++] = (uint8_t)(crc16 >> 8);
usart6_tx_dma_enable(frame, i);
}
底盘/modules/referee/referee.h → 底盘/modules/comms/referee.h
+6
View File
@@ -30,4 +30,10 @@ extern void referee_parse_bytes(const uint8_t * data, uint16_t len);
typedef void (*referee_keyboard_hook_f)(const uint8_t data[12]); typedef void (*referee_keyboard_hook_f)(const uint8_t data[12]);
extern void referee_set_keyboard_hook(referee_keyboard_hook_f hook); extern void referee_set_keyboard_hook(referee_keyboard_hook_f hook);
/* ---- 发送(机器人 -> 裁判系统/选手端) -------------------------------------
* 用于自定义UI(0x0301 机器人交互数据)。data_length 取 payload 字节数(不含 cmd_id,
* 与解析器一致);CRC8 覆盖前 4 字节,CRC16 覆盖除末 2 字节外全部(低字节先)。
* 内部静态缓冲 + USART6 TX DMA,立即返回,勿高频调用(链路 115200)。 */
extern void referee_send_frame(uint16_t cmd_id, const uint8_t * data, uint16_t len);
#endif #endif
底盘/modules/comms/referee_ui.c
+243
View File
@@ -0,0 +1,243 @@
/* =============================================================================
* referee_ui.c - 裁判系统自定义UI组包与发送(版式见 referee_ui.h / hud_mock5.png)
*
* 组包: 每包 = ext_student_interactive_header_data_t{data_cmd_id, sender_ID,
* receiver_ID} + N×15 字节图形数据,N 由 data_cmd_id 决定(1/2/5/7)。
* 图形名用 3 字符自增计数器(base36)保证唯一;首轮 ADD,之后 MODIFY。
* 发送: referee_send_frame(0x0301, payload, 6 + N*15) -> USART6 DMA
* ==========================================================================*/
#include "referee_ui.h"
#include "referee.h"
#include "robot_param.h"
#include <string.h>
/* 协议要求:图形数据恰好 15 字节(头 11 + 尾 4)。若 pack(1) 失效或位域被重排,
* 这里直接编译失败,而不是把错位的图形发给选手端。 */
_Static_assert(sizeof(ui_head_t) == 11, "ui_head_t 必须是 11 字节");
_Static_assert(sizeof(ui_draw_t) == 15, "ui_draw_t 必须是 15 字节");
_Static_assert(sizeof(ui_num_t) == 15, "ui_num_t 必须是 15 字节");
_Static_assert(sizeof(ui_graphic_t) == 15, "ui_graphic_t 必须是 15 字节");
/* ---- 图形名生成:base26 三字符(17576 个组合,足够) ---- */
static uint16_t ui_name_seq = 0;
static void ui_next_name(uint8_t name[3])
{
uint16_t v = ui_name_seq++;
name[0] = (uint8_t)('A' + (v / 676) % 26);
name[1] = (uint8_t)('A' + (v / 26) % 26);
name[2] = (uint8_t)('A' + v % 26);
}
/* ---- 打包缓冲 ---- */
#define UI_PAYLOAD_MAX (6 + UI_MAX_PER_PACKET * sizeof(ui_graphic_t))
static uint8_t ui_pkt[UI_PAYLOAD_MAX];
static uint8_t ui_pkt_n; /* 当前包里的图形个数 */
static uint8_t ui_op = UI_OP_ADD; /* 首轮 ADD,之后 MOD */
static uint8_t ui_ready = 0; /* 首轮是否已发完 */
static void ui_flush(void)
{
if (ui_pkt_n == 0)
return;
uint16_t cmd;
switch (ui_pkt_n)
{
case 1: cmd = UI_CMD_1; break;
case 2: cmd = UI_CMD_2; break;
case 5: cmd = UI_CMD_5; break;
case 7: cmd = UI_CMD_7; break;
default: return; /* 协议只支持 1/2/5/7,不该发生 */
}
/* header: data_cmd_id / sender_ID / receiver_ID,均小端 */
ui_pkt[0] = (uint8_t)(cmd & 0xFF);
ui_pkt[1] = (uint8_t)(cmd >> 8);
uint16_t sid = (uint16_t)referee.robot_id;
ui_pkt[2] = (uint8_t)(sid & 0xFF);
ui_pkt[3] = (uint8_t)(sid >> 8);
ui_pkt[4] = (uint8_t)(UI_RECEIVER_ID & 0xFF);
ui_pkt[5] = (uint8_t)(UI_RECEIVER_ID >> 8);
referee_send_frame(UI_CMD_BASE, ui_pkt, (uint16_t)(6 + ui_pkt_n * sizeof(ui_graphic_t)));
ui_pkt_n = 0;
}
static ui_graphic_t * ui_new(uint8_t type, uint8_t layer, uint8_t color)
{
if (ui_pkt_n >= UI_MAX_PER_PACKET)
ui_flush();
ui_graphic_t * g = (ui_graphic_t *)&ui_pkt[6 + ui_pkt_n * sizeof(ui_graphic_t)];
memset(g, 0, sizeof(*g));
ui_next_name(g->d.h.name); /* d.h 与 n.h 同址 */
g->d.h.type = type;
g->d.h.layer = layer;
g->d.h.color = color;
g->d.h.operate = ui_op;
ui_pkt_n++;
return g;
}
/* ---- 图元封装(绘制类写 d.*,数值类写 n.val) ---- */
static void ui_line(uint8_t layer, uint8_t color, int x0, int y0, int x1, int y1, int w)
{
ui_graphic_t * g = ui_new(UI_T_LINE, layer, color);
g->d.h.x0 = (uint32_t)x0; g->d.h.y0 = (uint32_t)y0;
g->d.x1 = (uint32_t)x1; g->d.y1 = (uint32_t)y1;
g->d.h.width = (uint32_t)w;
}
static void ui_rect(uint8_t layer, uint8_t color, int x0, int y0, int x1, int y1)
{
ui_graphic_t * g = ui_new(UI_T_RECT, layer, color);
g->d.h.x0 = (uint32_t)x0; g->d.h.y0 = (uint32_t)y0;
g->d.x1 = (uint32_t)x1; g->d.y1 = (uint32_t)y1;
}
static void ui_fval(uint8_t layer, uint8_t color, int x, int y, float v, int digits)
{
ui_graphic_t * g = ui_new(UI_T_FLOAT, layer, color);
g->n.h.x0 = (uint32_t)x; g->n.h.y0 = (uint32_t)y;
g->n.h.width = (uint32_t)digits; /* 浮点:小数位数 */
g->n.val.f = v;
}
static void ui_ival(uint8_t layer, uint8_t color, int x, int y, int32_t v)
{
ui_graphic_t * g = ui_new(UI_T_INT, layer, color);
g->n.h.x0 = (uint32_t)x; g->n.h.y0 = (uint32_t)y;
g->n.val.i = v;
}
static void ui_char(uint8_t layer, uint8_t color, int x, int y, const char * s)
{
ui_graphic_t * g = ui_new(UI_T_CHAR, layer, color);
g->n.h.x0 = (uint32_t)x; g->n.h.y0 = (uint32_t)y;
g->n.h.width = (uint32_t)strlen(s);
for (int i = 0; i < 4; i++) /* 最多 4 字节 ASCII */
g->n.val.s[i] = s[i];
}
/* 角度归一到 [0,360) */
static float ui_norm360(float a)
{
while (a < 0.0f) a += 360.0f;
while (a >= 360.0f) a -= 360.0f;
return a;
}
void referee_ui_update(const ui_data_t * d)
{
if (d == NULL)
return;
/* 裁判系统离线:发一轮删除图层,然后停发(避免选手端残留最后一帧) */
if (!d->ref_online)
{
if (ui_ready)
{
ui_pkt_n = 0;
ui_graphic_t * g = ui_new(UI_T_RECT, UI_L_FRAME, UI_C_BLACK);
g->d.h.operate = UI_OP_DEL;
g->d.h.x0 = 0; g->d.h.y0 = 0; g->d.x1 = 0; g->d.y1 = 0;
ui_flush();
ui_ready = 0;
ui_name_seq = 0;
ui_op = UI_OP_ADD;
}
return;
}
/* ---------------- 左上:预估底盘功率 ---------------- */
uint8_t pw_col = (d->pwr_w > d->pwr_limit_w) ? UI_C_RED
: (d->pwr_w > d->pwr_limit_w * 0.8f) ? UI_C_ORANGE : UI_C_GREEN;
ui_rect(UI_L_FRAME, UI_C_CYAN, UI_PWR_X0, UI_PWR_Y0, UI_PWR_X1, UI_PWR_Y0 + 4);
ui_char(UI_L_TEXT, UI_C_CYAN, UI_PWR_X0 + 20, UI_PWR_Y0 + 30, "PWR");
ui_fval(UI_L_TEXT, UI_C_WHITE, UI_PWR_X0 + 20, UI_PWR_Y0 + 64, d->pwr_w, 1);
ui_char(UI_L_TEXT, UI_C_WHITE, UI_PWR_X0 + 210, UI_PWR_Y0 + 84, "W");
int pw_w_px = (int)((UI_PWR_X1 - UI_PWR_X0 - 40) * (d->pwr_w / (d->pwr_limit_w > 1.0f ? d->pwr_limit_w : 1.0f)));
if (pw_w_px > UI_PWR_X1 - UI_PWR_X0 - 40) pw_w_px = UI_PWR_X1 - UI_PWR_X0 - 40;
if (pw_w_px < 0) pw_w_px = 0;
ui_rect(UI_L_GAUGE, pw_col, UI_PWR_X0 + 20, UI_PWR_Y0 + 120, UI_PWR_X0 + 20 + pw_w_px, UI_PWR_Y0 + 142);
ui_line(UI_L_GAUGE, UI_C_WHITE, UI_PWR_X0 + 20 + (UI_PWR_X1 - UI_PWR_X0 - 40) - 3, UI_PWR_Y0 + 116,
UI_PWR_X0 + 20 + (UI_PWR_X1 - UI_PWR_X0 - 40) - 3, UI_PWR_Y0 + 146, 2);
/* ---------------- 左中:超级电容 ---------------- */
uint8_t cap_col = (d->cap_pct < 15) ? UI_C_RED : (d->cap_pct < 40) ? UI_C_ORANGE : UI_C_CYAN;
ui_rect(UI_L_FRAME, cap_col, UI_CAP_X0, UI_CAP_Y0, UI_CAP_X1, UI_CAP_Y0 + 4);
ui_char(UI_L_TEXT, cap_col, UI_CAP_X0 + 20, UI_CAP_Y0 + 30, "CAP");
ui_ival(UI_L_TEXT, UI_C_WHITE, UI_CAP_X0 + 20, UI_CAP_Y0 + 64, (int32_t)d->cap_pct);
ui_char(UI_L_TEXT, UI_C_WHITE, UI_CAP_X0 + 130, UI_CAP_Y0 + 84, "%");
int cap_px = (int)((UI_CAP_X1 - UI_CAP_X0 - 40) * (d->cap_pct / 100.0f));
ui_rect(UI_L_GAUGE, cap_col, UI_CAP_X0 + 20, UI_CAP_Y0 + 118, UI_CAP_X0 + 20 + cap_px, UI_CAP_Y0 + 140);
/* ---------------- 顶部:罗盘刻度带(底盘正方向朝向) ---------------- */
{
float h = ui_norm360(d->hdg_deg);
ui_rect(UI_L_FRAME, UI_C_CYAN, UI_HDG_X0, UI_HDG_Y0, UI_HDG_X1, UI_HDG_Y0 + 4);
ui_line(UI_L_FRAME, UI_C_CYAN, UI_HDG_X0 + 20, UI_HDG_Y0 + 62, UI_HDG_X1 - 20, UI_HDG_Y0 + 62, 2);
int cx = (UI_HDG_X0 + UI_HDG_X1) / 2;
for (int k = -1; k <= 1; k++) /* 3 条刻度 */
{
int x = cx + k * (UI_HDG_TAPE_HALF / 2);
ui_line(UI_L_FRAME, UI_C_CYAN, x, UI_HDG_Y0 + 40, x, UI_HDG_Y0 + 62, k == 0 ? 3 : 2);
}
ui_ival(UI_L_TEXT, UI_C_CYAN, cx - UI_HDG_TAPE_HALF / 2 - 24, UI_HDG_Y0 + 18,
(int32_t)ui_norm360(h - 30.0f));
ui_ival(UI_L_TEXT, UI_C_CYAN, cx + UI_HDG_TAPE_HALF / 2 - 24, UI_HDG_Y0 + 18,
(int32_t)ui_norm360(h + 30.0f));
ui_line(UI_L_FRAME, UI_C_WHITE, cx, UI_HDG_Y0 + 62, cx - 8, UI_HDG_Y0 + 50, 2);
ui_line(UI_L_FRAME, UI_C_WHITE, cx, UI_HDG_Y0 + 62, cx + 8, UI_HDG_Y0 + 50, 2);
ui_char(UI_L_TEXT, UI_C_CYAN, UI_HDG_X0 + 24, UI_HDG_Y0 + 70, "HDG");
ui_ival(UI_L_TEXT, UI_C_WHITE, UI_HDG_X0 + 90, UI_HDG_Y0 + 66, (int32_t)h);
ui_char(UI_L_TEXT, UI_C_CYAN, UI_HDG_X1 - 170, UI_HDG_Y0 + 70, "YAW");
ui_ival(UI_L_TEXT, UI_C_WHITE, UI_HDG_X1 - 104, UI_HDG_Y0 + 66, (int32_t)ui_norm360(d->yaw_deg));
}
/* ---------------- 中央:俯仰梯 ---------------- */
ui_line(UI_L_FRAME, UI_C_WHITE, UI_LAD_CX - 44, UI_LAD_CY, UI_LAD_CX - 14, UI_LAD_CY, 4);
ui_line(UI_L_FRAME, UI_C_WHITE, UI_LAD_CX + 14, UI_LAD_CY, UI_LAD_CX + 44, UI_LAD_CY, 4);
for (int i = -2; i <= 2; i++) /* ±10/±5/0 五条梯线 */
{
if (i == 0) continue;
int v = i * 5;
int yy = UI_LAD_CY - v * UI_LAD_PX_PER_DEG;
int half = (i % 2 == 0) ? UI_LAD_HALF_LONG : UI_LAD_HALF_SHORT;
ui_line(UI_L_GAUGE, UI_C_CYAN, UI_LAD_CX - half, yy, UI_LAD_CX - 18, yy, 2);
ui_line(UI_L_GAUGE, UI_C_CYAN, UI_LAD_CX + 18, yy, UI_LAD_CX + half, yy, 2);
}
ui_ival(UI_L_TEXT, UI_C_CYAN, UI_LAD_CX - UI_LAD_HALF_SHORT - 40, UI_LAD_CY - 10 * UI_LAD_PX_PER_DEG - 10, 10);
ui_ival(UI_L_TEXT, UI_C_CYAN, UI_LAD_CX + UI_LAD_HALF_SHORT + 12, UI_LAD_CY - 10 * UI_LAD_PX_PER_DEG - 10, 10);
{
int py = UI_LAD_CY - (int)(d->pitch_deg * UI_LAD_PX_PER_DEG);
ui_line(UI_L_GAUGE, UI_C_ORANGE, UI_LAD_CX - UI_LAD_HALF_LONG - 20, py, UI_LAD_CX - UI_LAD_HALF_LONG - 4, py, 4);
ui_char(UI_L_TEXT, UI_C_ORANGE, UI_LAD_CX - UI_LAD_HALF_LONG - 132, py - 10, "PIT");
ui_fval(UI_L_TEXT, UI_C_WHITE, UI_LAD_CX - UI_LAD_HALF_LONG - 76, py - 14, d->pitch_deg, 1);
}
/* ---------------- 右上:摩擦轮速度 ---------------- */
{
uint8_t f_col = (d->fric_l_rpm > 1000.0f && d->fric_r_rpm > 1000.0f) ? UI_C_GREEN : UI_C_ORANGE;
ui_rect(UI_L_FRAME, f_col, UI_FRIC_X0, UI_FRIC_Y0, UI_FRIC_X1, UI_FRIC_Y0 + 4);
ui_char(UI_L_TEXT, UI_C_CYAN, UI_FRIC_X0 + 24, UI_FRIC_Y0 + 34, "FRIC");
ui_ival(UI_L_TEXT, UI_C_WHITE, UI_FRIC_X0 + 24, UI_FRIC_Y0 + 74, (int32_t)d->fric_l_rpm);
ui_ival(UI_L_TEXT, UI_C_WHITE, UI_FRIC_X0 + 250, UI_FRIC_Y0 + 74, (int32_t)d->fric_r_rpm);
ui_char(UI_L_TEXT, f_col, UI_FRIC_X0 + 24, UI_FRIC_Y1 - 60,
(d->fric_l_rpm > 1000.0f && d->fric_r_rpm > 1000.0f) ? "ARMED" : "IDLE");
int bl = (int)((UI_FRIC_Y1 - UI_FRIC_Y0 - 140) * (d->fric_l_rpm / 7000.0f));
int br = (int)((UI_FRIC_Y1 - UI_FRIC_Y0 - 140) * (d->fric_r_rpm / 7000.0f));
if (bl < 0) bl = 0; if (bl > UI_FRIC_Y1 - UI_FRIC_Y0 - 140) bl = UI_FRIC_Y1 - UI_FRIC_Y0 - 140;
if (br < 0) br = 0; if (br > UI_FRIC_Y1 - UI_FRIC_Y0 - 140) br = UI_FRIC_Y1 - UI_FRIC_Y0 - 140;
ui_rect(UI_L_GAUGE, UI_C_GREEN, UI_FRIC_X0 + 30, UI_FRIC_Y1 - 40 - bl, UI_FRIC_X0 + 74, UI_FRIC_Y1 - 40);
ui_rect(UI_L_GAUGE, UI_C_GREEN, UI_FRIC_X0 + 256, UI_FRIC_Y1 - 40 - br, UI_FRIC_X0 + 300, UI_FRIC_Y1 - 40);
}
/* ---------------- 底部:小陀螺状态 ---------------- */
ui_rect(UI_L_FRAME, d->spin_on ? UI_C_GREEN : UI_C_CYAN, UI_SPIN_X0, UI_SPIN_Y0, UI_SPIN_X1, UI_SPIN_Y0 + 4);
ui_char(UI_L_TEXT, UI_C_CYAN, UI_SPIN_X0 + 24, UI_SPIN_Y0 + 34, "SPIN");
if (d->spin_on)
ui_rect(UI_L_GAUGE, UI_C_GREEN, UI_SPIN_X0 + 160, UI_SPIN_Y0 + 26, UI_SPIN_X0 + 268, UI_SPIN_Y0 + 66);
ui_char(UI_L_TEXT, d->spin_on ? UI_C_BLACK : UI_C_WHITE, UI_SPIN_X0 + 184, UI_SPIN_Y0 + 36,
d->spin_on ? "ON" : "OFF");
/* 收尾:最后一包;首轮发完切换为 MODIFY */
ui_flush();
if (!ui_ready)
{
ui_ready = 1;
ui_op = UI_OP_MOD;
}
}
底盘/modules/comms/referee_ui.h
+159
View File
@@ -0,0 +1,159 @@
#ifndef REFEREE_UI_H
#define REFEREE_UI_H
#include <stdint.h>
/* ============================================================================
* referee_ui.h - 裁判系统"自定义UI"(机器人 -> 选手端 图形绘制)
*
* 通道: 底盘板 USART6(裁判系统) --cmd 0x0301 机器人交互数据--> 选手端叠加显示
* 依据: 裁判系统串口协议 自定义UI 章节 + ARUW Taproot / Triton TR-mbed 两套实现
* 版式: 详见 E:\\ELCT\\_mcp_recon\\hud_mock5.png(坐标与本文 UI_* 宏一一对应)
*
* 硬约束(协议):
* 1. 一条消息最多携带 7 个图形(0x0101/0x0102/0x0103/0x0104 = 1/2/5/7 个)
* 2. 图形总数有限,且图形用"名称"标识 -> 首轮用 ADD,之后一律 MODIFY,
* 否则会在选手端把图形表撑爆
* 3. 图层数 0~9;坐标原点在屏幕左上角,x 向右、y 向下,画布 1920x1080
* 4. 不得绘制官方保留区(顶部比分条 / 左下小地图 / 右下战车模块 / 中部两块保留)
* 5. 协议无"圆角矩形/渐变/图标"原语,只能用 线/矩形/圆/椭圆/弧 + 数值/字符
* ==========================================================================*/
/* ---- 图形数据 -------------------------------------------------------------
* 绘制类(线/矩形/圆/椭圆/弧) 与 数值类(浮点/整型/字符) 都是 15 字节:
* 头 11 字节(所有类型共用): name[3] + operate:3 type:3 layer:4 color:4
* sa:9 ea:9 width:10 x0:11 y0:11
* 后 4 字节: 绘制类 = radius:10 x1:11 y1:11;数值类 = 一个 float/int32/char[4]
* 所以用"共用头 + 联合体"表达,两种视图都是 15 字节。
* 必须 pack(1):否则位域会对齐到 4 字节,整体变 16 字节。 */
#pragma pack(push, 1)
typedef struct
{
uint8_t name[3]; /* 图形名(3 字节 ASCII) */
uint32_t operate : 3; /* UI_OP_* 空/加/改/删 */
uint32_t type : 3; /* UI_T_* 图形类型 */
uint32_t layer : 4; /* UI_L_* 图层 0~9 */
uint32_t color : 4; /* UI_C_* 颜色 */
uint32_t sa : 9; /* 起始角(弧/椭圆) */
uint32_t ea : 9; /* 结束角 */
uint32_t width : 10; /* 线宽 / 小数位 / 字符串长度 */
uint32_t x0 : 11; /* 起点 x */
uint32_t y0 : 11; /* 起点 y */
} ui_head_t; /* 3 + 8 = 11 字节 */
typedef struct { ui_head_t h; uint32_t radius : 10, x1 : 11, y1 : 11; } ui_draw_t;
typedef struct { ui_head_t h; union { float f; int32_t i; char s[4]; } val; } ui_num_t;
typedef union
{
ui_draw_t d; /* 绘制类视图 */
ui_num_t n; /* 数值类视图 */
uint8_t raw[15];
} ui_graphic_t;
#pragma pack(pop)
/* ---- 命令 / 操作码 / 图形类型 -------------------------------------------- */
#define UI_CMD_BASE 0x0301 /* 机器人交互数据 */
#define UI_CMD_1 0x0101
#define UI_CMD_2 0x0102
#define UI_CMD_5 0x0103
#define UI_CMD_7 0x0104
#define UI_MAX_PER_PACKET 7
#define UI_OP_NOP 0
#define UI_OP_ADD 1
#define UI_OP_MOD 2 /* 稳态刷新一律用它 */
#define UI_OP_DEL 3
#define UI_T_LINE 0
#define UI_T_RECT 1
#define UI_T_CIRCLE 2
#define UI_T_ELLIPSE 3
#define UI_T_ARC 4
#define UI_T_FLOAT 5
#define UI_T_INT 6
#define UI_T_CHAR 7
/* ---- 颜色(官方 0~8) ------------------------------------------------------ */
#define UI_C_RED 0
#define UI_C_YELLOW 1
#define UI_C_GREEN 2
#define UI_C_ORANGE 3
#define UI_C_PURPLE 4
#define UI_C_PINK 5
#define UI_C_CYAN 6
#define UI_C_BLACK 7
#define UI_C_WHITE 8
/* ---- 图层(协议 0~9) ------------------------------------------------------ */
#define UI_L_FRAME 0 /* 卡片边框/刻度 */
#define UI_L_GAUGE 1 /* 能量条/进度条 */
#define UI_L_TEXT 2 /* 数值/字符 */
/* ---- 刷新率:5Hz 约 3.8KB/s(115200 链路),协议允许再调高 ---------------- */
#define UI_SEND_HZ 5
/* ---- 发送/接收方 ID -----------------------------------------------------
* sender = 本机机器人 ID(referee.robot_id,无裁判系统时用 0)
* receiver= 选手端/客户端(官方 UI 约定 0x8080);若你的协议版本要求填机器人 ID,
* 改这里一处即可。 */
#define UI_RECEIVER_ID 0x8080
/* ---- 画布 ---------------------------------------------------------------- */
#define UI_SCREEN_W 1920
#define UI_SCREEN_H 1080
/* ---- 版式坐标(与 hud_mock5.png 对应,全部避开官方保留区) ---------------- */
/* 左上:预估底盘功率 */ /* 左上 x56..520 y176..386 */
#define UI_PWR_X0 56
#define UI_PWR_Y0 176
#define UI_PWR_X1 520
#define UI_PWR_Y1 386
/* 左中:超级电容 */ /* x56..520 y404..566 */
#define UI_CAP_X0 56
#define UI_CAP_Y0 404
#define UI_CAP_X1 520
#define UI_CAP_Y1 566
/* 顶部:罗盘刻度带(底盘正方向) */ /* x626..1258 y186..270 */
#define UI_HDG_X0 626
#define UI_HDG_Y0 186
#define UI_HDG_X1 1258
#define UI_HDG_Y1 270
#define UI_HDG_TAPE_HALF 216 /* 刻度带半宽(像素,对应 ±30°) */
/* 中央:俯仰梯 */ /* 以屏幕中心为基准 */
#define UI_LAD_CX 960
#define UI_LAD_CY 540
#define UI_LAD_PX_PER_DEG 11 /* 每度 11 像素 */
#define UI_LAD_HALF_LONG 96
#define UI_LAD_HALF_SHORT 62
/* 右上:摩擦轮速度 */ /* x1420..1864 y176..520 */
#define UI_FRIC_X0 1420
#define UI_FRIC_Y0 176
#define UI_FRIC_X1 1864
#define UI_FRIC_Y1 520
/* 底部:小陀螺状态 */ /* x700..1220 y874..1000 */
#define UI_SPIN_X0 700
#define UI_SPIN_Y0 874
#define UI_SPIN_X1 1220
#define UI_SPIN_Y1 1000
/* ---- 输入数据(referee_task 每周期填) ----------------------------------- */
typedef struct
{
float pitch_deg; /* 云台 pitch(来自 CToC) */
float yaw_deg; /* 云台 yaw(来自 CToC 姿态帧) */
float hdg_deg; /* 底盘正方向朝向(本板 att_deg_yaw) */
float pwr_w; /* 底盘实时功率(裁判 0x0202) */
float pwr_limit_w; /* 功率上限(裁判 0x0201) */
float buf_j; /* 缓冲能量/超级电容(J) */
uint8_t cap_pct; /* 电容百分比(0~100) */
float fric_l_rpm; /* 左摩擦轮转速(来自 CToC) */
float fric_r_rpm; /* 右摩擦轮转速 */
uint8_t spin_on; /* 小陀螺使能 */
uint8_t ref_online; /* 裁判系统在线(离线时整屏删图形) */
} ui_data_t;
/* 周期调用(建议 5Hz):组包并发送;裁判系统离线时发一次"删除全部图层" */
extern void referee_ui_update(const ui_data_t * d);
#endif
底盘/modules/chassis/chassis.c → 底盘/modules/control/chassis.c
+51 -13
View File
@@ -14,6 +14,7 @@
* 功率: chassis_control() 的 power_scale 直接缩放四轮输出电流 * 功率: chassis_control() 的 power_scale 直接缩放四轮输出电流
* ========================================================================== */ * ========================================================================== */
#include "chassis.h" #include "chassis.h"
#include <math.h>
#include "pid.h" #include "pid.h"
#include "filter.h" #include "filter.h"
#include "attitude.h" #include "attitude.h"
@@ -153,32 +154,30 @@ float chassis_follow_sign(void)
return dbg_follow_sign * (float)ctoc_encoder_ccw; return dbg_follow_sign * (float)ctoc_encoder_ccw;
} }
/* 跟随一步:反馈为 Yaw GM6020 编码器相对角(零漂移),输出 w */ /* 跟随一步:反馈为 Yaw GM6020 编码器对齐设定机械零位(4050),输出 w */
float chassis_follow_w(void) float chassis_follow_w(void)
{ {
/* 尚未收到 0x205 反馈时不锁定基准(避免首帧假偏差),输出 0 */ /* 尚未收到 0x205 反馈时输出 0 */
if (motor_yaw_follow.fdb.last_online_tick == 0) if (motor_yaw_follow.fdb.last_online_tick == 0)
{
follow_latched = 0;
return 0; return 0;
}
if (follow_reset_request || !follow_latched) if (follow_reset_request)
{ {
follow_reset_request = 0; follow_reset_request = 0;
follow_target_tick = (float)motor_yaw_follow.fdb.total_tick;
follow_latched = 1;
PID_PositionClean(&follow_pid); PID_PositionClean(&follow_pid);
} }
/* 计算当前机械编码值距离设定零位(4050)的最短差值(-4096 ~ +4096) */
int16_t tick_diff = (int16_t)motor_yaw_follow.fdb.raw_angle - (int16_t)YAW_GM6020_ENCODER_ZERO;
if (tick_diff > 4096)
tick_diff -= 8192;
else if (tick_diff < -4096)
tick_diff += 8192;
/* 相对角误差(°):tick 增向按云台自检结果折算为 CCW 正 */ /* 相对角误差(°):tick 增向按云台自检结果折算为 CCW 正 */
float err_deg = ((float)motor_yaw_follow.fdb.total_tick - follow_target_tick) * 360.0f / 8192.0f; float err_deg = (float)tick_diff * (360.0f / 8192.0f);
if (ctoc_encoder_ccw < 0) if (ctoc_encoder_ccw < 0)
err_deg = -err_deg; err_deg = -err_deg;
if (err_deg > 180.0f)
err_deg -= 360.0f;
else if (err_deg < -180.0f)
err_deg += 360.0f;
if (err_deg > -CHASSIS_FOLLOW_DEADBAND && err_deg < CHASSIS_FOLLOW_DEADBAND) if (err_deg > -CHASSIS_FOLLOW_DEADBAND && err_deg < CHASSIS_FOLLOW_DEADBAND)
err_deg = 0; err_deg = 0;
@@ -195,3 +194,42 @@ void chassis_output_zero(void)
{ {
dji_motor_send_current_low(&hcan1, 0, 0, 0, 0); dji_motor_send_current_low(&hcan1, 0, 0, 0, 0);
} }
#ifndef PI
#define PI 3.14159265358979323846f
#endif
void chassis_vector_to_body(float vx_g, float vy_g, float w, float * vx_c, float * vy_c)
{
/* 电机掉线兜底:若无云台编码器反馈,直接直通输出 */
if (motor_yaw_follow.fdb.last_online_tick == 0)
{
*vx_c = vx_g;
*vy_c = vy_g;
return;
}
/* 1. 计算当前机械编码值距离设定零位的最短差值(-4096 ~ +4096) */
int16_t tick_diff = (int16_t)motor_yaw_follow.fdb.raw_angle - (int16_t)YAW_GM6020_ENCODER_ZERO;
if (tick_diff > 4096)
tick_diff -= 8192;
else if (tick_diff < -4096)
tick_diff += 8192;
/* 2. 相对机械角误差(rad):tick 增向按云台自检结果折算为 CCW 正 */
float alpha_rad = (float)tick_diff * (2.0f * PI / 8192.0f);
int8_t enc_sign = (ctoc_encoder_ccw != 0) ? ctoc_encoder_ccw : 1;
if (enc_sign < 0)
alpha_rad = -alpha_rad;
/* 3. 叠加角速度前馈补偿(-kw 消除通信及响应时延滞后) */
float alpha_comp = alpha_rad - (CHASSIS_SPIN_FEEDFORWARD_K * w);
/* 4. 二维旋转矩阵投影(修正正负号: 适配 vx<0前进, vy>0向左 的坐标系定义) */
float cos_a = cosf(alpha_comp);
float sin_a = sinf(alpha_comp);
*vx_c = vx_g * cos_a + vy_g * sin_a;
*vy_c = -vx_g * sin_a + vy_g * cos_a;
}
底盘/modules/chassis/chassis.h → 底盘/modules/control/chassis.h
+4
View File
@@ -46,4 +46,8 @@ extern volatile float dbg_follow_err_deg;
extern volatile float dbg_follow_out; extern volatile float dbg_follow_out;
extern volatile float dbg_follow_sign; extern volatile float dbg_follow_sign;
/* 将以云台为基准的平移速度(vx_g, vy_g)旋转投影到底盘自身坐标系(vx_c, vy_c)
* 无论底盘是否处于小陀螺自旋状态，使 vx_g 始终代表沿云台枪口方向前进 */
extern void chassis_vector_to_body(float vx_g, float vy_g, float w, float * vx_c, float * vy_c);
#endif #endif
底盘/modules/dev/buzzer.c
+117
View File
@@ -0,0 +1,117 @@
/* =============================================================================
* buzzer.c - C 板无源蜂鸣器(TIM4_CH3, PD14)
* 说明: 音调表与 legacy 完全一致(ARR 固定 10 分频,改 PSC 变调,
* 装载用 HAL_TIM_GenerateEvent 立即生效);buzzer_init 必须在
* CAN_Task 中调用一次启动 PWM 输出,否则所有报警/音效无声
* ========================================================================== */
#include "buzzer.h"
#include "tim.h"
#include "robot_param.h"
extern TIM_HandleTypeDef htim4;
/* 蜂鸣器音调频率表(ARR 固定 10 分频下的 PSC 值,与 legacy 一致) */
static const uint16_t buzzer_tone_freq[37] = {
0,
32107, 30305, 28604, 26999, 25483, 24053, 22703, 21429, 20226, 19091, 18019, 17008,
16053, 15152, 14302, 13499, 12742, 12026, 11352, 10714, 10113, 9545, 9010, 8504,
8027, 7576, 7151, 6750, 6371, 6013, 5676, 5357, 5056, 4773, 4505, 4252,
};
void buzzer_init(void)
{
/* 与 legacy 相同:ARR=10 分频,CubeMX 的 ARR=65535 改为 9 */
__HAL_TIM_SET_AUTORELOAD(&htim4, 10 - 1);
__HAL_TIM_SET_PRESCALER(&htim4, 19091);
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 0);
HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_3);
}
void buzzer_on(buzzer_tone_e tone)
{
if (tone != BUZZER_P)
{
__HAL_TIM_SET_PRESCALER(&htim4, buzzer_tone_freq[tone]);
HAL_TIM_GenerateEvent(&htim4, TIM_EVENTSOURCE_UPDATE); // 立即装载 PSC
/* 占空比 30%:抑制高次尖锐谐波,使吉他拨弦音色圆润纯净 */
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 3);
}
else
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 0);
}
void buzzer_off(void)
{
__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 0);
}
/* ==========================================================================
* 周杰伦《晴天》- 标志性吉他前奏伴奏 Solo (纯享循环版)
* 调式: 1=C,处于 C 板蜂鸣器最佳频响区间(500~1000Hz)
* 结构: 完整还原原曲前奏 4 小节经典木吉他单音分解
* [小节1: Em] 3 2 1 1 2 1 5_ 1 2
* [小节2: C ] 3 2 1 1 2 1 6_ 1 2
* [小节3: G ] 3 2 1 1 2 1 5_ 1 2
* [小节4: D ] 3 2 1 1 2 1 2 3 5 3 2 1 2 -
* 吐音设计: 每个音符保留 BUZZER_P 间隔,模拟吉他真实的清脆拨弦与泛音
* ========================================================================== */
static const buzzer_tone_e buzzer_melody[] = {
/* ---- 小节 1 (Em): 3 2 1 1 2 1 | 5_ 1 2 ---- */
BUZZER_M3, BUZZER_P, // 3 (Mi)
BUZZER_M2, BUZZER_P, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_M1, // 1 (Do)
BUZZER_M2, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_L5, BUZZER_P, // 低音 5 (Sol)
BUZZER_M1, // 1 (Do)
BUZZER_M2, // 2 (Re)
/* ---- 小节 2 (C): 3 2 1 1 2 1 | 6_ 1 2 ---- */
BUZZER_M3, BUZZER_P, // 3 (Mi)
BUZZER_M2, BUZZER_P, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_M1, // 1 (Do)
BUZZER_M2, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_L6, BUZZER_P, // 低音 6 (La)
BUZZER_M1, // 1 (Do)
BUZZER_M2, // 2 (Re)
/* ---- 小节 3 (G): 3 2 1 1 2 1 | 5_ 1 2 ---- */
BUZZER_M3, BUZZER_P, // 3 (Mi)
BUZZER_M2, BUZZER_P, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_M1, // 1 (Do)
BUZZER_M2, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_L5, BUZZER_P, // 低音 5 (Sol)
BUZZER_M1, // 1 (Do)
BUZZER_M2, // 2 (Re)
/* ---- 小节 4 (D): 3 2 1 1 2 1 | 2 3 5 3 2 1 2 - ---- */
BUZZER_M3, BUZZER_P, // 3 (Mi)
BUZZER_M2, BUZZER_P, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_M1, // 1 (Do)
BUZZER_M2, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_M2, BUZZER_P, // 2 (Re)
BUZZER_M3, // 3 (Mi)
BUZZER_M5, // 5 (Sol)
BUZZER_M3, // 3 (Mi)
BUZZER_M2, // 2 (Re)
BUZZER_M1, BUZZER_P, // 1 (Do)
BUZZER_M2, BUZZER_M2, BUZZER_M2, BUZZER_M2, BUZZER_M2, BUZZER_P, // 2 (Re 长音结音)
BUZZER_P, BUZZER_P, BUZZER_P, BUZZER_P, // 尾部休止 400ms 后循环
};
void buzzer_play_melody(uint16_t step)
{
const uint16_t steps =
(uint16_t)(sizeof(buzzer_melody) / sizeof(buzzer_melody[0]));
#ifndef BUZZER_MELODY_SPEED_DIV
#define BUZZER_MELODY_SPEED_DIV 1
#endif
buzzer_on(buzzer_melody[(step / BUZZER_MELODY_SPEED_DIV) % steps]);
}
云台/modules/BUZZER/buzzer.h → 底盘/modules/dev/buzzer.h
+6
View File
@@ -16,4 +16,10 @@ typedef enum
extern void buzzer_init(void); extern void buzzer_init(void);
extern void buzzer_on(buzzer_tone_e tone); extern void buzzer_on(buzzer_tone_e tone);
extern void buzzer_off(void); extern void buzzer_off(void);
/* ---- 调试用旋律 ----
* 按固定 100ms/步 推进的短旋律(音符表见 buzzer.c),用于把掉线警告音换成
* 音乐。step 由调用方每 100ms 递增 1,内部对旋律总步数取模自动循环;
* 旋律中的空拍 BUZZER_P 由 buzzer_on() 解释为静音,用来断音。 */
extern void buzzer_play_melody(uint16_t step);
#endif #endif
底盘/modules/LED/led.c → 底盘/modules/dev/led.c
View File
底盘/modules/LED/led.h → 底盘/modules/dev/led.h
View File
底盘/modules/attitude/attitude.c → 底盘/modules/imu/attitude.c
+4
View File
@@ -34,6 +34,10 @@ void attitude_init(void)
void attitude_update(void) void attitude_update(void)
{ {
/* 先做数据路健康检查:IMU 数据一旦冻结,姿态会安静地"保持不动"甚至
* 匀速漂移(见 bmi088.c 的 bmi088_health_check 说明) */
bmi088_health_check();
ins_update(0.001f); // 1kHz 固定节拍 ins_update(0.001f); // 1kHz 固定节拍
att_deg_yaw = INS.YawTotalAngle; // 多圈连续角,云台 Yaw 环直接使用 att_deg_yaw = INS.YawTotalAngle; // 多圈连续角,云台 Yaw 环直接使用
底盘/modules/attitude/attitude.h → 底盘/modules/imu/attitude.h
View File
云台/modules/ins/ins.c → 底盘/modules/imu/ins.c
+52 -6
View File
@@ -16,11 +16,33 @@
#include "main.h" #include "main.h"
#include <string.h> #include <string.h>
/* z 轴静止零偏标定参数:QEKF 无法观测 z 偏置(见 ins.h),上电采 1s */ /* ---- z 轴静止零偏标定 ----
#define INS_Z_CAL_SAMPLES 1000 * 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 float gyro_z_offset = 0;
static uint8_t z_cal_done = 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; INS_t INS;
static const float xb[3] = {1, 0, 0}; static const float xb[3] = {1, 0, 0};
@@ -31,15 +53,39 @@ void ins_init(void)
{ {
memset(&INS, 0, sizeof(INS)); memset(&INS, 0, sizeof(INS));
/* z 轴静止零偏标定:上电静止采 1s 取平均(x/y 偏置由 EKF 在线估计, /* 等 SPI 采样路稳定,避免把未就绪/残留的样本算进零偏 */
* 无需校准;此期间 ins_update() 不输出,控制任务由 ins_ready() 门控) */ 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++)
{
float sum = 0; float sum = 0;
uint16_t valid = 0;
for (uint16_t i = 0; i < INS_Z_CAL_SAMPLES; i++) for (uint16_t i = 0; i < INS_Z_CAL_SAMPLES; i++)
{ {
sum += bmi088_gyro[2]; 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); HAL_Delay(1);
} }
gyro_z_offset = sum / INS_Z_CAL_SAMPLES;
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;
}
/* 否则认为上电时车体在动,整轮作废、再来一轮 */
}
ins_gyro_z_offset = gyro_z_offset;
z_cal_done = 1; z_cal_done = 1;
INS.Initialized = 1; INS.Initialized = 1;
底盘/modules/ins/ins.h → 底盘/modules/imu/ins.h
+7
View File
@@ -48,6 +48,13 @@ extern void ins_update(float dt);
/* 1-标定完成且已有有效姿态输出(控制任务以此为门控) */ /* 1-标定完成且已有有效姿态输出(控制任务以此为门控) */
extern uint8_t ins_ready(void); 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 BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q);
extern void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q); extern void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q);
底盘/modules/ins/QuaternionEKF.c → 底盘/modules/imu/qekf/QuaternionEKF.c
View File
底盘/modules/ins/QuaternionEKF.h → 底盘/modules/imu/qekf/QuaternionEKF.h
View File
底盘/modules/ins/kalman_filter.c → 底盘/modules/imu/qekf/kalman_filter.c
View File
底盘/modules/ins/kalman_filter.h → 底盘/modules/imu/qekf/kalman_filter.h
View File
底盘/modules/motor/dji_motor.c
+31 -9
View File
@@ -7,8 +7,7 @@
* 记录 last_online_tick 供掉线检测 * 记录 last_online_tick 供掉线检测
* 控制路径(任务上下文): * 控制路径(任务上下文):
* dji_motor_send_current_low/high() 0x200 / 0x1FF 电流帧(+/-16384 = 20A/10A) * dji_motor_send_current_low/high() 0x200 / 0x1FF 电流帧(+/-16384 = 20A/10A)
* dji_motor_send_voltage_high() 0x2FF 电压帧(GM6020 ID5~7) * dji_motor_send_gm6020_current_low/high() 0x1FE / 0x2FE GM6020 电流帧(+/-16384 = 3A)
* 注意 0x1FF 帧为 M3508/M2006 高位电流与 GM6020 低位电压共用,按槽位区分
* ========================================================================== */ * ========================================================================== */
#include "dji_motor.h" #include "dji_motor.h"
@@ -20,7 +19,8 @@
static dji_motor_t * dji_motor_list[DJI_MOTOR_MAX]; static dji_motor_t * dji_motor_list[DJI_MOTOR_MAX];
static uint8_t dji_motor_count = 0; static uint8_t dji_motor_count = 0;
static void dji_motor_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc); static void dji_motor_can1_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc);
static void dji_motor_can2_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc);
/* 解析一帧反馈,多圈累计(无 ±1 圈限制,底盘小陀螺时 Yaw 电机可连续旋转) */ /* 解析一帧反馈,多圈累计(无 ±1 圈限制,底盘小陀螺时 Yaw 电机可连续旋转) */
static void dji_motor_parse(dji_motor_t * motor, const uint8_t data[8]) static void dji_motor_parse(dji_motor_t * motor, const uint8_t data[8])
@@ -71,16 +71,32 @@ void dji_motor_init(dji_motor_t * motor, hcan_t * hcan, uint16_t rx_id, dji_moto
if (dji_motor_count < DJI_MOTOR_MAX) if (dji_motor_count < DJI_MOTOR_MAX)
{ {
dji_motor_list[dji_motor_count++] = motor; dji_motor_list[dji_motor_count++] = motor;
can_rx_register(hcan, rx_id, dji_motor_rx_callback); if (hcan == &hcan1)
can_rx_register(hcan, rx_id, dji_motor_can1_rx_callback);
else
can_rx_register(hcan, rx_id, dji_motor_can2_rx_callback);
} }
} }
static void dji_motor_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc) static void dji_motor_can1_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
{ {
(void)dlc; (void)dlc;
for (uint8_t i = 0; i < dji_motor_count; i++) for (uint8_t i = 0; i < dji_motor_count; i++)
{ {
if (dji_motor_list[i]->rx_id == std_id) if (dji_motor_list[i]->hcan == &hcan1 && dji_motor_list[i]->rx_id == std_id)
{
dji_motor_parse(dji_motor_list[i], rx_data);
return;
}
}
}
static void dji_motor_can2_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
{
(void)dlc;
for (uint8_t i = 0; i < dji_motor_count; i++)
{
if (dji_motor_list[i]->hcan == &hcan2 && dji_motor_list[i]->rx_id == std_id)
{ {
dji_motor_parse(dji_motor_list[i], rx_data); dji_motor_parse(dji_motor_list[i], rx_data);
return; return;
@@ -119,8 +135,14 @@ void dji_motor_send_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t
dji_motor_send_int16x4(hcan, 0x1FF, v); dji_motor_send_int16x4(hcan, 0x1FF, v);
} }
void dji_motor_send_voltage_high(hcan_t * hcan, int16_t v5, int16_t v6, int16_t v7) void dji_motor_send_gm6020_current_low(hcan_t * hcan, int16_t i1, int16_t i2, int16_t i3, int16_t i4)
{ {
const int16_t v[4] = {v5, v6, v7, 0}; const int16_t v[4] = {i1, i2, i3, i4};
dji_motor_send_int16x4(hcan, 0x2FF, v); dji_motor_send_int16x4(hcan, 0x1FE, v);
}
void dji_motor_send_gm6020_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t i7, int16_t i8)
{
const int16_t v[4] = {i5, i6, i7, i8};
dji_motor_send_int16x4(hcan, 0x2FE, v);
} }
底盘/modules/motor/dji_motor.h
+7 -6
View File
@@ -5,13 +5,13 @@
/* DJI 电机(M3508/M2006/GM6020)统一驱动 /* DJI 电机(M3508/M2006/GM6020)统一驱动
* - 反馈:can_start() 后通过 can_rx_register 按 rx_id 注册,接收中断内解析(多圈累计) * - 反馈:can_start() 后通过 can_rx_register 按 rx_id 注册,接收中断内解析(多圈累计)
* - 控制:组装 0x200/0x1FF/0x2FF 控制帧,经 CAN_SendTxMessage 发送 */ * - 控制:组装 0x200/0x1FF/0x1FE/0x2FE 控制帧,经 CAN_SendTxMessage 发送 */
typedef enum typedef enum
{ {
DJI_MOTOR_3508, // 减速比 3591:187(≈19:1), 转矩常数 0.3N·m/A DJI_MOTOR_3508, // 减速比 3591:187(≈19:1), 转矩常数 0.3N·m/A
DJI_MOTOR_2006, // 减速比 36:1, 转矩常数 0.18N·m/A DJI_MOTOR_2006, // 减速比 36:1, 转矩常数 0.18N·m/A
DJI_MOTOR_6020, // 直驱, 电压控制 DJI_MOTOR_6020, // 直驱, 电流控制(ID1~4 为 0x1FE 帧, ±16384 对应 ±3A)
} dji_motor_type_e; } dji_motor_type_e;
typedef struct typedef struct
@@ -20,7 +20,7 @@ typedef struct
int64_t total_tick; // 累计角度位置原始数据(8192*圈数+raw,无 ±1 圈限制) int64_t total_tick; // 累计角度位置原始数据(8192*圈数+raw,无 ±1 圈限制)
float rotor_angle_deg; // 累计转子机械角(°) float rotor_angle_deg; // 累计转子机械角(°)
int16_t rotor_speed; // 转子转速(RPM) int16_t rotor_speed; // 转子转速(RPM)
int16_t raw_torque; // 转矩电流/转矩电压原始数据 int16_t raw_torque; // 转矩电流原始数据
float torque_current; // 转矩电流(A) float torque_current; // 转矩电流(A)
uint8_t temperature; // 电机温度(℃) uint8_t temperature; // 电机温度(℃)
uint32_t last_online_tick; // 最近一次收到反馈的 tick(HAL_GetTick) uint32_t last_online_tick; // 最近一次收到反馈的 tick(HAL_GetTick)
@@ -39,11 +39,12 @@ extern void dji_motor_init(dji_motor_t * motor, hcan_t * hcan, uint16_t rx_id, d
/* ---- 控制帧发送 ---- /* ---- 控制帧发送 ----
* M3508/M2006 ID1~4:0x200 电流帧(±16384 对应 ±20A/±10A) * M3508/M2006 ID1~4:0x200 电流帧(±16384 对应 ±20A/±10A)
* M3508/M2006 ID5~8:0x1FF 电流帧;GM6020 ID1~4:0x1FF 电压帧(±25000) * M3508/M2006 ID5~8:0x1FF 电流帧(±16384)
* GM6020 ID5~7:0x2FF 电压帧 */ * GM6020 电流控制:ID1~4 为 0x1FE 电流帧(±16384 对应 ±3A),ID5~8 为 0x2FE 电流帧(±16384) */
extern void dji_motor_send_current_low(hcan_t * hcan, int16_t i1, int16_t i2, int16_t i3, int16_t i4); extern void dji_motor_send_current_low(hcan_t * hcan, int16_t i1, int16_t i2, int16_t i3, int16_t i4);
extern void dji_motor_send_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t i7, int16_t i8); extern void dji_motor_send_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t i7, int16_t i8);
extern void dji_motor_send_voltage_high(hcan_t * hcan, int16_t v5, int16_t v6, int16_t v7); extern void dji_motor_send_gm6020_current_low(hcan_t * hcan, int16_t i1, int16_t i2, int16_t i3, int16_t i4);
extern void dji_motor_send_gm6020_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t i7, int16_t i8);
/* 通用 int16x4 发送(板间通讯等复用) */ /* 通用 int16x4 发送(板间通讯等复用) */
extern void dji_motor_send_int16x4(hcan_t * hcan, uint32_t std_id, const int16_t val[4]); extern void dji_motor_send_int16x4(hcan_t * hcan, uint32_t std_id, const int16_t val[4]);
底盘/modules/BMI088/bmi088.c → 底盘/modules/sensor/bmi088.c
+168 -1
View File
@@ -49,6 +49,8 @@ float bmi088_temperature;
uint8_t bmi088_online = 0; uint8_t bmi088_online = 0;
uint8_t bmi088_last_acc_id = 0; uint8_t bmi088_last_acc_id = 0;
uint8_t bmi088_last_gyro_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_tx_buf[8];
static uint8_t spi_rx_buf[8]; static uint8_t spi_rx_buf[8];
@@ -63,6 +65,17 @@ typedef enum
} bmi088_phase_e; } bmi088_phase_e;
static volatile bmi088_phase_e read_phase = BMI088_PHASE_NONE; 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; 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[0] = raw[0] * 0.0008974358974f; // (raw/32768*3)*9.8
bmi088_accel[1] = raw[1] * 0.0008974358974f; bmi088_accel[1] = raw[1] * 0.0008974358974f;
bmi088_accel[2] = raw[2] * 0.0008974358974f; bmi088_accel[2] = raw[2] * 0.0008974358974f;
bmi088_accel_cnt++;
} }
static void process_temperature(void) 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[0] = raw[0] * 0.0010652644360316952f; // (raw/32768*2000)/180*π
bmi088_gyro[1] = raw[1] * 0.0010652644360316952f; bmi088_gyro[1] = raw[1] * 0.0010652644360316952f;
bmi088_gyro[2] = raw[2] * 0.0010652644360316952f; bmi088_gyro[2] = raw[2] * 0.0010652644360316952f;
bmi088_gyro_cnt++;
} }
/* 启动一次 SPI DMA 读(若 SPI 忙则丢弃本次,等下一个数据就绪沿) */ /* 启动一次 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) if (HAL_SPI_GetState(&hspi1) != HAL_SPI_STATE_READY)
return; return;
read_phase = phase; read_phase = phase;
read_phase_tick = HAL_GetTick();
spi_tx_buf[0] = (uint8_t)(reg | 0x80); spi_tx_buf[0] = (uint8_t)(reg | 0x80);
for (uint8_t i = 1; i < len; i++) for (uint8_t i = 1; i < len; i++)
spi_tx_buf[i] = 0x00; 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 else
BMI088_CS_GYRO(0); 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) if (!bmi088_online)
return; // 初始化完成前不启动 DMA 读,避免占用 SPI 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 (GPIO_Pin == INT1_ACCEL_Pin)
{ {
if (read_phase == BMI088_PHASE_NONE) if (read_phase == BMI088_PHASE_NONE)
@@ -303,3 +337,136 @@ void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef * hspi)
break; 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++;
}
底盘/modules/BMI088/bmi088.h → 底盘/modules/sensor/bmi088.h
+16
View File
@@ -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_acc_id; // 最近一次读到的加速度计 ID(诊断用,正常 0x1E)
extern uint8_t bmi088_last_gyro_id; // 最近一次读到的陀螺仪 ID(诊断用,正常 0x0F) 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 检测循环;失败时绿灯报警并重试) */ /* 初始化(任务上下文调用,含阻塞式寄存器配置与 ID 检测循环;失败时绿灯报警并重试) */
extern void bmi088_init(void); extern void bmi088_init(void);

Some files were not shown because too many files have changed in this diff Show More