# ARES 项目交接文档

> **最后更新**: 2025 年  
> **项目性质**: B类"马铃薯捡拾机器人竞技"走廊导航系统  
> **本文目标**: 让任何人 (人类或 AI) 能在 30 分钟内理解整个项目并开始工作

---

## 目录

1. [一句话概述](#1-一句话概述)
2. [比赛规则速览](#2-比赛规则速览)
3. [硬件架构](#3-硬件架构)
4. [软件架构总览](#4-软件架构总览)
5. [代码目录结构](#5-代码目录结构)
6. [数据流水线 (核心)](#6-数据流水线-核心)
7. [各模块详解](#7-各模块详解)
8. [FreeRTOS 任务一览](#8-freertos-任务一览)
9. [CAN 通信协议](#9-can-通信协议)
10. [EKF 算法详解](#10-ekf-算法详解)
11. [控制器与安全层](#11-控制器与安全层)
12. [比赛流程状态机](#12-比赛流程状态机)
13. [全部可调参数](#13-全部可调参数)
14. [已知问题与待办](#14-已知问题与待办)
15. [构建与烧录](#15-构建与烧录)
16. [实车调试流程](#16-实车调试流程)
17. [文件快速索引](#17-文件快速索引)

---

## 1. 一句话概述

一个运行在 **STM32H743 (FreeRTOS)** 上的走廊跟踪导航系统——通过 **4 个 VL53L0X 侧向测距** 做横向/航向定位（鲁棒 EKF），**4 个前后激光** 做防撞和到端检测，PD 控制器纠偏，安全状态机兜底，最终通过 **CAN 总线** 将速度指令发给 STM32F407 底盘控制器。

---

## 2. 比赛规则速览

| 项目 | 规格 |
|------|------|
| 场地尺寸 | 390 cm × 300 cm |
| 垄沟（走廊）宽度 | **40 cm**（允许 ±5% 误差） |
| 田垄数量 | 5 条 |
| 垄间模式需遍历 | **6 条垄沟** |
| 机器人尺寸 | **20 cm × 20 cm**（含外壳） |
| 驱动方式 | 四轮差速 |
| 比赛时间 | 5 分钟 |
| 任务 | 在走廊中往返行驶，捡拾目标物 |

**关键约束**: 走廊 40cm - 车体 20cm = 每边仅 **10cm 余量**。1cm 的传感器偏差就是 10% 的误差。

### 2.1 赛道拓扑理解（垄间模式）

本项目当前最相关的正式比赛模式是 **垄间作业模式**。

#### 赛道几何

- 整个比赛场地尺寸为 `390cm × 300cm`
- 场地内部共有 **5 条田垄**
- 每条田垄：长 `220cm`、宽 `30cm`、高 `12cm`
- 围栏与田垄之间、以及相邻田垄之间，均形成宽 `40cm` 的可通行垄沟
- 场地仅有 **1 个出入口**，宽 `40cm`
- 出入口外侧紧邻 **比赛启动区**，尺寸 `40cm × 100cm`

#### 为什么是 6 条垄沟，不是 5 条

虽然场地内只有 **5 条田垄**，但在垄间模式下，机器人实际需要遍历的是 **6 条垄沟**：

1. 最左侧围栏与第 1 条田垄之间的垄沟
2. 第 1 条田垄与第 2 条田垄之间的垄沟
3. 第 2 条田垄与第 3 条田垄之间的垄沟
4. 第 3 条田垄与第 4 条田垄之间的垄沟
5. 第 4 条田垄与第 5 条田垄之间的垄沟
6. 第 5 条田垄与最右侧围栏之间的垄沟

也就是说：

- **田垄数 = 5**
- **垄间可通行垄沟数 = 6**

这是赛道理解里最容易搞错的地方。若只按“5 条走廊”建模，会与正式比赛要求不一致。

#### 垄间模式下的导航任务本质

在垄间模式下，导航任务不是“单条走廊往返”，而是：

1. 从启动区进入场地
2. 进入某一条垄沟
3. 沿垄沟稳定行驶并完成作业
4. 到达端部后完成转向和换沟
5. 依次遍历 **全部 6 条垄沟**
6. 最终从唯一出入口驶离场地
7. 自主停在启动区内

从导航角度看，赛道是一个 **6 段平行窄走廊 + 端部换沟动作 + 单出入口回停** 的组合问题。

#### 对导航系统的直接要求

- **窄走廊居中跟踪**
  走廊宽 `40cm`，若车体宽 `20cm`，理论左右余量各仅 `10cm`
- **到端检测**
  能识别走到垄沟尽头，避免撞围栏
- **端部转向**
  在垄沟末端稳定完成姿态调整
- **换沟**
  从当前垄沟切换到相邻垄沟，而不是原地掉头后回到同一条垄沟
- **赛道级状态机**
  知道当前是第几条垄沟、下一条目标是哪条、何时结束遍历
- **最终退出与停区**
  驶离场地后必须自主停在启动区，否则可能判 0 分

#### 当前项目与赛道要求的关系

当前 ARES 项目已经具备：

- 单条走廊内的相对定位
- 走廊跟踪控制
- 到端检测
- 原地转向
- 安全停车与防撞

但当前脚本状态机更接近：进入一条走廊、沿走廊前进、到端后转向、再走一趟、然后退出。

它更像 **单垄沟闭环验证系统**，还不是完整的 **6 垄沟遍历赛道导航系统**。

后续若要适配正式比赛，重点不是重写 EKF，而是补上：

- 多垄沟遍历状态机
- 端部换沟策略
- 最终出场与启动区停车逻辑

---

## 3. 硬件架构

### 3.1 双 MCU 体系

```
┌─────────────────────────────────┐     CAN Bus     ┌─────────────────────────┐
│  STM32H743 (上位机/主控)         │ ◄──────────────► │  STM32F407 (底盘控制器)   │
│  480MHz Cortex-M7, DP-FPU       │    0x100 TX      │  LADRC 电机控制          │
│  FreeRTOS + 传感器 + 导航算法     │    0x181/200 RX  │  四路编码器 + 四路电机     │
└─────────────────────────────────┘                  └─────────────────────────┘
```

### 3.2 传感器清单

| 传感器 | 数量 | 用途 | 接口 | 量程 |
|--------|------|------|------|------|
| **VL53L0X** ToF | 4 (左前/左后/右前/右后) | 侧向墙壁测距，走廊跟踪 | I2C1 + I2C2 | 2cm ~ 2m |
| **STP-23L** 激光 | 2 (前/后) | 远距离测距，到端检测 | UART2 / UART4 (230400bps) | 7cm ~ 7.5m |
| **ATK-MS53L1M** 激光 | 2 (前/后) | 近距离补盲 (填 STP 的 7cm 盲区) | UART3 / UART6 (115200bps) | ~4m |
| **HWT101** IMU | 1 | 单轴陀螺仪 (航向角速度 wz) | UART7 | ±2000°/s |

### 3.3 VL53L0X 接线

```
左侧 (I2C2):                          右侧 (I2C1):
  左前 LF: XSHUT=PB1,  地址=0x62        右前 RF: XSHUT=PD13, 地址=0x56
  左后 LR: XSHUT=PC5,  地址=0x64        右后 RR: XSHUT=PD14, 地址=0x58
```

### 3.4 其他 GPIO

| 引脚 | 功能 |
|------|------|
| PE3 | CAN 活动 LED (100ms 翻转) |

---

## 4. 软件架构总览

```
┌─────────────────────────────────────────────────────────────────────────────┐
│                              FreeRTOS 任务层                                │
│   canTxTask   monitorTask   navTask   laserTestTask   vl53Task   imuTask   │
│    (20ms)      (100ms)      (20ms)     (50ms)         (100ms)    (10ms)    │
└──────┬──────────┬──────────┬──────────┬───────────────┬──────────┬─────────┘
       │          │          │          │               │          │
       │  CmdSlot │          │     Blackboard (全局传感器数据中心)    │
       │   Pop    │          │     taskENTER_CRITICAL 保护           │
       ▼          ▼          ▼                                      │
  ┌─────────┐ ┌────────┐ ┌───────────────────────────────────┐     │
  │CAN 0x100│ │Odom    │ │      导航流水线 (navTask, 20ms)     │     │
  │→ 底盘    │ │积分    │ │  Preproc → EKF → Script           │     │
  │         │ │→黑板   │ │  → Ctrl → SafetyFSM → CmdSlot     │     │
  └─────────┘ └────────┘ └───────────────────────────────────┘     │
                                                                     │
         ┌───────────────────────────────────────────────────────────┘
         │
    各传感器驱动：hwt101.c / vl53_board.c / laser_manager.c
    → Blackboard_Update*()
```

---

## 5. 代码目录结构

```
D:\ARES\
├── App/                              ← 【所有业务代码在这里】
│   ├── robot_params.h                ← ★ 全局参数配置中心 (唯一的调参入口)
│   ├── app_tasks.c/.h                ← 所有 FreeRTOS 任务实现 + 系统初始化
│   ├── retarget.c/.h                 ← printf → USB CDC 重定向
│   │
│   ├── Can/
│   │   └── snc_can_app.c/.h          ← CAN 协议层 (编解码/CRC8/滤波器)【已冻结，勿动】
│   │
│   ├── Contract/                     ← 模块间数据契约
│   │   ├── robot_blackboard.c/.h     ← 全局传感器数据黑板 (线程安全)
│   │   ├── robot_odom.c/.h           ← 差速里程计积分
│   │   ├── robot_cmd_slot.c/.h       ← 导航→CAN 指令传递槽
│   │   └── chassis_can_msg.h         ← CAN 帧结构体定义
│   │
│   ├── est/                          ← 状态估计
│   │   ├── corridor_ekf.c/.h         ← ★ 鲁棒 EKF 核心 (3 状态)
│   │   └── corridor_filter.c/.h      ← EKF 的兼容包装层
│   │
│   ├── nav/                          ← 导航与控制
│   │   ├── corridor_ctrl.c/.h        ← PD 走廊控制器
│   │   ├── segment_fsm.c/.h          ← 安全状态机 (防撞/急停)
│   │   └── nav_script.c/.h           ← 比赛流程编排
│   │
│   ├── preproc/                      ← 传感器预处理
│   │   ├── corridor_preproc.c/.h     ← 数据清洗 + 互补融合
│   │   └── corridor_msgs.h           ← 所有模块间消息结构体
│   │
│   ├── IMU/
│   │   └── hwt101.c/.h               ← HWT101 IMU 驱动 (UART DMA)
│   │
│   ├── laser/
│   │   └── laser_manager.c/.h        ← 前后激光驱动 (STP+ATK, UART DMA)
│   │
│   └── VL53L0X_API/
│       ├── platform/
│       │   ├── vl53_board.c/.h       ← 多传感器管理 + 卡尔曼滤波
│       │   └── vl53l0x_platform.c/.h ← ST API HAL 适配
│       └── core/                     ← ST 官方 VL53L0X API (不修改)
│
├── Core/                             ← CubeMX 生成的 HAL 初始化代码
├── Drivers/                          ← STM32H7 HAL 驱动
├── Middlewares/                       ← FreeRTOS + USB
├── USB_DEVICE/                        ← CDC 虚拟串口
├── ARES.ioc                           ← CubeMX 工程文件
├── CMakeLists.txt                     ← 构建入口
└── STM32H743XX_FLASH.ld              ← 链接脚本
```

---

## 6. 数据流水线 (核心)

这是整个系统的心脏，运行在 `navTask` 中，**每 20ms 执行一次**：

```
Step 1:  Blackboard_GetSnapshot(&board)         拍摄传感器快照 (无撕裂)
           │
Step 2:  CorridorPreproc_ExtractObs()           数据清洗
           │  • VL53L0X: mm→m, 范围校验 (0.02~2.0m)
           │  • STP+ATK: 互补融合 (ATK 填 STP 的 7cm 盲区)
           │  • 前后激光偏移补偿 (d -= PARAM_FRONT_LASER_OFFSET)
           │  → 输出: CorridorObs_t {d_lf, d_lr, d_rf, d_rr, d_front, d_back, valid_mask}
           │
Step 3:  CorridorFilter_Update()                EKF 状态估计
           │  → Predict(odom_vx, imu_wz, dt)    预测步 (IMU wz + 里程计)
           │  → Update(obs)                      观测步 (VL53 侧向测距)
           │  → UpdateIMUYaw(yaw_cont, ref)      IMU 航向观测 (独立 1DOF 更新)
           │  → 输出: CorridorState_t {e_y, e_th, s, conf}
           │
Step 4:  NavScript_Update()                     比赛流程编排
           │  → 决定当前阶段: 入口对准/走廊跟踪/转向/退出
           │  → 180° 转弯判定使用 IMU 连续 yaw (非 EKF e_th)
           │  → 输出: NavScriptOutput_t {use_override, override_v/w, request_corridor}
           │
Step 5:  CorridorCtrl_Compute()                 PD 控制律 (仅在走廊跟踪模式)
           │  → w = kp_theta·e_th + kd_theta·(-wz) + kp_y·e_y
           │  → v = v_cruise × (1 - 0.4 × |w/w_max|)
           │  → 输出: RawCmd_t {v, w}
           │
Step 6:  SegFsm_Update()                       安全仲裁
           │  → 前方太近? → 减速/停车
           │  → 置信度太低? → 紧急停车
           │  → 输出: SegFsmOutput_t {safe_v, safe_w}
           │
Step 7:  CmdSlot_Push(safe_v, safe_w)           推入指令槽 → canTxTask 取走
```

---

## 7. 各模块详解

### 7.1 传感器驱动层

#### VL53L0X 侧向测距 (`vl53_board.c`)
- 上电时所有 XSHUT 拉低，逐个拉高分配 I2C 地址
- 测距预算 100ms (100000μs)，连续测距模式
- 每个传感器读数经过独立的 **1D 卡尔曼滤波** (Q=10.0, R=14.1)
- 输出 `Vl53BoardSnapshot_t` → `Blackboard_UpdateVl53()`

#### 前后激光测距 (`laser_manager.c`)
- DMA 环形缓冲区轮询，内部自有 FreeRTOS 任务 `LaserTsk` (10ms)
- **STP-23L**: 二进制协议，含校验和；3 帧中值滤波去尖刺
- **ATK-MS53L1M**: ASCII 文本 `"d: 1234\r\n"`；3 帧中值滤波
- 互补融合策略:
  - ATK 距离 < 8cm 时无条件信任 ATK (STP 盲区)
  - STP 卡在 7cm 时用 ATK
  - 两者都活着取更近的 (保守防撞)
  - STP 独活但在盲区 → 报失效 (宁可急停也不信假数据)

#### IMU (`hwt101.c`)
- UART DMA 11 字节二进制帧
- 输出: `yaw` (°)、`yaw_continuous` (°) 和 `wz` (°/s)
- `yaw`: 原始偏航角，范围 [-180°, +180°)，跨界时跳变
- `yaw_continuous`: **unwrap 后的连续偏航角**，消除 ±180° 跳变，可直接做差计算任意转角
- 注意: **输出单位是°/s，不是 rad/s**
- `HWT101_ZeroYaw()` 会同时重置 unwrap 状态

### 7.2 数据契约层

#### 黑板 (`robot_blackboard.c`)
- 全局结构体 `g_blackboard`，所有传感器数据汇聚于此
- 所有读写都在 `taskENTER_CRITICAL()` 临界区内
- 消费者调用 `Blackboard_GetSnapshot()` 获取无撕裂快照
- IMU 字段: `imu_yaw` (原始°)、`imu_yaw_continuous` (连续°)、`imu_wz` (°/s)

#### 里程计 (`robot_odom.c`)
- 从 CAN 0x200 获取四轮编码器增量，经 **ISR 侧累加器** 汇聚后由 monitorTask 一次性取走积分
- 消费链路（BUG-8 修复后）:
  ```
  CAN Rx ISR → snc_parse_odom_delta()  每帧累加到 odom_accum（不覆盖）
                    ↓
  monitorTask → SNC_CAN_ConsumeOdomDelta()  原子取走累计值并清零
                    ↓
               Odom_Update()  仅在有新帧时调用（杜绝重复积分）
  ```
- 差速运动学:
  ```
  v_left  = (fl_delta + rl_delta) / 2 × (π × 0.060 / 500) / dt
  v_right = (fr_delta + rr_delta) / 2 × (π × 0.060 / 500) / dt
  vx = (v_left + v_right) / 2
  wz = (v_right - v_left) / 0.140
  ```
- 3 帧连续零增量 → 强制 vx=wz=0 (静止检测)

#### 指令槽 (`robot_cmd_slot.c`)
- 单槽无锁设计，`CmdSlot_Push()` 写入，`CmdSlot_Pop()` 读取
- 超过 100ms 未更新 → canTxTask 自动发零速指令 (看门狗)

### 7.3 估计层

详见 [第10节 EKF 算法详解](#10-ekf-算法详解)。

### 7.4 控制层

详见 [第11节 控制器与安全层](#11-控制器与安全层)。

### 7.5 导航层

详见 [第12节 比赛流程状态机](#12-比赛流程状态机)。

---

## 8. FreeRTOS 任务一览

| 任务名 | 周期 | 优先级 | 栈大小 | 职责 |
|--------|------|--------|--------|------|
| `canTxTask` | 20ms | AboveNormal (28) | 2048B | CAN 0x100 硬实时发送；指令槽看门狗 |
| `navTask` | 20ms | AboveNormal (28) | 4096B | 完整导航流水线 (7步) |
| `LaserTsk` (内部) | 10ms | AboveNormal (28) | 4096B | 4路 UART DMA 解析 |
| `monitorTask` | 100ms | Normal (24) | 4096B | CAN 健康监控；里程计更新 |
| `laserTestTask` | 50ms | Normal (24) | 16384B | 激光轮询 + 推送黑板 |
| `vl53Task` | 100ms | Normal (24) | 4096B | VL53L0X 读取 + 推送黑板 |
| `imuTask` | 10ms | BelowNormal (20) | 2048B | IMU 解析 + 推送黑板 |
| `defaultTask` | — | Normal (24) | 512B | USB CDC 初始化；空闲循环 |

**全局配置**: Tick 频率 1000Hz, 堆 65536 字节, 最大优先级 56

---

## 9. CAN 通信协议

### 9.1 TX: H743 → F407

#### 0x100 — 速度指令 (每 20ms 发送)

| 字节 | 字段 | 类型 | 编码 |
|------|------|------|------|
| 0-1 | vx | int16 LE | vx (m/s) × 1000 |
| 2-3 | wz | int16 LE | wz (rad/s) × 1000 |
| 4 | ctrl_flags | uint8 | 控制标志位 |
| 5 | reserved | uint8 | 固定 0 |
| 6 | rolling_counter | uint8 | 单调递增 |
| 7 | crc8 | uint8 | CRC8-SAE-J1850 (bytes[0..6]) |

**硬性约束**: 必须每 ≤150ms 发一次，否则底盘进入 `SAFE_FAULT`。即使停车也要发 vx=0, wz=0。

#### 0x080 — 心跳 (每 20ms, DLC=0)

### 9.2 RX: F407 → H743

#### 0x181 — 底盘状态 (20ms)

| 字节 | 字段 |
|------|------|
| 0 | system_state: 0=BOOTING, 1=OPERATIONAL, 2=SAFE_FAULT |
| 1 | system_health: 0=OK, 1=WARNING, 2=FAULT |
| 2-5 | diag_bits (uint32 LE): 通信超时/CAN BUS OFF/电机堵转等 |
| 6 | cmd_age_10ms: 上次有效指令距今 (×10ms) |
| 7 | status_counter |

#### 0x200 — 里程计增量 (~60ms)

| 字节 | 字段 |
|------|------|
| 0-1 | fl_delta_ticks (int16 LE) — 左前轮编码器增量 |
| 2-3 | rl_delta_ticks (int16 LE) — 左后轮 |
| 4-5 | fr_delta_ticks (int16 LE) — 右前轮 |
| 6-7 | rr_delta_ticks (int16 LE) — 右后轮 |

#### 0x184 — 通信诊断 (100ms)

CRC 错误计数、计数器拒绝计数、CAN 丢包等统计信息。

---

## 10. EKF 算法详解

### 状态向量

```
x = [e_y, e_th, s]ᵀ
```

| 状态 | 含义 | 单位 |
|------|------|------|
| `e_y` | 车体中心相对走廊中心线的横向偏差 | m (左偏为正) |
| `e_th` | 车头相对走廊方向的航向偏差 | rad (左偏为正) |
| `s` | 沿走廊行驶里程 | m |

### 预测步 (每 20ms)

```
输入: odom_vx (m/s), imu_wz (rad/s), dt (s)

e_y_new  = e_y  + vx × sin(e_th) × dt
e_th_new = e_th + wz × dt
s_new    = s    + vx × cos(e_th) × dt

雅可比矩阵 F:
  [1,   vx·cos(e_th)·dt,   0]
  [0,   1,                  0]
  [0,  -vx·sin(e_th)·dt,   1]

P_pred = F·P·Fᵀ + Q
```

### 观测步

```
d_center = (W - Rw) / 2 + inset
         = (0.40 - 0.20) / 2 + 0 = 0.10m  (车居中时传感器到墙的距离)

左侧观测:
  z_ey   = d_center - (d_lf + d_lr)/2       横向偏差
  z_eth_L = atan2(d_lr - d_lf, L_s)          航向偏差

右侧观测:
  z_ey   = (d_rf + d_rr)/2 - d_center       横向偏差
  z_eth_R = atan2(d_rf - d_rr, L_s)          航向偏差

双侧有效时: z_ey 取两侧平均 = (d_right - d_left) / 2
  → d_center 被消掉，结果只取决于左右差值

单侧退化时: z_ey = d_center - d_one_side
  → d_center 的准确性至关重要！

IMU 航向观测 (侧墙更新之后独立执行):
  z_eth_imu = (imu_yaw_continuous - yaw_ref) × DEG2RAD
  yaw_ref 在 EKF 置信度 ≥ 0.5 时首次锁定
  使用 1DOF 标量 EKF 更新，R 值 (PARAM_EKF_R_ETH_IMU) 远大于侧墙 R
  → 侧墙有效时 IMU 影响小；侧墙丢失时 IMU 提供航向约束
```

### 鲁棒拒绝

每个观测独立做 **χ² 马氏距离检验**:
- 1 自由度门限 3.84 (95% 置信度)
- 超过门限的观测被标记为异常并跳过更新

### 置信度计算

```
conf = f(协方差迹) × 侧面因子 × (1 - 拒绝比例 × 0.5)
  侧面因子: 双侧=1.0, 单侧=0.7
  两侧全失效: 协方差膨胀，conf 趋向 0 → 触发 E-STOP
```

---

## 11. 控制器与安全层

### 11.1 PD 走廊控制器

```
w_cmd = kp_theta × e_th + kd_theta × (-imu_wz) + kp_y × e_y
        ───────────────   ─────────────────────   ────────────
        航向比例纠偏       航向微分阻尼(用IMU)     横向比例纠偏

w_cmd = clamp(w_cmd, ±1.5 rad/s)

v_cmd = v_cruise × (1 - 0.4 × |w/w_max|)     弯道减速
v_cmd = clamp(v_cmd, 0, 0.3 m/s)

置信度保护:
  conf < 0.3 → v × 0.3  (三折)
  conf < 0.6 → v × 0.7  (七折)
```

**参数**: kp_theta=2.0, kd_theta=0.1, kp_y=3.0, v_cruise=0.15m/s

### 11.2 安全状态机 (`segment_fsm.c`)

```
                    conf < 0.1
              ┌──────────────────┐
              ▼                  │
 ┌──────┐  Start  ┌──────────┐  │  d_front ≤ 0.25m  ┌──────────┐
 │ IDLE ├────────►│ CORRIDOR ├──┴──────────────────►│ APPROACH │
 └──────┘         └────┬─────┘  ◄────────────────── └────┬─────┘
                       │        d_front > 0.25m          │
                       │                          d_front ≤ 0.08m
                  conf ≥ 0.5                             │
                  ┌────┴─────┐                    ┌──────▼─────┐
                  │  E-STOP  │                    │    STOP    │
                  │  v=0,w=0 │                    │   v=0,w=0  │
                  └──────────┘                    └────────────┘
```

| 状态 | 行为 |
|------|------|
| **CORRIDOR** | 放行控制器输出 |
| **APPROACH** | 线性减速: v 从 raw_v 衰减至 0.05m/s (d 从 25cm→8cm) |
| **STOP** | 强制零速 |
| **E-STOP** | 强制零速；conf ≥ 0.5 时自动恢复 |

---

## 12. 比赛流程状态机

> 注意：以下状态机描述的是 **当前固件已实现的单垄沟往返流程**，用于验证走廊跟踪、到端检测、转向与退出链路。
> 它 **不等价于** 正式比赛垄间模式要求的“遍历全部 6 条垄沟”的完整赛道级导航状态机。

```
IDLE → ENTRY_ALIGN → CORRIDOR_FORWARD → TURN_AT_END → CORRIDOR_BACKWARD
                                                ↓
                                    (pass_count ≥ 2)
                                                ↓
                                              EXIT → FINISHED
```

| 阶段 | 触发条件 | 行为 | 退出条件 |
|------|----------|------|----------|
| **IDLE** | 初始状态 | 零速 | `NavScript_Start()` |
| **ENTRY_ALIGN** | Start() | 慢速前进 0.08m/s，等侧向雷达找到墙 | 双侧有效 + conf≥0.8；或超时 30s |
| **CORRIDOR_FWD** | 入口对准完成 | 走廊控制器跟踪 | d_front ≤ 0.10m (到端) |
| **TURN_AT_END** | 到端 | 原地转 180° (1.0 rad/s, 接近时减速)；使用 IMU 连续 yaw 判定转角 | 转角 ≥ π-0.1 rad |
| **CORRIDOR_BWD** | 第1次转完 | 走廊控制器跟踪 | d_back ≤ 0.10m (到端) |
| **EXIT** | 第2次转完 | 0.5m/s 直线冲出 | 双侧 VL53 全丢 + 行驶 ≥ 2m |
| **FINISHED** | 冲出完成 | 零速停车 | 终态 |

---

## 13. 全部可调参数

所有参数集中在 **`App/robot_params.h`** 中，修改后需重新编译烧录。

### P0 — 几何参数 (必须实测)

| 参数名 | 当前值 | 单位 | 说明 |
|--------|--------|------|------|
| `PARAM_ROBOT_WIDTH` | **0.200** | m | 车体外轮廓宽度 |
| `PARAM_ROBOT_LENGTH` | **0.200** | m | 车体外轮廓长度 |
| `PARAM_WHEEL_DIAMETER` | **0.060** | m | 驱动轮外径 |
| `PARAM_WHEEL_TRACK` | **0.140** | m | 左右轮中心距 |
| `PARAM_SENSOR_BASE_LENGTH` | **0.120** | m | 同侧前后 VL53 间距 |
| `PARAM_CORRIDOR_WIDTH` | **0.40** | m | 走廊宽度 |
| `PARAM_FRONT_LASER_OFFSET` | **0.0** | m | 前激光到车头前缘的内缩距离 |
| `PARAM_REAR_LASER_OFFSET` | **0.0** | m | 后激光到车尾后缘的内缩距离 |
| `PARAM_VL53_SIDE_INSET` | **0.0** | m | 侧向 VL53 到车体外壳的内缩距离 |
| `PARAM_ENCODER_CPR` | **500** | — | 编码器每转脉冲数 |

### P2 — EKF 滤波器

| 参数名 | 当前值 | 说明 |
|--------|--------|------|
| `PARAM_EKF_Q_EY` | **0.01** | 横向过程噪声 (越大越信观测) |
| `PARAM_EKF_Q_ETH` | **0.001** | 航向过程噪声 |
| `PARAM_EKF_Q_S` | **0.1** | 里程过程噪声 |
| `PARAM_EKF_R_EY` | **0.002** | 横向观测噪声 (越大越不信雷达) |
| `PARAM_EKF_R_ETH` | **0.001** | 航向观测噪声 (侧墙) |
| `PARAM_EKF_R_ETH_IMU` | **0.01** | 航向观测噪声 (IMU yaw)，远大于侧墙，用于长时约束 |
| `PARAM_EKF_P0_EY` | **0.1** | e_y 初始不确定度 |
| `PARAM_EKF_P0_ETH` | **0.1** | e_th 初始不确定度 |

### P3 — 控制器

| 参数名 | 当前值 | 单位 | 说明 |
|--------|--------|------|------|
| `PARAM_CTRL_KP_THETA` | **2.0** | — | 航向比例增益 |
| `PARAM_CTRL_KD_THETA` | **0.1** | — | 航向微分增益 |
| `PARAM_CTRL_KP_Y` | **3.0** | — | 横向比例增益 |
| `PARAM_CTRL_V_CRUISE` | **0.15** | m/s | 巡航速度 |
| `PARAM_CTRL_W_MAX` | **1.5** | rad/s | 最大角速度 |
| `PARAM_CTRL_V_MAX` | **0.3** | m/s | 最大线速度 |

### P4 — 安全与脚本

| 参数名 | 当前值 | 单位 | 说明 |
|--------|--------|------|------|
| `PARAM_SAFE_D_FRONT_STOP` | **0.08** | m | 前向强制停车距离 |
| `PARAM_SAFE_D_FRONT_APPROACH` | **0.25** | m | 前向减速预警距离 |
| `PARAM_SAFE_APPROACH_MIN_V` | **0.05** | m/s | 减速区最低速度 |
| `PARAM_SAFE_CONF_ESTOP` | **0.10** | — | E-Stop 置信度阈值 |
| `PARAM_SCRIPT_ENTRY_TIMEOUT` | **30000** | ms | 入口对准超时 |
| `PARAM_SCRIPT_TURN_OMEGA` | **1.0** | rad/s | 转向角速度 |
| `PARAM_SCRIPT_EXIT_RUNOUT` | **2.0** | m | 退出场地后冲距离 |

### P5 — 传感器驱动

| 参数名 | 当前值 | 说明 |
|--------|--------|------|
| `PARAM_VL53_TIMING_BUDGET` | **100000** | VL53L0X 测距预算 (μs) |
| `PARAM_VL53_KALMAN_Q` | **10.0** | VL53 卡尔曼过程噪声 |
| `PARAM_VL53_KALMAN_R` | **14.1** | VL53 卡尔曼观测噪声 |
| `PARAM_IMU_YAW_OFFSET` | **0.0** | IMU 零位偏置 (rad) |

---

## 14. 已知问题与待办

### 严重 (已全部修复 ✅)

| # | 问题 | 状态 | 修复位置 |
|---|------|------|----------|
| **BUG-1** | IMU 输出 `wz` 单位是 **°/s**，但 EKF 预测步按 **rad/s** 使用，导致航向积分放大 57.3 倍 | ✅ 已修复 | `app_tasks.c:305` — 加了 `PARAM_DEG2RAD()` 转换 |
| **BUG-2** | `corridor_filter.c` 中 EKF 的 Q/R/P0 参数全部硬编码，修改 `robot_params.h` 中的 `PARAM_EKF_*` 不生效 | ✅ 已修复 | `corridor_filter.c:40-56` — 改为读取 `PARAM_EKF_*` 宏 |
| **BUG-3** | `AppTasks_Init()` 只调用 `SegFsm_Init()`，未调用 `SegFsm_Start()`，导致安全状态机长期处于 IDLE，输出始终为零速 | ✅ 已修复 | `app_tasks.c:382` — 补加 `SegFsm_Start()` |
| **BUG-4** | `CORRIDOR_BACKWARD` 段转 180° 后正向行驶，但到端检测错误使用 `d_back`（后向雷达），应为 `d_front` | ✅ 已修复 | `nav_script.c:CORRIDOR_BACKWARD` — 改为检查 `d_front` |
| **BUG-5** | `ENTRY_ALIGN` 段超时保护硬编码了 `30000`，`entry_align_timeout` 参数实际不生效 | ✅ 已修复 | `nav_script.c:155` — 改为 `s_cfg.entry_align_timeout` |
| **BUG-6** | `EXIT` 段退出直线速度误用角速度参数 `turn_omega * 0.5f`，调转向速度会意外影响退出速度 | ✅ 已修复 | `nav_script.h` 加 `exit_v` 字段；`nav_script.c:262` 读取它；`app_tasks.c` 传入 `PARAM_SCRIPT_EXIT_V`；`robot_params.h` 加宏 |
| **BUG-7** | EKF 卡尔曼增益计算 `K = P·H^T·S⁻¹` 只乘 `S_inv` 对角项，忽略观测间相关性，增益不正确 | ✅ 已修复 | `corridor_ekf.c:576-607` — 改为完整两步矩阵乘法：先算 `PHT = P·H^T`，再算 `K = PHT·S_inv` |
| **BUG-8** | `0x200 Odom Delta` 按"状态型最新值"消费，实为"事件型增量数据"：ISR 直接覆盖旧帧导致**漏积分**（60ms帧率 vs 100ms消费，必然丢帧）；monitorTask 无条件调用 `Odom_Update()` 导致**重复积分**（无新帧时同一份 delta 被积分两次） | ✅ 已修复 | `snc_can_app.h` 新增 `SNC_OdomDeltaAccum_t`；`snc_can_app.c` ISR 改为累加并实现 `SNC_CAN_ConsumeOdomDelta()` 原子取走；`app_tasks.c:monitorTask` 改用新 API，仅在有新帧时调用积分 |
| **BUG-9** | IMU `yaw` 输出范围 [-180°, +180°)，跨界时跳变，且项目完全未使用 `yaw`；180° 转弯判定纯依赖 EKF `e_th` (无侧墙观测时靠 wz 积分漂移) | ✅ 已修复 | `hwt101.c` 新增 yaw unwrap → `yaw_continuous`；`robot_blackboard.h` 新增 `imu_yaw_continuous`；`nav_script.c` 转弯判定改用 IMU 连续 yaw 差值；`corridor_ekf.c` 新增 `CorridorEKF_UpdateIMUYaw()` 1DOF 标量观测更新；`corridor_filter.c` 管理 yaw_ref 参考值并在侧墙更新后注入 IMU 航向观测 |

### 需要实车标定

| # | 问题 | 说明 |
|---|------|------|
| **CAL-1** | `PARAM_FRONT_LASER_OFFSET = 0.0` | 如果传感器在车体内部，停车距离会比预期偏大 |
| **CAL-2** | `PARAM_REAR_LASER_OFFSET = 0.0` | 同上 |
| **CAL-3** | `PARAM_VL53_SIDE_INSET = 0.0` | 如果传感器内缩，单侧退化时会有系统偏差 |
| **CAL-4** | `PARAM_IMU_YAW_OFFSET = 0.0` | 声明了但代码中未使用 |

### 设计风险 / 尚未处理

| # | 问题 | 当前影响 | 建议方向 |
|---|------|----------|----------|
| **RISK-1** | `TURN_AT_END` 原地转向阶段与 `SegFsm` 前向防撞逻辑存在潜在冲突 | `nav_script` 在到端后会输出 `v=0, w!=0` 的原地转向命令；但 `segment_fsm` 仍按“前向距离过近 → STOP”处理，可能把转向角速度也清零，导致机器人到端后想转却被安全层按住 | 后续应为安全层引入“动作语义”或“模式感知”，区分走廊前进与原地转向；转向阶段允许 `v=0` 时保留受限 `w`，而不是沿用普通前向防撞全停策略 |

补充说明：

- 该问题的根因不是传感器噪声，而是 **脚本层与安全层的仲裁语义不一致**
- 侧向 VL53 在端部转向时本来也会偏离正常走廊几何，因此当前已经用 IMU `yaw_continuous` 来判定转角，这部分方向是对的
- 真正需要补的是：**转向阶段的专用安全策略**，否则后续扩展到多垄沟遍历时，端部动作会成为卡点

### 代码质量 (已全部修复 ✅)

| # | 问题 | 状态 | 修复位置 |
|---|------|------|----------|
| **Q-1** | `PARAM_CTRL_SPEED_REDUCTION` 未被控制器读取，硬编码了 `0.4f` | ✅ 已修复 | `corridor_ctrl.h` 加字段 `speed_reduction_k`；`corridor_ctrl.c:59` 读取它；`app_tasks.c` 传入 `PARAM_CTRL_SPEED_REDUCTION` |
| **Q-2** | `PARAM_SCRIPT_ENTRY_V` 未被脚本读取，硬编码了 `0.08f` | ✅ 已修复 | `nav_script.h` 加字段 `entry_align_v`；`nav_script.c:146` 读取它；`app_tasks.c` 传入 `PARAM_SCRIPT_ENTRY_V` |
| **Q-3** | `PARAM_SCRIPT_EXIT_RUNOUT` 未被脚本读取，硬编码了 `2.0f` | ✅ 已修复 | `nav_script.h` 加字段 `exit_runout_m`；`nav_script.c:275` 读取它；`app_tasks.c` 传入 `PARAM_SCRIPT_EXIT_RUNOUT` |
| **Q-4** | `PARAM_VL53_TIMING_BUDGET` 未被使用，直接传字面量 `100000` | ✅ 已修复 | `app_tasks.c:222,232` 改为传 `PARAM_VL53_TIMING_BUDGET` |
| **Q-5** | `exit_start_s` 是函数内 `static` 局部变量，`NavScript_Reset()` 无法清除它 | ✅ 已修复 | 移入 `s_internal` 结构体，`memset(&s_internal, 0, ...)` 统一清零 |
| **Q-6** | 转向角度通过 EKF 的 `e_th` 差值测量，不是绝对角度。如果 EKF 发散或走廊参照丢失，转向判断可能不准 | ✅ 已修复 | `nav_script.c` — 改用 IMU `yaw_continuous` (unwrap 后的连续偏航角) 判定转角，不再依赖 EKF `e_th` |
| **Q-7** | `g_snc_can_app` 被中断写、被任务读，没有互斥保护 (中等风险) | ✅ 部分修复 | `odom_accum` 的取走/清零已通过 `taskENTER_CRITICAL` 保护（BUG-8 修复）；其余字段（status/rpm/diag）仍无保护，但读写均为原子宽度或单次赋值，风险可接受 |

---

## 15. 构建与烧录

### 构建

```bash
# 确保 gcc-arm-none-eabi 在 PATH 中
cmake --preset Debug
cmake --build build/Debug
```

- 输出: `build/Debug/ARES.elf`
- 工具链: `cmake/gcc-arm-none-eabi.cmake`
- C 标准: C11
- 浮点 printf: 已开启 (`-u _printf_float`)

### 烧录

使用 **STM32CubeProgrammer** 或 **OpenOCD** 烧录 `.elf` 文件到 STM32H743。

### 注意事项

- `CMakeLists.txt` 第 38 行有一个大写 `.C` 后缀 (`laser_manager.C`)，在大小写敏感的文件系统上可能会构建失败
- MPU Region 1 配置了 `0x30000000` 32KB 为非缓存区，所有 DMA 缓冲区必须放在此区域 (`.dma_buffer` section)
- USB CDC 用于 `printf` 输出 (通过 `retarget.c`)

---

## 16. 实车调试流程

### 建议顺序

```
P0: 几何参数 → 用卷尺实测，填入 robot_params.h
      ↓
P1: 里程计标定 → 直线跑 10m，对比编码器累积距离
      ↓
P2: EKF 调优 → 从保守 Q/R 开始，观察走廊跟踪稳定性
      ↓
P3: 控制器调参 → 先调 kp_theta/kd_theta (航向)，再调 kp_y (横向)
      ↓
P4: 安全阈值 → 根据实际场地微调停车/减速距离
```

### 常见问题诊断

| 现象 | 可能原因 | 调整方法 |
|------|----------|----------|
| 车头左右摆动 | kp_theta 过大 或 kd_theta 过小 | 减小 kp_theta 或增大 kd_theta |
| 横向纠偏太慢 | kp_y 过小 | 增大 kp_y |
| 到端刹不住 | d_front_stop 过小 或 approach 区间太短 | 增大 PARAM_SAFE_D_FRONT_STOP |
| 总是急停 | EKF 置信度低，可能是 VL53 数据不稳定 | 增大 R (降低观测信任) 或检查传感器接线 |
| EKF 发散 | Q 过小 (不信观测) 或 R 过小 (过信噪声数据) | 增大 Q 或增大 R |
| 单侧靠墙走 | PARAM_VL53_SIDE_INSET 未校准 | 实测传感器内缩距离并填入 |

---

## 17. 文件快速索引

| 你想做什么 | 去看哪个文件 |
|-----------|-------------|
| 改任何调参数值 | `App/robot_params.h` |
| 理解系统初始化 | `App/app_tasks.c` → `AppTasks_Init()` |
| 理解导航流水线 | `App/app_tasks.c` → `AppTasks_RunNavTask_Impl()` |
| 看 EKF 数学 | `App/est/corridor_ekf.c` |
| 看控制律 | `App/nav/corridor_ctrl.c` |
| 看安全逻辑 | `App/nav/segment_fsm.c` |
| 看比赛编排 | `App/nav/nav_script.c` |
| 看传感器预处理 | `App/preproc/corridor_preproc.c` |
| 看 CAN 协议 | `App/Can/snc_can_app.c` + `App/Contract/chassis_can_msg.h` |
| 看全局数据结构 | `App/Contract/robot_blackboard.c/.h` |
| 看里程计 | `App/Contract/robot_odom.c` |
| 看激光驱动 | `App/laser/laser_manager.c` |
| 看 VL53 驱动 | `App/VL53L0X_API/platform/vl53_board.c` |
| 看 IMU 驱动 | `App/IMU/hwt101.c` |
| 看消息结构定义 | `App/preproc/corridor_msgs.h` |

---

> **给接手者的最后提醒**:
> 1. **先修 BUG-1** (IMU 单位转换)，否则 EKF 完全不可用
> 2. **再修 BUG-2** (EKF 参数硬编码)，否则调参改了白改
> 3. 拿到实车后第一件事：用卷尺量 CAL-1 ~ CAL-3 的三个偏移量
> 4. CAN 协议层 (`snc_can_app.c`) 已冻结，**不要修改**
> 5. IMU `yaw_continuous` 已接入转弯判定，实车调试时对比 `wz` 积分与 `yaw` 哪个更稳，参见 `IMU_YAW_WZ_ISSUE.md`