# 草坪割草机闭环控制系统

## 系统架构

```
┌─────────────────────────────────────────────────────────────┐
│                        STM32H7 MCU                           │
│                                                              │
│  GPS/IMU (UART2)  →  解析  →  PythonLink  →  UART5         │
│       10Hz/100Hz         ↓                     ↓ 921600bps  │
│                      数据打包              发送GPS/IMU       │
│                                                              │
│  UART5接收  ←  PythonLink  ←  控制命令解析                   │
│      ↓                                                       │
│  SBUS模块 (CH8检测)                                          │
│      ↓                                                       │
│  CH8 > 1500? ─YES→ 自动模式 → 应用Python命令 → PWM输出      │
│      ↓ NO                                                    │
│  手动模式 → 应用SBUS遥控器 → PWM输出                         │
└─────────────────────────────────────────────────────────────┘
                              ↕ UART5 (921600bps)
┌─────────────────────────────────────────────────────────────┐
│                      Python 控制程序                          │
│                                                              │
│  接收GPS/IMU  →  坐标转换  →  MowerController               │
│                                  ↓                           │
│                          计算控制信号                         │
│                          (前进, 转向)                         │
│                                  ↓                           │
│                      转换为PWM值 (1000-2000us)               │
│                                  ↓                           │
│                          发送控制命令                         │
└─────────────────────────────────────────────────────────────┘
```

## 通信协议

### 1. STM32 → Python (GPS/IMU数据)

**帧格式:**
```
AA 55 TYPE LEN(2) PAYLOAD CHECKSUM(2) 0D 0A
```

**GPS数据包 (Type=0x01, 56字节):**
- 纬度/经度 (double, 8字节 x2)
- 航向角 (float, 4字节)
- 东/北/天速度 (float, 4字节 x3)
- 高程 (float, 4字节)
- UTC时间 (uint32, 4字节)
- 定位质量/卫星数 (uint8, 1字节 x2)
- 对齐保留 (2字节)

**IMU数据包 (Type=0x02, 32字节):**
- XYZ加速度 (float, 4字节 x3)
- XYZ角速度 (float, 4字节 x3)
- 温度 (float, 4字节)
- UTC时间 (uint32, 4字节)

**校验和:** 16位累加和 (sum(所有字节) & 0xFFFF)

**频率:**
- GPS: 10Hz
- IMU: 100Hz

### 2. Python → STM32 (控制命令)

**帧格式:**
```
AA 55 10 04 00 STEERING(2) THROTTLE(2) CHECKSUM(2) 0D 0A
```

**控制数据包 (Type=0x10, 4字节):**
- 转向PWM值 (uint16, 1000-2000us)
- 油门PWM值 (uint16, 1000-2000us)

**校验和:** 16位累加和

**频率:** 74Hz (控制周期)

**超时保护:** 1秒无命令则恢复中位值(1500us)

## 模式切换 (SBUS CH8)

**手动模式 (CH8 < 1500):**
- 遥控器直接控制PWM输出
- Python命令被忽略
- 适用于手动操作和测试

**自动模式 (CH8 > 1500):**
- Python命令控制PWM输出
- 遥控器信号被忽略
- 适用于自动路径跟踪

**安全保护:**
- SBUS信号丢失1秒 → 中立位置(1500us)
- Python命令超时1秒 → 中立位置(1500us)
- 任何时候可通过CH8切换回手动模式

## 文件说明

### STM32端

**核心模块:**
- `STM32H743/FML/PythonLink.h/c` - Python通信链路
  - GPS/IMU数据发送 (DMA)
  - 控制命令接收和解析
  - 超时保护

- `STM32H743/FML/SBUS.h/c` - SBUS遥控器
  - RC信号解析
  - CH8模式检测
  - 自动/手动模式切换
  - Failsafe保护

- `STM32H743/FML/GPS.h/c` - GPS/IMU解析
  - NMEA解析 (GPRMI)
  - IMU数据解析 (GPIMU)
  - 即时发送到Python

- `STM32H743/FML/pwm_ctrol.h/c` - PWM控制
  - 转向/油门PWM输出
  - 脉宽设置函数

**配置:**
- UART2: GPS/IMU接收 (115200 bps)
- UART4: SBUS接收 (100000 bps, 偶校验, 2停止位)
- UART5: Python通信 (921600 bps, DMA TX/RX)

### Python端

**核心模块:**
- `python/gps_imu_receiver.py` - GPS/IMU数据接收器
  - 协议解析
  - 校验和验证
  - 数据结构定义

- `python/realtime_control.py` - 实时闭环控制器
  - GPS/IMU接收
  - 坐标转换
  - 运动控制算法集成
  - 控制命令发送

- `python/mower_controller.py` - 运动控制算法
  - 纯追踪路径跟踪
  - PID控制
  - 速度规划

- `python/test_control_protocol.py` - 协议测试工具
  - 测试控制命令发送
  - PWM值验证
  - 硬件测试辅助

**依赖:**
```
pip install pyserial numpy matplotlib
```

## 使用流程

### 1. 硬件准备
- 确保GPS模块连接到UART2
- SBUS接收器连接到UART4
- USB转串口连接到UART5 (连接到PC)
- 电机和舵机正确接线

### 2. STM32固件烧录
```bash
# 使用Keil MDK或STM32CubeIDE编译并烧录
cd MDK-ARM
# 打开 STM32H743.uvprojx
# 编译并下载到STM32
```

### 3. Python环境配置
```bash
cd python
pip install -r requirements.txt
```

### 4. 测试步骤

**步骤1: 验证GPS/IMU数据接收**
```bash
python gps_imu_receiver.py
# 修改COM端口为实际端口
# 应该看到GPS和IMU数据打印
```

**步骤2: 测试控制命令协议**
```bash
python test_control_protocol.py
# 选择测试项目
# 在手动模式下(CH8<1500)验证遥控器控制正常
# 切换到自动模式(CH8>1500)验证Python命令生效
```

**步骤3: 准备路径文件**
```bash
# 使用path_planner.py或手动创建路径JSON文件
# 格式: [[x1,y1], [x2,y2], ...]
```

**步骤4: 运行实时控制**
```bash
python realtime_control.py
# 修改配置: PORT, BAUDRATE, PATH_FILE
# 确保CH8 > 1500 (自动模式)
# 系统开始闭环控制
```

## 调试技巧

### STM32端调试
```c
// 在main.c或app.c中打印调试信息
PythonLink_PrintDebugInfo();  // 打印通信统计
SBUS_PrintDebugInfo();         // 打印SBUS状态
```

### Python端调试
```python
# 在realtime_control.py中启用详细日志
# 每秒打印一次控制信息
# 查看横向误差、航向误差等
```

### 常见问题

**Q: GPS数据无法接收?**
- 检查UART2连接和波特率(115200)
- 确认GPS模块有卫星信号(LED闪烁)
- 检查GPS_ParseGPRMI()是否调用PythonLink_SendGPSData()

**Q: Python命令不生效?**
- 确认CH8 > 1500 (切换到自动模式)
- 检查UART5连接和波特率(921600)
- 用test_control_protocol.py测试协议
- 查看PythonLink_PrintDebugInfo()统计

**Q: 控制不稳定或振荡?**
- 调整mower_controller.py中的PID参数
- 降低max_yawrate限制转向速度
- 增加lookahead_min提高稳定性
- 检查GPS更新频率和延迟

**Q: 突然停止不动?**
- 检查SBUS信号是否丢失(1秒failsafe)
- 检查Python命令是否超时(1秒超时)
- 确认电池电量充足
- 查看PWM输出是否正常

## 安全注意事项

⚠️ **重要安全提示:**

1. **始终保持遥控器开机** - CH8可随时切换到手动模式
2. **测试时保持低速** - 先用小的PWM偏移测试
3. **确保急停可用** - 遥控器的停止开关或断电开关
4. **在空旷区域测试** - 避免障碍物和人员
5. **检查电池电压** - 低电压会导致异常行为
6. **验证方向正确** - 前进/后退、左转/右转方向与预期一致

## 参数调优

### 控制器参数 (realtime_control.py)
```python
params = {
    'k_heading': 2.0,        # 航向增益 (增大→更激进转向)
    'k_xte': 0.8,            # 横向误差增益 (增大→更快纠正偏离)
    'k_heading_d': 0.1,      # 航向微分增益 (增大→抑制振荡)
    'max_forward_mps': 1.2,  # 最大速度 m/s
    'max_yawrate': π/2.5,    # 最大转向速率 rad/s
    'lookahead_min': 0.35,   # 最小前视距离 m
    'lookahead_max': 1.2,    # 最大前视距离 m
}
```

### PWM映射 (realtime_control.py)
```python
# 前进信号: -100~100 映射到 1000~2000us
throttle_pwm = 1500 + forward * 5.0

# 转向信号: -100~100 映射到 1000~2000us  
steering_pwm = 1500 + turn * 5.0
```

根据实际电机和舵机响应调整映射关系。

## 性能指标

- **GPS频率:** 10Hz
- **IMU频率:** 100Hz
- **控制频率:** 74Hz
- **通信波特率:** 921600 bps
- **路径跟踪精度:** < 0.5m (取决于GPS质量)
- **响应延迟:** < 50ms (GPS采样 + 算法计算 + 串口传输)

## 扩展功能建议

1. **障碍物检测** - 添加超声波/激光雷达传感器
2. **电池监控** - 添加电压监测和低电报警
3. **数据记录** - 记录轨迹和控制信号用于分析
4. **Web界面** - 实时监控和参数调整
5. **多路径规划** - 支持复杂割草区域

## 开发团队

自动生成的草坪割草机控制系统
基于STM32H7 + Python运动控制算法

---

**最后更新:** 2025-11-13
**版本:** v1.0