项目架构：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_ref/pitch_ref 直接设定 │
│  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_ref, pitch_ref)
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_ref/pitch_ref
  ├── 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_ref/pitch_ref → 直接设定
  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，防止切回使能时突变

---

十二、核心数据类型

// 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_ref, pitch_ref;         // 手动模式: 角度参考
    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%)