SkyLiCoder
da6acdcc89
Manual mode now receives angular velocity (degrees/s) via 0xA6 frame instead of absolute angle reference. The firmware integrates velocity * dt into gimbal angle ref. Motors are no longer auto-enabled at DMMotorInit — they stay disabled until a mode switch out of DEBUG. Extract ROBOT_CMD_DELAY macro to replace magic number. Fix "volity" typo → "velocity" and update docs/protocol comments accordingly.
356 lines
15 KiB
Markdown
356 lines
15 KiB
Markdown
# 项目架构：Basic_Framework_MC02 (DM 云台控制系统)
|
||
|
||
## 概览
|
||
|
||
基于 **STM32H723** (Cortex-M7, 480MHz) 的达妙电机云台控制系统。采用 FreeRTOS，通过 IMU 反馈实现角度闭环，通过串口接收上位机指令实现视觉追踪和调参。项目源自 HNU YueLu EC RoboMaster 项目，经重构精简为纯云台控制。
|
||
|
||
---
|
||
|
||
## 一、硬件与通信
|
||
|
||
| 资源 | 用途 |
|
||
|------|------|
|
||
| **FDCAN1** | 电机控制总线 (DM-G6220 yaw, DM-J4310-2EC pitch) |
|
||
| **USART10** | 上位机通信 (master_machine 协议) |
|
||
| **USART7** | VoFA 调试输出 |
|
||
| **USART1** | 板间通信 (BoardComm) |
|
||
| **SPI2** | BMI088 IMU 传感器 |
|
||
|
||
---
|
||
|
||
## 二、分层架构
|
||
|
||
```
|
||
┌──────────────────────────────────────────────┐
|
||
│ application/ 应用层 │
|
||
│ robot.c → robot_cmd → gimbal │
|
||
├──────────────────────────────────────────────┤
|
||
│ modules/ 模块层 │
|
||
│ motor, imu, master_machine, algorithm, ... │
|
||
├──────────────────────────────────────────────┤
|
||
│ bsp/ 板级支持包 │
|
||
│ can, usart, spi, dwt, log, gpio │
|
||
├──────────────────────────────────────────────┤
|
||
│ Drivers/ HAL 驱动层 (CubeMX 生成) │
|
||
│ STM32H7xx_HAL, CMSIS, FreeRTOS │
|
||
└──────────────────────────────────────────────┘
|
||
```
|
||
|
||
---
|
||
|
||
## 三、启动流程
|
||
|
||
```
|
||
main()
|
||
├── HAL_Init() → SystemClock_Config() → MX_*_Init() // CubeMX 外设初始化
|
||
└── RobotInit()
|
||
├── BSPInit() // DWT + Log
|
||
├── RobotCMDInit() // pub-sub, 视觉PID, 串口初始化
|
||
├── GimbalInit() // IMU + DM 电机初始化
|
||
└── OSTaskInit() // 创建 4 个 RTOS 任务
|
||
```
|
||
|
||
### FreeRTOS 任务
|
||
|
||
| 任务 | 频率 | 优先级 | 功能 |
|
||
|------|------|--------|------|
|
||
| **INS Task** | 1kHz | AboveNormal | BMI088 读取 → 四元数 EKF 姿态解算 |
|
||
| **Motor Task** | 1kHz | Normal | DM 电机角度 PID → CAN 速度指令发送 |
|
||
| **Robot Task** | 500Hz | Normal | 上位机命令处理 + 模式切换 + 控制执行 |
|
||
| **Daemon Task** | 100Hz | Normal | 模块看门狗 + 蜂鸣器 |
|
||
|
||
---
|
||
|
||
## 四、PID 双环架构
|
||
|
||
```
|
||
┌──────────────────────────────────────────────────────┐
|
||
│ application/cmd/robot_cmd.c (外环, 500Hz) │
|
||
│ │
|
||
│ AUTO: 上位机 0xA5 帧 → error_x/y → KF → vision PID│
|
||
│ → 角度增量 → 累计 angle ref │
|
||
│ MANUAL: 上位机 0xA6 帧 → yaw/pitch 角速度 → 角度增量积分 │
|
||
│ DEBUG: 上位机 0xA8 帧 → 实时更新 PID 参数 │
|
||
│ │
|
||
│ ProcessCommand() 处理模式切换 / 激光控制 │
|
||
└──────────────────────┬───────────────────────────────┘
|
||
│ gimbal_cmd (pub-sub)
|
||
▼
|
||
┌──────────────────────────────────────────────────────┐
|
||
│ application/gimbal/gimbal.c (传递, 500Hz) │
|
||
│ │
|
||
│ DMMotorSetRef(yaw, cmd.yaw) │
|
||
│ DMMotorSetRef(pitch, cmd.pitch) │
|
||
│ 电机使能/失能订阅: motor_enable │
|
||
│ 电机角 PID 调参订阅: motor_pid │
|
||
│ 反馈: gimbal_feed → gimbal_IMU_data 指针 │
|
||
└──────────────────────┬───────────────────────────────┘
|
||
│
|
||
▼
|
||
┌──────────────────────────────────────────────────────┐
|
||
│ modules/motor/DMmotor/ (内环, 1kHz) │
|
||
│ │
|
||
│ IMU 角度反馈 → angle PID → velocity output │
|
||
│ → DMMotorSendVelocity() → CAN 速度模式 │
|
||
│ DISABLE 时自动清零 PID + 发送零速指令 │
|
||
│ output_reverse 控制输出方向 │
|
||
└──────────────────────────────────────────────────────┘
|
||
```
|
||
|
||
---
|
||
|
||
## 五、上位机串口协议
|
||
|
||
位于 `modules/master_machine/`。四种帧类型，通过帧头区分。详细说明见 `docs/host_comm_guide.md`。
|
||
|
||
### 帧类型总览
|
||
|
||
| 帧头 | 长度 | 用途 |
|
||
|------|------|------|
|
||
| 0xA5 | 10 | 自动/视觉追踪数据帧 (error_x, error_y) |
|
||
| 0xA6 | 11 | 手动控制数据帧 (zero_force, yaw_velocity, pitch_velocity) |
|
||
| 0xA7 | 3 | 命令帧 (模式切换, 激光) |
|
||
| 0xA8 | 50 | 调试调参帧 (12 个 float PID 参数) |
|
||
|
||
### 命令码
|
||
|
||
| cmd | 含义 |
|
||
|-----|------|
|
||
| 0x01 | 切换到自动/视觉追踪模式 |
|
||
| 0x02 | 切换到手动模式 |
|
||
| 0x03 | 激光开 |
|
||
| 0x04 | 激光关 |
|
||
| 0x05 | 切换到调试模式 |
|
||
|
||
### 工作模式
|
||
|
||
| 模式 | 值 | 说明 |
|
||
|------|-----|------|
|
||
| AUTO | 0 | 视觉误差 → KF → vision PID → 角度增量 |
|
||
| MANUAL | 1 | 上位机下发角速度，固件积分累加角度参考 |
|
||
| DEBUG | 2 | 失能电机，接收 PID 参数实时调参 |
|
||
|
||
### 数据流
|
||
|
||
```
|
||
上位机 USART10 → DecodeVision() → vision_data (全局结构体)
|
||
├── 0xA5 → auto_updated=1, error_x/y
|
||
├── 0xA6 → manual_updated=1, zero_force, yaw/pitch velocity
|
||
├── 0xA7 → cmd_pending=1, cmd
|
||
└── 0xA8 → debug_updated=1, 12 个 PID 参数
|
||
|
||
robot_cmd 每轮:
|
||
ProcessCommand() → 模式切换 / 激光
|
||
AUTO: vision_data.error_x/y → KF → vision PID → 角度增量
|
||
MANUAL: vision_data.yaw/pitch velocity → 角度增量积分
|
||
DEBUG: vision_data PID 参数 → 直接写入 vision PID, 发布 motor_pid 话题
|
||
PubPushMessage(gimbal_cmd) → gimbal 执行
|
||
```
|
||
|
||
---
|
||
|
||
## 六、Pub-Sub 话题
|
||
|
||
| 话题名 | 发布者 | 订阅者 | 数据类型 |
|
||
|--------|--------|--------|----------|
|
||
| `gimbal_cmd` | robot_cmd | gimbal | `Gimbal_Ctrl_Cmd_s` (yaw, pitch) |
|
||
| `gimbal_feed` | gimbal | robot_cmd | `Gimbal_Upload_Data_s` (INS 指针) |
|
||
| `motor_enable` | robot_cmd | gimbal | `Motor_Enable_s` (enable: 0/1) |
|
||
| `motor_pid` | robot_cmd | gimbal | `Motor_PID_Tune_s` (yaw/pitch Kp,Ki,Kd) |
|
||
|
||
---
|
||
|
||
## 七、DM 电机驱动
|
||
|
||
位于 `modules/motor/DMmotor/`。基于 DM_6220 参考协议。
|
||
|
||
### 速度模式 (当前默认)
|
||
|
||
CAN ID = motor_id + 0x200，发送 float velocity (4 bytes)。
|
||
|
||
```
|
||
DMMotorControl() @ 1kHz:
|
||
PIDCalculate(angle_PID, IMU_angle, pid_ref) → velocity
|
||
output_reverse? → DMMotorSendVelocity() → CAN
|
||
```
|
||
|
||
### 电机配置
|
||
|
||
| 电机 | CAN ID | 反馈 ID | 模式 | MIT KP/KD |
|
||
|------|--------|---------|------|-----------|
|
||
| DM-G6220 (Yaw) | 0x01 | 0xF1 | Velocity | — |
|
||
| DM-J4310-2EC (Pitch) | 0x02 | 0xF2 | Velocity | — |
|
||
|
||
### API
|
||
|
||
| 函数 | 说明 |
|
||
|------|------|
|
||
| `DMMotorInit()` | 初始化电机实例 (CAN 注册, PID 初始化, daemon) |
|
||
| `DMMotorSetRef()` | 设定角度参考值 (degrees) |
|
||
| `DMMotorControl()` | 1kHz 控制循环 (所有电机) |
|
||
| `DMMotorSetMode()` | 发送命令 (使能/停止/清零/清除错误), DISABLE 时自动清零 PID+零速 |
|
||
| `DMMotorSendVelocity()` | 速度模式 CAN 发送 |
|
||
| `DMMotorSendMIT()` | MIT 模式 CAN 发送 |
|
||
| `DMMotorSendPosVel()` | 位置-速度模式 CAN 发送 |
|
||
|
||
### DISABLE 保护
|
||
|
||
调用 `DMMotorSetMode(DM_CMD_DISABLE_MODE, motor)` 时：
|
||
1. 先发送零速指令 (`DMMotorSendVelocity(can, 0.0f)`) 让电机停止转动
|
||
2. 清理 angle_PID 全部状态 (积分/微分/误差)
|
||
3. 清零 pid_ref
|
||
4. 发送失能 CAN 命令 (0xFD)
|
||
|
||
防止切回 ENABLE 时因 PID 残余积分项或位置跳变导致的突变。
|
||
|
||
---
|
||
|
||
## 八、模块层
|
||
|
||
| 分类 | 模块 | 功能 |
|
||
|------|------|------|
|
||
| **电机** | `motor/DMmotor` | 达妙电机驱动 (MIT/PosVel/Velocity 三模式) |
|
||
| | `motor/motor_task` | 电机控制统一调度 (`DMMotorControl`) |
|
||
| **姿态** | `imu/ins_task` | 惯导解算 (四元数 EKF, 返回 INS_t 指针) |
|
||
| | `imu/BMI088driver` | BMI088 SPI 读取 |
|
||
| | `imu/BMI088Middleware` | BMI088 数据换算 |
|
||
| **算法** | `algorithm/controller` | PID 控制器 (梯形积分/变速积分/微分先行/输出滤波) |
|
||
| | `algorithm/kalman_filter` | 卡尔曼滤波器 (自适应) |
|
||
| | `algorithm/QuaternionEKF` | 四元数 EKF (陀螺零偏估计/卡方检验) |
|
||
| | `algorithm/crc8/crc16` | CRC 校验 |
|
||
| | `algorithm/user_lib` | 数学/工具函数 |
|
||
| **通信** | `master_machine` | 上位机通信 (0xA5/0xA6/0xA7/0xA8 协议, 接收+解析) |
|
||
| | `timachine/board_comm` | 板间通信 |
|
||
| | `message_center` | Pub-Sub 消息中间件 (链表, FIFO) |
|
||
| **系统** | `daemon` | 看门狗 (重载计数+离线回调) |
|
||
| | `alarm/buzzer` | 蜂鸣器 |
|
||
| | `vofa` | VoFA+ 调试输出 |
|
||
|
||
---
|
||
|
||
## 九、BSP 驱动层
|
||
|
||
实例化注册模式 (不注册不初始化)：
|
||
|
||
| 模块 | 功能 |
|
||
|------|------|
|
||
| `bsp_dwt` | DWT 周期计数器 (μs/ms 延时, 时间戳) |
|
||
| `bsp_can` | FDCAN 收发 (FIFO+中断) |
|
||
| `bsp_usart` | 串口收发 (DMA+环形缓冲区) |
|
||
| `bsp_spi` | SPI 通信 |
|
||
| `bsp_log` | SEGGER RTT + 串口日志 |
|
||
| `bsp_pwm` | PWM 输出 |
|
||
|
||
---
|
||
|
||
## 十、数据流
|
||
|
||
```
|
||
┌──────────────┐
|
||
│ 上位机 │
|
||
│ (USART10) │
|
||
└──────┬───────┘
|
||
│ 0xA5 / 0xA6 / 0xA7 / 0xA8
|
||
▼
|
||
┌────────────────────────┐
|
||
│ RobotCMD │
|
||
│ ProcessCommand() │ ← 模式切换/激光
|
||
│ VisionTracking() │ ← KF → vision PID → 角度增量
|
||
│ ManualMode() │ ← 角速度积分
|
||
│ DebugMode() │ ← 实时 PID 调参
|
||
│ PubPushMessage() │
|
||
└───────┬───────┬────────┘
|
||
│ gimbal_cmd │ motor_pid / motor_enable
|
||
▼ ▼
|
||
┌────────────────────────────┐
|
||
│ Gimbal │
|
||
│ DMMotorSetRef() │
|
||
│ 电机使能/失能控制 │
|
||
│ 电机角 PID 在线更新 │
|
||
│ 反馈: gimbal_feed │ ← INS_t* 指针
|
||
└───────────┬────────────────┘
|
||
│ angle ref
|
||
▼
|
||
┌────────────────────────┐
|
||
│ DMMotor │ (1kHz Motor Task)
|
||
│ IMU angle → PID │
|
||
│ → Velocity mode CAN │
|
||
└────────────────────────┘
|
||
│
|
||
┌───────────┴────────────┐
|
||
│ DM-G6220 DM-J4310 │
|
||
│ (Yaw) (Pitch) │
|
||
└────────────────────────┘
|
||
```
|
||
|
||
---
|
||
|
||
## 十一、关键设计
|
||
|
||
1. **IMU 反馈替代编码器** — 云台 Yaw/Pitch 使用 INS_t 中的 YawTotalAngle/Pitch 和 Gyro[]
|
||
2. **速度模式控制** — MCU 跑角度 PID，电机内部处理速度闭环
|
||
3. **伪 Pub-Sub** — 应用间通过 message_center 解耦
|
||
4. **Daemon 看门狗** — 每个外设模块独立 DaemonInstance，离线自动回调
|
||
5. **BSP 实例化注册** — CAN/串口不注册不初始化
|
||
6. **KF + PID 级联** — 视觉误差经 KF 平滑 → vision PID → 角度增量 → 内环角度 PID
|
||
7. **模式切换重置** — 切换到自动模式时重置 KF 状态 (x=0, P=diag(5,5)) 和 PID 积分项
|
||
8. **串口命令控制** — 模式切换、激光控制均通过 0xA7 命令帧触发
|
||
9. **output_reverse** — PID 输出方向统一通过单标志位控制
|
||
10. **调试模式** — 上位机可实时调整视觉环和电机角度环 PID 参数，无需重新编译
|
||
11. **DISABLE 保护** — 电机失能时自动发送零速指令并清理 PID，防止切回使能时突变
|
||
|
||
---
|
||
|
||
## 十二、核心数据类型
|
||
|
||
```c
|
||
// INS 惯导实例 (ins_task.h)
|
||
typedef struct {
|
||
float q[4]; // 四元数
|
||
float Gyro[3]; // 角速度 (rad/s)
|
||
float Accel[3]; // 加速度 (m/s²)
|
||
float Roll, Pitch, Yaw;
|
||
float YawTotalAngle; // 多圈 yaw (degrees)
|
||
} INS_t;
|
||
|
||
// 云台控制指令 (robot_def.h)
|
||
typedef struct {
|
||
float yaw; // yaw 目标角度 (degrees)
|
||
float pitch; // pitch 目标角度 (degrees)
|
||
} Gimbal_Ctrl_Cmd_s;
|
||
|
||
// 上位机数据 (master_machine.h)
|
||
typedef struct {
|
||
float error_x, error_y; // 自动模式: 归一化误差
|
||
uint8_t auto_updated;
|
||
uint8_t zero_force; // 手动模式: 零力标志
|
||
float yaw_degree_velocity, pitch_degree_velocity; // 手动模式: 角速度
|
||
uint8_t manual_updated;
|
||
// 调试模式数据
|
||
float vision_yaw_Kp, vision_yaw_Ki, vision_yaw_Kd;
|
||
float vision_pitch_Kp, vision_pitch_Ki, vision_pitch_Kd;
|
||
float motor_yaw_Kp, motor_yaw_Ki, motor_yaw_Kd;
|
||
float motor_pitch_Kp, motor_pitch_Ki, motor_pitch_Kd;
|
||
uint8_t debug_updated;
|
||
uint8_t cmd, cmd_pending; // 命令
|
||
float frequency;
|
||
} VisionData_t;
|
||
```
|
||
|
||
---
|
||
|
||
## 十三、项目规模
|
||
|
||
| 类别 | 数量 |
|
||
|------|------|
|
||
| C 源文件 | ~96 |
|
||
| 应用层模块 | 3 (robot, robot_cmd, gimbal) |
|
||
| 模块层子模块 | 12 |
|
||
| BSP 驱动模块 | 8 |
|
||
| FreeRTOS 任务 | 4 |
|
||
| Pub-Sub 话题 | 4 |
|
||
| 最高控制频率 | 1kHz |
|
||
| CPU 频率 | 480MHz |
|
||
| FLASH 使用 | ~132 KB / 1 MB (13%) |
|
||
| DTCMRAM 使用 | ~44 KB / 128 KB (34%) |
|