# 项目架构：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)         │
│                                                      │
│  上位机 0xA5 帧 → error_x/y → KF → vision PID      │
│                 → 角度增量 → 累计 angle ref          │
│  上位机 0xA6 帧 → yaw_ref/pitch_ref 直接设定        │
│                                                      │
│  ProcessCommand() 处理模式切换 / 激光控制            │
└──────────────────────┬───────────────────────────────┘
                       │ gimbal_cmd (pub-sub)
                       ▼
┌──────────────────────────────────────────────────────┐
│  application/gimbal/gimbal.c  (传递, 500Hz)         │
│                                                      │
│  DMMotorSetRef(yaw, cmd.yaw)                        │
│  DMMotorSetRef(pitch, cmd.pitch)                    │
│  反馈: gimbal_feed → gimbal_IMU_data 指针           │
└──────────────────────┬───────────────────────────────┘
                       │
                       ▼
┌──────────────────────────────────────────────────────┐
│  modules/motor/DMmotor/  (内环, 1kHz)                │
│                                                      │
│  IMU 角度反馈 → angle PID → velocity output         │
│  → DMMotorSendVelocity() → CAN 速度模式             │
│  output_reverse 控制输出方向                         │
└──────────────────────────────────────────────────────┘
```

---

## 五、上位机串口协议

位于 `modules/master_machine/`。三种帧类型，通过帧头区分。

### 自动/视觉追踪数据帧 `0xA5` (10 字节)

| 字节 | 0 | 1-4 | 5-8 | 9 |
|------|---|-----|-----|---|
| 内容 | 0xA5 | error_x (float) | error_y (float) | checksum |

### 手动控制数据帧 `0xA6` (11 字节)

| 字节 | 0 | 1 | 2-5 | 6-9 | 10 |
|------|---|-----|-----|-----|----|
| 内容 | 0xA6 | zero_force | yaw_ref (float) | pitch_ref (float) | checksum |

### 命令帧 `0xA7` (3 字节)

| 字节 | 0 | 1 | 2 |
|------|---|-----|----|
| 内容 | 0xA7 | cmd | checksum |

命令码：`0x01` 切换到自动模式, `0x02` 切换到手动模式, `0x03` 激光开, `0x04` 激光关

### 数据流

```
上位机 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

robot_cmd 每轮:
  ProcessCommand() → 模式切换 / 激光
  AUTO:  vision_data.error_x/y → KF → vision PID → 角度增量
  MANUAL: vision_data.yaw_ref/pitch_ref → 直接设定
  PubPushMessage(gimbal_cmd) → gimbal 执行
```

---

## 六、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()` | 发送命令 (使能/停止/清零/清除错误) |
| `DMMotorSendVelocity()` | 速度模式 CAN 发送 |
| `DMMotorSendMIT()` | MIT 模式 CAN 发送 |
| `DMMotorSendPosVel()` | 位置-速度模式 CAN 发送 |

---

## 七、模块层

| 分类 | 模块 | 功能 |
|------|------|------|
| **电机** | `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 协议, 接收+解析) |
|  | `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
                           ▼
              ┌────────────────────────┐
              │      RobotCMD          │
              │   ProcessCommand()     │ ← 模式切换/激光
              │   VisionTrackingSet()  │ ← KF → vision PID → 角度增量
              │   ManualModeSet()      │ ← 直接角度参考
              │   PubPushMessage()     │
              └───────────┬────────────┘
                          │ gimbal_cmd
                          ▼
              ┌────────────────────────┐
              │      Gimbal            │
              │   DMMotorSetRef()      │
              │   反馈: 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 输出方向统一通过单标志位控制

---

## 十一、项目规模

| 类别 | 数量 |
|------|------|
| C 源文件 | 96 |
| ASM 源文件 | 2 |
| 应用层模块 | 3 (robot, robot_cmd, gimbal) |
| 模块层子模块 | 10 (motor, imu, algorithm, master_machine, message_center, daemon, buzzer, vofa, board_comm) |
| BSP 驱动模块 | 8 |
| FreeRTOS 任务 | 4 |
| 最高控制频率 | 1kHz |
| CPU 频率 | 480MHz |
| FLASH 使用 | ~133 KB / 1 MB (13%) |
| DTCMRAM 使用 | ~59 KB / 128 KB (46%) |

---

## 十二、核心数据类型

```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_ref, pitch_ref;     // 手动模式: 角度参考
    uint8_t manual_updated;
    uint8_t cmd, cmd_pending;     // 命令
    float frequency;
} VisionData_t;
```