# 车辆运动模型自动校准程序使用说明

## 功能概述

本程序实现车辆运动模型的**自动校准**，通过执行预定的测试序列（直线加速、恒速巡航、转向等），同时采集PWM控制信号、GPS位置速度、IMU惯导数据，为后续建立精确运动模型提供数据基础。

## 安全机制

**关键安全开关：SBUS CH8**
- **CH8 < 1500**：允许运行测试
- **CH8 ≥ 1500**：立即紧急停止，回中PWM

**重要**：测试过程中，操作员应随时准备将CH8拨到≥1500位置以紧急停止车辆。

## 启动方式

### 自动启动（推荐）
1. 将车辆放置在**完全空旷**的场地（至少50m×50m开阔区域）
2. 确保GPS已定位、IMU正常
3. 将遥控器CH8拨到**< 1500**位置（安全允许）
4. 将遥控器CH7拨到**> 1800**位置
5. 程序自动检测到触发条件，开始测试序列

### 手动启动（备用）
通过UART5发送命令（如果实现了命令接口）或调用`MotionCalibration_Start()`函数。

## 测试序列

| 序号 | 状态名称           | 油门PWM | 舵机PWM | 持续时间 | 说明                     |
|------|-------------------|---------|---------|----------|--------------------------|
| 1    | WARMUP            | 1500    | 1500    | 2秒      | 预热，静止检查传感器     |
| 2    | ACCEL_LOW         | 1600    | 1500    | 3秒      | 低速加速                 |
| 3    | CRUISE_LOW        | 1600    | 1500    | 5秒      | 低速巡航                 |
| 4    | REST_1            | 1500    | 1500    | 1秒      | 间歇停顿                 |
| 5    | ACCEL_MID         | 1700    | 1500    | 3秒      | 中速加速                 |
| 6    | CRUISE_MID        | 1700    | 1500    | 5秒      | 中速巡航                 |
| 7    | REST_2            | 1500    | 1500    | 1秒      | 间歇停顿                 |
| 8    | ACCEL_HIGH        | 1800    | 1500    | 3秒      | 高速加速                 |
| 9    | CRUISE_HIGH       | 1800    | 1500    | 5秒      | 高速巡航                 |
| 10   | REST_3            | 1500    | 1500    | 1秒      | 间歇停顿                 |
| 11   | TURN_LEFT_LIGHT   | 1600    | 1600    | 4秒      | 轻微左转                 |
| 12   | REST_4            | 1500    | 1500    | 1秒      | 间歇停顿                 |
| 13   | TURN_RIGHT_LIGHT  | 1600    | 1400    | 4秒      | 轻微右转                 |
| 14   | REST_5            | 1500    | 1500    | 1秒      | 间歇停顿                 |
| 15   | TURN_LEFT_HEAVY   | 1600    | 1700    | 4秒      | 大幅左转                 |
| 16   | REST_6            | 1500    | 1500    | 1秒      | 间歇停顿                 |
| 17   | TURN_RIGHT_HEAVY  | 1600    | 1300    | 4秒      | 大幅右转                 |
| 18   | BRAKE             | 1500    | 1500    | 2秒      | 制动                     |
| 19   | FINISHED          | 1500    | 1500    | -        | 完成                     |

**总耗时**：约 **48秒**

## 数据采集格式

程序以20Hz频率输出数据到UART5（波特率115200），格式为ASCII字符串：

```
$CAL,seq,time_ms,state,throttle_pwm,steering_pwm,lat,lon,alt,hdg,pitch,roll,gx,gy,gz,ax,ay,az
```

### 字段说明
- `seq`：采样序号（从0开始）
- `time_ms`：MCU时间戳（毫秒）
- `state`：当前测试状态名称
- `throttle_pwm`：油门PWM脉宽（微秒）
- `steering_pwm`：舵机PWM脉宽（微秒）
- `lat`：纬度（度，6位小数）
- `lon`：经度（度，6位小数）
- `alt`：高程（米，2位小数）
- `hdg`：航向角（度，2位小数）
- `pitch`：俯仰角（度，2位小数）
- `roll`：横滚角（度，2位小数）
- `gx,gy,gz`：陀螺仪角速度（毫度/秒，整数）
- `ax,ay,az`：加速度计（毫g，整数）

### 示例
```
$CAL,0,1234,WARMUP,1500,1500,39.831867,116.280907,47.50,90.00,0.10,-0.05,12,-5,3,0,1,-1000
$CAL,1,1284,WARMUP,1500,1500,39.831867,116.280907,47.50,90.01,0.11,-0.06,13,-4,2,1,2,-999
...
```

## 数据记录建议

### 使用上位机Python脚本记录
```python
import serial
import datetime

port = serial.Serial('COM5', 115200)  # 修改为实际端口
log_file = f"calibration_{datetime.datetime.now().strftime('%Y%m%d_%H%M%S')}.log"

with open(log_file, 'w') as f:
    while True:
        line = port.readline().decode('utf-8', errors='ignore')
        print(line, end='')
        f.write(line)
        f.flush()
        
        if 'FINISHED' in line or 'EMERGENCY' in line:
            break
```

### 数据离线分析
保存的`.log`文件可用于：
1. 绘制速度-时间曲线，拟合加速度/减速度参数
2. 分析转向半径与PWM、速度的关系
3. 识别系统延迟、死区
4. 建立差速/阿克曼转向模型
5. 优化控制器增益（PID参数）

## 调参指南

可在`motion_calibration_task.c`中修改以下参数：

### PWM档位
```c
#define CAL_PWM_LOW_THROTTLE        (1600U)      // 低速
#define CAL_PWM_MID_THROTTLE        (1700U)      // 中速
#define CAL_PWM_HIGH_THROTTLE       (1800U)      // 高速
#define CAL_PWM_LIGHT_TURN          (1600U)      // 轻转向
#define CAL_PWM_HEAVY_TURN          (1700U)      // 重转向
```

### 测试时长
```c
#define CAL_DURATION_WARMUP         (2000U)      // 预热
#define CAL_DURATION_ACCEL          (3000U)      // 加速
#define CAL_DURATION_CRUISE         (5000U)      // 巡航
#define CAL_DURATION_TURN           (4000U)      // 转向
#define CAL_DURATION_BRAKE          (2000U)      // 制动
#define CAL_DURATION_REST           (1000U)      // 间歇
```

### 采样频率
```c
#define CAL_TASK_PERIOD_MS          (50U)        // 20Hz采样
```

## 注意事项

1. **场地要求**：
   - 必须是完全空旷的平坦场地
   - 避免障碍物、人员
   - GPS信号良好
   - 地面摩擦力均匀

2. **测试前检查**：
   - GPS已定位（卫星数≥8）
   - IMU数据正常
   - SBUS信号稳定
   - 电池电量充足

3. **安全操作**：
   - 操作员全程监控
   - 随时准备拨动CH8紧急停止
   - 首次测试建议降低PWM档位
   - 建议2-3人协同（1人操作遥控器，1人监控数据，1人现场观察）

4. **重复测试**：
   - 建议每种场地条件（干/湿、平/坡）测试3次以上
   - 记录环境信息（温度、风速、地面材质）

## 故障排查

| 现象                          | 可能原因                     | 解决方法                          |
|------------------------------|------------------------------|-----------------------------------|
| 无法启动（CH7>1800无反应）    | CH8≥1500或SBUS信号丢失       | 检查CH8位置、SBUS连接             |
| 启动后立即停止                | CH8被误触或信号干扰          | 确保CH8<1500且信号稳定            |
| 数据LOG中GPS为0              | GPS未定位                    | 等待GPS定位，检查天线             |
| 车辆不动/PWM无输出            | 电机驱动器未启动             | 检查硬件、供电                    |
| 采样数据不连续                | UART5缓冲区溢出              | 降低采样频率或增大缓冲区          |

## 后续数据处理流程

1. **数据预处理**：
   - 提取$CAL行
   - 按state分组
   - 时间戳对齐
   - 滤除异常值

2. **特征提取**：
   - 加速度：`Δv/Δt`
   - 转弯半径：`v²/(|ω|·v)` 或轨迹拟合
   - 响应延迟：PWM变化到速度变化的时滞
   - 死区：最小有效PWM差值

3. **模型拟合**：
   - 运动学模型（差速/阿克曼）
   - 动力学模型（质量、惯性、摩擦力）
   - 执行器模型（电机响应、转向比）

4. **参数更新**：
   - 将拟合参数更新到`motion_config.h`
   - 重新编译、测试闭环控制效果

## 代码占用

- Flash: ~6KB（状态机+数据采集逻辑）
- RAM: ~1KB（任务栈+缓冲区）
- 任务优先级：3（低于控制任务，高于空闲任务）

## 作者与日期

- 文件：`motion_calibration_task.c/h`
- 创建：2025-11-25
- 版本：v1.0