diff --git a/Doc/HANDOFF_v2.md b/Doc/HANDOFF_v2.md new file mode 100644 index 0000000..b26c5f1 --- /dev/null +++ b/Doc/HANDOFF_v2.md @@ -0,0 +1,815 @@ +# ARES 项目交接文档 v2 + +> **版本**: v2.1 — 连接段三信号联合判定 +> **基于**: v2.0 + 赛道几何理解修正 + 连接段判定算法升级 +> **更新内容**: 修正地图几何理解、修复入场/连接段传感器误用 bug、升级连接段为三信号联合判定 +> **本文目标**: 完整描述当前代码库状态，让任何人能在 30 分钟内理解并继续工作 + +--- + +## 目录 + +1. [一句话概述](#1-一句话概述) +2. [与 v1.0 的核心差异](#2-与-v10-的核心差异) +3. [代码目录结构（当前）](#3-代码目录结构当前) +4. [软件架构总览](#4-软件架构总览) +5. [数据流水线（当前）](#5-数据流水线当前) +6. [赛道级导航状态机详解](#6-赛道级导航状态机详解) +7. [安全层改造详解](#7-安全层改造详解) +8. [地图模块详解](#8-地图模块详解) +9. [EKF 重置机制](#9-ekf-重置机制) +10. [编译开关与模式切换](#10-编译开关与模式切换) +11. [全部可调参数](#11-全部可调参数) +12. [FreeRTOS 任务一览](#12-freertos-任务一览) +13. [文件快速索引](#13-文件快速索引) +14. [已知问题与待办](#14-已知问题与待办) +15. [实车调试建议](#15-实车调试建议) + +--- + +## 1. 一句话概述 + +在 v1.0 的单垄沟闭环基础上，新增了**赛道级混合导航系统**：用固定地图描述 S 型遍历拓扑，用 11 状态的 `GlobalNav` 状态机管理 6 条垄沟的完整遍历，用改造后的安全层支持"动作语义感知"，最终实现从启动区入场 → 遍历全部 6 条垄沟 → 出场回停的完整比赛流程。 + +--- + +## 2. 与 v1.0 的核心差异 + +### 2.1 新增能力 + +| 能力 | v1.0 | v2.0 | +|------|------|------| +| 单垄沟往返 | ✅ | ✅ | +| 到端检测 | ✅ | ✅ | +| 6 条垄沟 S 型遍历 | ❌ | ✅ | +| 90° 转向（入/出沟） | ❌ | ✅ | +| 连接段三信号联合判定 | ❌ | ✅ | +| 入沟重捕获确认 | ❌ | ✅ | +| 赛道级状态机 | ❌ | ✅ | +| 安全层动作语义感知 | ❌ | ✅ | +| 每次入沟 EKF 重置 | ❌ | ✅ | +| 出场与回停启动区 | ❌ | ✅ | + +### 2.2 已解决的已知问题 + +| 编号 | 问题 | 解决方式 | +|------|------|---------| +| **RISK-1** | 转向阶段安全层卡死 (`v=0, w≠0` 被前向防撞清零) | 新增 `SAFETY_MODE_TURN`，转向时直接放行 | + +### 2.3 没有动的部分 + +以下模块**完全未修改**，行为与 v1.0 一致： + +- `corridor_ekf.c/.h` — EKF 数学核心 +- `corridor_preproc.c/.h` — 传感器预处理 +- `corridor_ctrl.c/.h` — 沟内 PD 控制器 +- `nav_script.c/.h` — 保留，通过编译开关可切换回单沟测试 +- `snc_can_app.c/.h` — CAN 协议层（已冻结） +- 所有传感器驱动（VL53、激光、IMU） +- 黑板、里程计、指令槽 + +--- + +## 3. 代码目录结构（当前） + +``` +D:\ARES\ +├── App/ +│ ├── robot_params.h ← ★ 全局参数配置（含新增 P6 赛道级参数） +│ ├── app_tasks.c/.h ← 导航流水线（已改造，#if USE_GLOBAL_NAV 切换） +│ │ +│ ├── nav/ ← 导航与控制 +│ │ ├── global_nav.c/.h ← ★★ 新增：赛道级总控（11状态机） +│ │ ├── track_map.c/.h ← ★★ 新增：S型遍历地图 +│ │ ├── corridor_ctrl.c/.h ← 沟内 PD 控制器（未改） +│ │ ├── segment_fsm.c/.h ← 安全状态机（已改造：SafetyMode） +│ │ └── nav_script.c/.h ← 单沟测试脚本（保留，未改） +│ │ +│ ├── est/ +│ │ ├── corridor_filter.c/.h ← 已改造：新增 CorridorFilter_Reset() +│ │ └── corridor_ekf.c/.h ← EKF 核心（未改） +│ │ +│ ├── preproc/ +│ │ ├── corridor_msgs.h ← 已改造：新增 SafetyMode_t 枚举 +│ │ └── corridor_preproc.c/.h ← 传感器预处理（未改） +│ │ +│ ├── Contract/ ← 数据契约（未改） +│ ├── Can/ ← CAN 协议（已冻结，未改） +│ ├── IMU/ ← IMU 驱动（未改） +│ ├── laser/ ← 激光驱动（未改） +│ └── VL53L0X_API/ ← ToF 驱动（未改） +│ +├── Doc/ +│ ├── HANDOFF.md ← v1.0 交接文档 +│ ├── 混合导航方案.md ← 方案设计文档 +│ ├── 实施方案.md ← 详细实施文档 +│ ├── HANDOFF_v2.md ← ★ 本文档（v2.0） +│ └── map.md ← 赛道地图 +│ +├── build/Debug/ARES.elf ← 当前可用固件（编译通过） +└── CMakeLists.txt +``` + +--- + +## 4. 软件架构总览 + +``` +┌───────────────────────────────────────────────────────────────────────────┐ +│ FreeRTOS 任务层 │ +│ canTxTask monitorTask navTask laserTestTask vl53Task imuTask │ +│ (20ms) (100ms) (20ms) (50ms) (100ms) (10ms) │ +└──────────────────────────┬────────────────────────────────────────────────┘ + │ + navTask 内每 20ms 执行一次导航流水线 + │ +┌──────────────────────────▼────────────────────────────────────────────────┐ +│ Blackboard (全局传感器数据黑板) │ +└──────────────────────────┬────────────────────────────────────────────────┘ + │ GetSnapshot() +┌──────────────────────────▼────────────────────────────────────────────────┐ +│ CorridorPreproc (传感器清洗 → CorridorObs_t) │ +└──────────────────────────┬────────────────────────────────────────────────┘ + │ +┌──────────────────────────▼────────────────────────────────────────────────┐ +│ CorridorFilter / EKF (状态估计 → CorridorState_t {e_y,e_th,s}) │ +└──────────────────────────┬────────────────────────────────────────────────┘ + │ + ┌─────────────────┴──────────────────────┐ + │ USE_GLOBAL_NAV=1 (赛道模式) │ USE_GLOBAL_NAV=0 (测试模式) + ▼ ▼ +┌─────────────────────┐ ┌──────────────────────┐ +│ GlobalNav │ │ NavScript │ +│ (赛道级状态机) │ │ (单沟往返脚本) │ +│ 11 个阶段 │ │ IDLE→CORRIDOR→EXIT │ +│ ↕ TrackMap │ └──────────────────────┘ +└────────┬────────────┘ + │ nav_out.safety_mode + │ nav_out.request_corridor / use_override + ▼ +┌─────────────────────┐ ┌────────────────────┐ +│ CorridorCtrl │◄────────│ (request_corridor) │ +│ (沟内 PD 控制) │ └────────────────────┘ +└────────┬────────────┘ + │ RawCmd_t + ▼ +┌─────────────────────────────────────────────────────────┐ +│ SegFsm (安全仲裁) │ +│ 感知 SafetyMode → 动作语义相关安全策略 │ +│ CORRIDOR: 前向减速/停车/E-STOP 全开 │ +│ TURN: 直接放行 (允许 v=0, w≠0) │ +│ STRAIGHT: 仅前向防撞，不检查 conf │ +│ IDLE: 全部清零 │ +└────────┬────────────────────────────────────────────────┘ + │ SegFsmOutput_t {safe_v, safe_w} + ▼ + CmdSlot_Push → canTxTask → CAN 0x100 → STM32F407 底盘 +``` + +--- + +## 5. 数据流水线（当前） + +`navTask` 每 20ms 执行，共 7 步： + +``` +Step 1: Blackboard_GetSnapshot(&board) + │ 原子快照，无撕裂 + │ +Step 2: CorridorPreproc_ExtractObs(&board, now_ms, &obs) + │ VL53: mm→m，范围校验 + │ STP+ATK: 互补融合 + │ → CorridorObs_t {d_lf/d_lr/d_rf/d_rr, d_front, d_back, valid_mask} + │ +Step 3: CorridorFilter_Update(&obs, imu_wz, odom_vx, dt, yaw_rad, yaw_ok, &state) + │ EKF 预测 + 侧墙观测更新 + IMU 航向约束 + │ → CorridorState_t {e_y, e_th, s, conf} + │ +Step 4: GlobalNav_Update(&obs, &state, &board, &nav_out) 【赛道模式】 + │ 赛道级状态机推进 + │ → GlobalNavOutput_t {stage, safety_mode, request_corridor, override_v/w} + │ +Step 5: if (nav_out.request_corridor) + │ CorridorCtrl_Compute(&state, &obs, imu_wz, &raw_cmd) ← 沟内 PD + │ else + │ raw_cmd = {override_v, override_w} ← 上层覆盖 + │ +Step 6: SegFsm_Update(&raw_cmd, &obs, &state, nav_out.safety_mode, &fsm_out) + │ 感知 SafetyMode 的安全仲裁 + │ → SegFsmOutput_t {safe_v, safe_w} + │ +Step 7: CmdSlot_Push(fsm_out.safe_v, fsm_out.safe_w, 0) + → canTxTask 取走 → CAN 0x100 +``` + +--- + +## 6. 赛道级导航状态机详解 + +### 6.1 文件 + +| 文件 | 内容 | +|------|------| +| `App/nav/global_nav.h` | 枚举、配置结构、输出结构、API 声明 | +| `App/nav/global_nav.c` | 完整状态机实现（596 行） | + +### 6.2 状态枚举 + +```c +typedef enum { + GNAV_IDLE = 0, // 启动区等待 + GNAV_ENTRY_STRAIGHT, // 入场直线 + GNAV_TURN_INTO_CORRIDOR, // 第一次入沟转向 (90°) + GNAV_REACQUIRE, // 重捕获走廊 + GNAV_CORRIDOR_TRACK, // 沟内闭环跟踪 + GNAV_TURN_OUT_OF_CORRIDOR, // 出沟转向 (90°) + GNAV_LINK_STRAIGHT, // 连接段直行 + GNAV_TURN_INTO_NEXT, // 入下一条沟转向 (90°) + GNAV_EXIT_STRAIGHT, // 出场直行 + GNAV_DOCK, // 回停启动区 + GNAV_FINISHED, // 终态 + GNAV_ERROR // 异常态 (超时兜底) +} GlobalNavStage_t; +``` + +### 6.3 完整状态转移图 + +``` + GlobalNav_Start() + │ + ▼ + GNAV_IDLE（启动区等待） + │ 第一个 Update 周期自动 + ▼ + GNAV_ENTRY_STRAIGHT（入场直线） ──── [里程≥0.30m 或 超时10s] + │ (不用VL53：左侧围栏始终有效会误触发) + ▼ + GNAV_TURN_INTO_CORRIDOR（第一次入沟转向） ── [IMU 转角 ≥ 85°] + │ + ▼ + GNAV_REACQUIRE（重捕获走廊） ─────── [3+传感器有效 + 宽度匹配 + conf≥0.6 持续5拍] + │ [超时5s → ERROR] + ▼ + GNAV_CORRIDOR_TRACK（沟内闭环跟踪） ───── [d_front≤0.10m + 里程>1.0m] + │ [里程>2.5m 超长保护] + │ + ┌──────────┘ + ▼ + GNAV_TURN_OUT_OF_CORRIDOR（出沟转向） ────────────── [IMU 转角 ≥ 85°] + │ + ├── [非最后一条沟] + │ │ + │ ▼ + │ GNAV_LINK_STRAIGHT（连接段直行） ──────── [B: 前激光变化≥0.595m] + │ │ [或 (A: 里程≥0.595m) AND (C: VL53沟口检测连续2拍)] + │ │ [超时8s → ERROR] + │ ▼ + │ GNAV_TURN_INTO_NEXT（入下一条沟转向） ──────── [IMU 转角 ≥ 85°] + │ │ + │ └─────────────────── → GNAV_REACQUIRE (循环) + │ + └── [最后一条沟 (C6)] + │ + ▼ + GNAV_EXIT_STRAIGHT（出场直行） ─────────── [侧向VL53全丢 + 冲刺≥1.5m] + │ [里程>4.5m 或 超时30s → DOCK] + ▼ + GNAV_DOCK（回停启动区） ──────────────── [里程≥0.5m 或 超时5s] + │ + ▼ + GNAV_FINISHED（终态） ──────────── 终态，停车 + +任意阶段超时 → GNAV_ERROR（异常态） → 2s 后 → GNAV_FINISHED（终态） +``` + +### 6.4 各状态行为速查表 + +| 状态 | v (m/s) | w (rad/s) | 安全模式 | 传感器依赖 | 退出条件 | +|------|---------|-----------|---------|-----------|---------| +| IDLE（启动区等待） | 0 | 0 | IDLE | — | Start() 触发 | +| ENTRY_STRAIGHT（入场直线） | 0.08 | P保直 | STRAIGHT | IMU航向+前激光 | 里程≥0.30m 或 超时 | +| TURN_INTO_CORRIDOR（第一次入沟转向） | 0 | ±1.0 | TURN | IMU yaw | 转角≥85° | +| REACQUIRE（重捕获走廊） | 0.05 | P保直 | STRAIGHT | VL53+EKF | 双侧锁定持续5拍 | +| CORRIDOR_TRACK（沟内闭环跟踪） | 0.15 (PD) | PD | CORRIDOR | VL53+IMU+前激光 | d_front≤0.10m+里程>1m | +| TURN_OUT（出沟转向） | 0 | ±1.0 | TURN | IMU yaw | 转角≥85° | +| LINK_STRAIGHT（连接段直行） | 0.10 | P保直 | STRAIGHT | 前激光+里程+非围栏侧VL53 | 三信号联合判定(见6.8) | +| TURN_INTO_NEXT（入下一条沟转向） | 0 | ±1.0 | TURN | IMU yaw | 转角≥85° | +| EXIT_STRAIGHT（出场直行） | 0.15 | P保直 | STRAIGHT | IMU+里程+VL53全丢 | VL53全丢+冲刺1.5m | +| DOCK（回停启动区） | 0.05 | 0 | STRAIGHT | 里程计 | 里程到 | +| FINISHED（终态） | 0 | 0 | IDLE | — | 终态 | +| ERROR（异常态） | 0 | 0 | IDLE | — | 2s后→FINISHED | + +### 6.5 S 型遍历中的转向方向 + +场地结构关键理解： +- 垄沟沿 **X 轴（横向）** 分布，长 220cm，宽 40cm +- 左右两端各有一条 **纵向端部通道**（宽 40cm，长 390cm） +- 启动区在左下角，入口对齐左端通道 +- C1 在最南端（离入口最近），C6 在最北端 +- 机器人从启动区向北进入左端通道，入场距离仅约 10~30cm 即到 C1 入口 + +端部通道传感器特点： +- **一侧贴围栏**：VL53 能测到（~20cm） +- **另一侧交替出现垄背端面和垄沟开口**：垄背端面处能测到，垄沟开口处测不到（开口通向 220cm 远的对端，远超 VL53 的 1.2m 有效距离） +- 因此端部通道内**不能依赖 EKF**，必须用 IMU 航向保持 + 前/后激光到端检测 + +从地图推导的 S 型转向规律（北 = Y 递减 = 地图上方）： + +| 垄沟 | 行驶方向 | 入沟转向(从纵向通道) | 到端后出沟转向 | 连接段方向 | +|------|---------|-------------------|-------------|-----------| +| C1 | →东 | 右转(CW) | 左转(CCW) | 北行(右端通道) | +| C2 | ←西 | 左转(CCW) | 右转(CW) | 北行(左端通道) | +| C3 | →东 | 右转(CW) | 左转(CCW) | 北行(右端通道) | +| C4 | ←西 | 左转(CCW) | 右转(CW) | 北行(左端通道) | +| C5 | →东 | 右转(CW) | 左转(CCW) | 北行(右端通道) | +| C6 | ←西 | 左转(CCW) | **左转(CCW)→朝南** | **南行出场** | + +> **C6 出沟左转说明**: C6 向西走到左端通道，前激光测到左围栏。此时需要朝南回到入口出场，从朝西左转 90° 正好朝南。 + +### 6.6 转向执行细节 + +三种转向状态（TURN_INTO_CORRIDOR、TURN_OUT、TURN_INTO_NEXT）共用同一套 `execute_turn()` 逻辑： + +```c +// 已转过的角度 = (当前yaw - 起始yaw) × 方向符号(±1) +float delta = (imu_yaw - turn_start_yaw) * turn_sign; + +// 减速区: 剩余角度 < 28.6° 时开始线性减速 +float omega = turn_omega; // 默认 1.0 rad/s +if (remaining_deg < decel_zone_deg) { + omega = turn_min_omega + ratio * (turn_omega - turn_min_omega); +} +// 最低角速度 0.3 rad/s，防止接近目标时停转 + +// 完成判定: 已转 ≥ 85° (容差 5°) +if (delta >= 90° - 5°) → 转移到下一状态 +``` + +### 6.7 重捕获判据 + +进入新垄沟后，`GNAV_REACQUIRE` 阶段低速前进，同时检查三个条件： + +1. **至少 3 个侧向传感器有效**（4 个 VL53 中至少 3 个返回合法距离） +2. **几何宽度匹配**：若左右两侧都有效，则 `d_left + d_right + 车宽` 与走廊标称宽度 40cm 的误差 ≤ 5cm +3. **EKF 置信度 ≥ 0.6** + +三个条件同时满足，连续 5 拍（100ms），才认为重捕获成功，切换到 `GNAV_CORRIDOR_TRACK`。 + +### 6.8 连接段三信号联合判定（核心新设计） + +#### 场景描述 + +走完垄沟后，机器人在端部纵向通道里面朝北行驶，目标是走 70cm 到达下一条垄沟的入口。这 70cm 等于一条垄沟宽（40cm）加一条垄背宽（30cm）。 + +端部通道里的传感器情况： + +``` + 围栏侧（VL53始终有效，不参与判定） + │ + ┌───────┤ ┌── 垄背端面（VL53有效 ~10cm） + │ 车 │ │ + │ │ ─────► │ ← 非围栏侧 + │ │ │ + └───────┤ └── 垄沟开口（VL53无效 / >1.2m） + │ + 围栏侧 + + 前激光 ↑ 测到上围栏（距离随北行递减） + 后激光 ↓ 测到下围栏（距离随北行递增） +``` + +非围栏侧 VL53 的信号特征： +- 经过垄背端面时：读数 ≈ `(通道宽 - 车宽) / 2 - VL53内缩` = `(40-20)/2 - 0 = 10cm` → **有效** +- 进入垄沟开口时：射线穿入沟内（沟长 220cm + 对端通道 40cm ≈ 260cm）→ **超出 VL53 有效距离 1.2m** → 返回无效或 0 + +当车身中心对准垄沟入口时，前后两颗 VL53（间距 12cm）都已进入开口区域（沟口宽 40cm），两颗均应丢失。 + +#### 三个信号定义 + +| 信号 | 来源 | 触发条件 | 精度 | 权重 | +|------|------|---------|------|------| +| **A 里程计** | `odom_distance_accum` | `odom >= 0.70m × 0.85 = 0.595m` | 低（打滑） | 低 | +| **B 前激光变化** | `d_front_start - d_front_now` | 变化量 `>= 0.70m × 0.85 = 0.595m` | 高（绝对距离） | 高 | +| **C VL53 沟口检测** | 非围栏侧前/后 VL53 | 读数 `> 0.5m` 或无效，连续 **2 拍**（40ms）| 中（直接探沟口） | 中 | + +> **阈值 0.5m 的来源**：正常贴垄背端面时 VL53 读数 ≈ 10cm，进入沟口后 >120cm（超出量程）。0.5m 在两者正中间，充分区分。 + +> **0.85 容差系数**：为里程计打滑和激光安装偏置留 15% 裕量。 + +#### 触发逻辑 + +``` +B || (A && C) +``` + +- **B 单独成立** → 前激光变化量足够，直接触发（最可靠，不依赖其他） +- **A 且 C 同时成立** → 里程计粗定位 + VL53 精确探到沟口，联合校验后触发 + +#### 哪一侧是"非围栏侧" + +由**刚走完的那条沟的行驶方向**决定，在 `LINK_STRAIGHT` 阶段 `current_corridor_id` 还未更新： + +| 刚走完的沟 | 到达哪个通道 | 围栏侧 | **非围栏侧（检查这侧）** | +|-----------|------------|--------|----------------------| +| `TRAVEL_DIR_EAST`（向东） | 右端通道 | 右侧 | **左侧 VL53** | +| `TRAVEL_DIR_WEST`（向西） | 左端通道 | 左侧 | **右侧 VL53** | + +#### 关键代码（`global_nav.c`） + +```c +/* 信号 B: 前激光变化量 */ +float d_front_delta = s_nav.link_d_front_start - obs->d_front; +bool laser_ok = (d_front_delta >= s_nav.cfg.link_distance * 0.85f); + +/* 信号 A: 里程计 */ +bool odom_ok = odom_since_entry() >= s_nav.cfg.link_distance * 0.85f; + +/* 信号 C: 非围栏侧 VL53 沟口检测（连续2拍）*/ +bool gap_now = gap_detected_on_open_side(obs, cd->travel_dir); // cd = 当前沟 +if (gap_now) s_nav.link_gap_count++; +else s_nav.link_gap_count = 0; +bool gap_confirmed = (s_nav.link_gap_count >= 2); + +/* 联合判定 */ +if (laser_ok || (odom_ok && gap_confirmed)) + transition_to(GNAV_TURN_INTO_NEXT, board); +``` + +--- + +## 7. 安全层改造详解 + +### 7.1 改造动机 + +v1.0 的 `segment_fsm` 统一用前向距离判定安全策略，导致**转向阶段被卡死**（RISK-1）： +- 到端后 `d_front` 很近，`SegFsm` 进入 STOP 状态 +- 此时脚本输出 `v=0, w≠0` 期望原地转向 +- 但 STOP 状态将 `safe_w` 也清零 → 转不动 + +### 7.2 改造方案 + +新增 `SafetyMode_t` 枚举作为安全层的"动作语义输入"： + +```c +// corridor_msgs.h +typedef enum { + SAFETY_MODE_IDLE, // 零速，不做任何裁剪 + SAFETY_MODE_CORRIDOR, // 完整安全检查（前向减速/停车/E-STOP） + SAFETY_MODE_TURN, // 转向：直接放行，不检查前距和置信度 + SAFETY_MODE_STRAIGHT // 直行：仅前向防撞，不检查 conf +} SafetyMode_t; +``` + +### 7.3 各模式行为 + +| SafetyMode | 前向减速/停车 | E-STOP (conf<0.1) | w 保留 | +|------------|-------------|------------------|--------| +| IDLE | ❌ | ❌ | ❌ (全清零) | +| CORRIDOR | ✅ | ✅ | ✅ APPROACH 时保留 | +| **TURN** | **❌** | **❌** | **✅ 完全放行** | +| STRAIGHT | ✅ | ❌ | ✅ APPROACH 时保留 | + +### 7.4 函数签名变化 + +```c +// v1.0 (旧) +void SegFsm_Update(const RawCmd_t *raw_cmd, + const CorridorObs_t *obs, + const CorridorState_t *state, + SegFsmOutput_t *out); + +// v2.0 (新) — 增加 SafetyMode_t mode 参数 +void SegFsm_Update(const RawCmd_t *raw_cmd, + const CorridorObs_t *obs, + const CorridorState_t *state, + SafetyMode_t mode, // ← 新增 + SegFsmOutput_t *out); +``` + +> **兼容性**: 旧的 `nav_script` 调用路径（`USE_GLOBAL_NAV=0` 时）传入 `SAFETY_MODE_CORRIDOR`，行为与 v1.0 完全一致。 + +--- + +## 8. 地图模块详解 + +### 8.1 文件 + +| 文件 | 内容 | +|------|------| +| `App/nav/track_map.h` | 数据结构 + API 声明 | +| `App/nav/track_map.c` | 硬编码 S 型遍历表 + 查询实现 | + +### 8.2 设计原则 + +地图**不做全局坐标**，只回答三个问题： +1. 从第 N 条沟完成后，下一条是第几条？ +2. 这次该往哪转？（左/右） +3. 当前是不是最后一条沟？ + +### 8.3 核心数据 + +```c +// 单条垄沟描述 +typedef struct { + uint8_t id; // 0-5 + TravelDirection_t travel_dir; // EAST(→) 或 WEST(←) + TurnDirection_t exit_turn_dir; // 出沟转向方向 + TurnDirection_t entry_turn_dir; // 入沟转向方向(从纵向通道转入) + bool is_last; // 是否为最后一条 +} CorridorDescriptor_t; +``` + +硬编码的遍历表（`track_map.c`）： + +``` +C0(C1): →东 entry:右转 exit:左转 is_last:false +C1(C2): ←西 entry:左转 exit:右转 is_last:false +C2(C3): →东 entry:右转 exit:左转 is_last:false +C3(C4): ←西 entry:左转 exit:右转 is_last:false +C4(C5): →东 entry:右转 exit:左转 is_last:false +C5(C6): ←西 entry:左转 exit:左转 is_last:true ← 最后一条，左转朝南出场 +``` + +### 8.4 API + +```c +void TrackMap_Init(void); +const TrackMap_t* TrackMap_Get(void); +const CorridorDescriptor_t* TrackMap_GetCorridor(uint8_t id); +uint8_t TrackMap_GetNextCorridorId(uint8_t current_id); +bool TrackMap_IsLastCorridor(uint8_t id); +TurnDirection_t TrackMap_GetExitTurnDir(uint8_t id); +TurnDirection_t TrackMap_GetEntryTurnDir(uint8_t id); +``` + +--- + +## 9. EKF 重置机制 + +### 9.1 为什么每次入沟都要重置 + +不同垄沟的 `e_y` 参考基准不同（每条沟的"居中"对应的 VL53 读数不同）。如果不重置，EKF 会用上一条沟的历史协方差和参考值去更新新沟的观测，导致： +- 初始几拍置信度虚高（P 矩阵太小） +- 横向偏差 `e_y` 带着上条沟的偏置 + +### 9.2 实现 + +```c +// corridor_filter.h — 新增接口 +void CorridorFilter_Reset(void); + +// corridor_filter.c — 实现 +void CorridorFilter_Reset(void) { + if (!s_initialized) return; + CorridorEKF_Reset(); // EKF 状态归零，P 恢复初始值 + s_imu_yaw_ref_rad = 0.0f; // 解锁 yaw_ref + s_imu_yaw_ref_set = false; // 等待在新沟中重新锁定 +} +``` + +### 9.3 调用时机 + +`GlobalNav` 在 `transition_to(GNAV_REACQUIRE, ...)` 时调用，即每次转向完成后、开始寻找新走廊之前。 + +--- + +## 10. 编译开关与模式切换 + +### 10.1 控制宏 + +```c +// App/robot_params.h +#define USE_GLOBAL_NAV 1 // 1=赛道模式 0=单沟测试模式 +``` + +### 10.2 两种模式的差异 + +| | `USE_GLOBAL_NAV=1` | `USE_GLOBAL_NAV=0` | +|-|-------------------|-------------------| +| 导航模块 | `GlobalNav` | `NavScript` | +| 启动调用 | `GlobalNav_Start()` | `NavScript_Start()` | +| 遍历范围 | 6 条垄沟 S 型 | 单垄沟往返 | +| 安全模式 | 按阶段动态切换 | 固定 `SAFETY_MODE_CORRIDOR` | +| 适用场景 | 正式比赛 | 单条走廊调试 | + +### 10.3 切换方法 + +仅修改 `robot_params.h` 中的宏，**重新编译烧录**即可。所有逻辑通过 `#if USE_GLOBAL_NAV` 在 `app_tasks.c` 中编译时切换，不增加运行时开销。 + +--- + +## 11. 全部可调参数 + +### P0 — 几何参数（实测填写） + +| 参数 | 当前值 | 说明 | +|------|--------|------| +| `PARAM_ROBOT_WIDTH` | 0.200 m | 车体宽度 | +| `PARAM_ROBOT_LENGTH` | 0.200 m | 车体长度 | +| `PARAM_WHEEL_DIAMETER` | 0.080 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） | +| `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 | 最大线速度 | +| `PARAM_CTRL_SPEED_REDUCTION` | 0.4 | 弯道减速系数 | + +### 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 置信度阈值（CORRIDOR模式） | + +### P6 — 赛道级导航参数（新增，v2.0） + +| 参数 | 当前值 | 说明 | +|------|--------|------| +| `USE_GLOBAL_NAV` | 1 | 编译开关 | +| **入场段** | | | +| `PARAM_GNAV_ENTRY_V` | 0.08 m/s | 入场速度 | +| `PARAM_GNAV_ENTRY_DISTANCE` | 0.30 m | 入场里程上限 (启动区到C1入口仅约10~30cm) | +| `PARAM_GNAV_ENTRY_TIMEOUT` | 10000 ms | 入场超时 | +| **转向** | | | +| `PARAM_GNAV_TURN_OMEGA` | 1.0 rad/s | 转向角速度 | +| `PARAM_GNAV_TURN_TOLERANCE` | 0.087 rad (~5°) | 转向完成容差 | +| `PARAM_GNAV_TURN_DECEL_ZONE` | 0.5 rad (~28°) | 减速区起始角度 | +| `PARAM_GNAV_TURN_MIN_OMEGA` | 0.3 rad/s | 减速区最低角速度 | +| `PARAM_GNAV_TURN_TIMEOUT` | 8000 ms | 单次转向超时 | +| **重捕获** | | | +| `PARAM_GNAV_REACQUIRE_V` | 0.05 m/s | 重捕获速度 | +| `PARAM_GNAV_REACQUIRE_CONF` | 0.6 | 置信度阈值 | +| `PARAM_GNAV_REACQUIRE_WIDTH_TOL` | 0.05 m | 走廊宽度容差 | +| `PARAM_GNAV_REACQUIRE_TICKS` | 5 拍 | 连续确认次数 | +| `PARAM_GNAV_REACQUIRE_TIMEOUT` | 5000 ms | 重捕获超时 | +| **沟内** | | | +| `PARAM_GNAV_CORRIDOR_END_DIST` | 0.10 m | 到端检测距离 | +| `PARAM_GNAV_CORRIDOR_MAX_LEN` | 2.50 m | 沟内里程保护上限 | +| **连接段** | | | +| `PARAM_GNAV_LINK_V` | 0.10 m/s | 连接段速度 | +| `PARAM_GNAV_LINK_DISTANCE` | 0.70 m | 连接段标称距离 | +| `PARAM_GNAV_LINK_TIMEOUT` | 8000 ms | 连接段超时 | +| **出场** | | | +| `PARAM_GNAV_EXIT_V` | 0.15 m/s | 出场速度 | +| `PARAM_GNAV_EXIT_RUNOUT` | 1.50 m | 侧向丢失后冲刺距离 | +| `PARAM_GNAV_EXIT_MAX_DIST` | 4.50 m | 出场里程保护 (纵向通道全长约3.9m) | +| `PARAM_GNAV_EXIT_TIMEOUT` | 30000 ms | 出场超时 (纵向通道距离长，给足时间) | +| **回停** | | | +| `PARAM_GNAV_DOCK_V` | 0.05 m/s | 回停速度 | +| `PARAM_GNAV_DOCK_DISTANCE` | 0.50 m | 回停距离 | +| **其他** | | | +| `PARAM_GNAV_HEADING_KP` | 0.03 | 航向保持 P 增益（°→rad/s） | + +--- + +## 12. FreeRTOS 任务一览 + +与 v1.0 完全一致，无新增任务： + +| 任务名 | 周期 | 优先级 | 职责 | +|--------|------|--------|------| +| `canTxTask` | 20ms | AboveNormal | CAN 0x100 发送；100ms 看门狗 | +| `navTask` | 20ms | AboveNormal | **完整导航流水线（含新 GlobalNav）** | +| `LaserTsk`（内部） | 10ms | AboveNormal | 4 路 UART DMA 激光解析 | +| `monitorTask` | 100ms | Normal | CAN 健康；里程计积分 | +| `laserTestTask` | 50ms | Normal | 激光推送黑板 | +| `vl53Task` | 100ms | Normal | VL53L0X 读取推送黑板 | +| `imuTask` | 10ms | BelowNormal | IMU 解析推送黑板 | +| `defaultTask` | — | Normal | USB CDC 初始化；空闲循环 | + +--- + +## 13. 文件快速索引 + +| 你想做什么 | 去看哪个文件 | +|-----------|-------------| +| 改调参数值 | `App/robot_params.h` | +| 切换赛道/单沟模式 | `App/robot_params.h` → `USE_GLOBAL_NAV` | +| 看赛道状态机逻辑 | `App/nav/global_nav.c` | +| 看 S 型遍历拓扑表 | `App/nav/track_map.c` | +| 看安全模式定义 | `App/preproc/corridor_msgs.h` → `SafetyMode_t` | +| 看安全层实现 | `App/nav/segment_fsm.c` | +| 看沟内控制律 | `App/nav/corridor_ctrl.c` | +| 看 EKF 数学 | `App/est/corridor_ekf.c` | +| 看 EKF 重置 | `App/est/corridor_filter.c` → `CorridorFilter_Reset()` | +| 看导航流水线入口 | `App/app_tasks.c` → `AppTasks_RunNavTask_Impl()` | +| 看系统初始化 | `App/app_tasks.c` → `AppTasks_Init()` | +| 看 CAN 协议 | `App/Can/snc_can_app.c` | +| 看全局数据结构 | `App/Contract/robot_blackboard.h` | +| 看传感器预处理 | `App/preproc/corridor_preproc.c` | +| 看赛道地图 | `Doc/map.md` | +| 看单沟测试脚本 | `App/nav/nav_script.c` | + +--- + +## 14. 已知问题与待办 + +### 已在 v2.0 解决 + +| # | 问题 | 解决方式 | +|---|------|---------| +| **RISK-1** | 转向阶段被安全层卡死 | `SAFETY_MODE_TURN` 直接放行 | +| — | 缺少 6 沟遍历能力 | `global_nav.c` 完整实现 | +| — | 缺少 EKF 入沟重置 | `CorridorFilter_Reset()` | + +### v1.0 遗留的已解决 BUG（已全部修复） + +BUG-1 至 BUG-9 均在 v1.0 已修复，v2.0 继承已修复状态。详见 `HANDOFF.md`。 + +### 当前待办 + +| # | 问题 | 优先级 | 说明 | +|---|------|--------|------| +| **CAL-1** | `PARAM_FRONT_LASER_OFFSET = 0.0` | 高 | 实测填写，影响到端检测精度 | +| **CAL-2** | `PARAM_REAR_LASER_OFFSET = 0.0` | 高 | 同上 | +| **CAL-3** | `PARAM_VL53_SIDE_INSET = 0.0` | 高 | 单侧退化时影响 EKF 精度 | +| **CAL-4** | `PARAM_IMU_YAW_OFFSET = 0.0` | 中 | 声明了但代码未使用 | +| **TODO-1** | `GlobalNav` 里程计时间源 | 中 | 当前用 `imu_wz.timestamp_ms` 估算时间，可改为 `HAL_GetTick()` 从外部传入，更准确 | +| **TODO-2** | 连接段过短/过长自适应 | 低 | 当前固定 0.70m，实际可能因转向角度偏差需要微调 | +| **TODO-3** | 重捕获单侧有效策略 | 低 | 若入沟角度大导致某侧 VL53 暂时无效，当前判定较严格 | + +### 设计风险（已知，待注意） + +| # | 风险 | 影响 | 说明 | +|---|------|------|------| +| **RISK-2** | 连接段偏航 | 中 | IMU 短时漂移 + 轮滑可能导致连接段走歪，靠里程计上限保护 | +| **RISK-3** | 转向精度 | 中 | IMU 积分漂移，90°实际转角可能有 2-5° 误差，重捕获阶段兜底 | +| **RISK-4** | 入场阶段判据 | 中 | "侧向 VL53 有效"作为进入第一条沟的触发，若传感器偶发有效可能提前转向 | + +--- + +## 15. 实车调试建议 + +### 建议调试顺序 + +``` +第1步: 几何参数实测 (P0) + → 卷尺量 PARAM_FRONT/REAR_LASER_OFFSET、PARAM_VL53_SIDE_INSET + +第2步: USE_GLOBAL_NAV=0，单沟测试 + → 验证走廊跟踪、到端检测、180°转向（nav_script 旧路径） + +第3步: 切换 USE_GLOBAL_NAV=1，测试单次 90° 转向 + → 手动把机器人放在沟内，触发 TURN_OUT，看 IMU 转角是否准确 + +第4步: 测试重捕获 + → 手动转向后进入 REACQUIRE，看 5 拍锁定时间，调 CONF 阈值 + +第5步: 测试双沟 S 型 (C1 → C2) + → 验证连接段长度、入沟对准 + +第6步: 全 6 沟测试 + → 观察每条沟的入沟质量、完成率 + +第7步: 调优参数 + → TURN_OMEGA, LINK_DISTANCE, REACQUIRE_TICKS 等 +``` + +### 常见问题诊断（新增 v2.0 相关） + +| 现象 | 可能原因 | 处置方法 | +|------|----------|---------| +| 转向后没进下一阶段 | IMU 转角计算偏差 | 检查 `imu_yaw_continuous` 是否正常更新；调小 `TURN_TOLERANCE` | +| 重捕获超时（5s） | 入沟角度偏大，VL53 看不到两侧 | 增大 `REACQUIRE_TIMEOUT`；或降低 `REACQUIRE_TICKS` | +| 连接段走过头 | 里程计打滑 | 适当减小 `LINK_DISTANCE`；靠 VL53 探壁辅助触发 | +| 连接段走不够 | 里程计未积分（odom=0） | 检查 monitorTask 里程计链路 | +| 安全层卡住转向 | `SAFETY_MODE_TURN` 未传入 | 检查 `GlobalNav_Update()` 的 `nav_out.safety_mode` 输出 | +| 出场后未停车 | VL53 未丢失触发 | 调小 `EXIT_RUNOUT`；检查 `all_side_lost()` 判定 | +| 每次入沟偏向一侧 | `PARAM_VL53_SIDE_INSET` 未校准 | 实测传感器内缩距离并填入 | + +--- + +> **给接手者的提醒**: +> 1. 调参前先确认 `USE_GLOBAL_NAV` 的模式，不同模式下调的参数组不同 +> 2. v2.0 新增的 P6 参数全部有默认值，实车前**必须根据实际场地微调** `LINK_DISTANCE` 和 `ENTRY_DISTANCE` +> 3. `CorridorFilter_Reset()` 在每次入沟时自动调用，**不需要手动干预** +> 4. 如果出现整体行为异常，先把 `USE_GLOBAL_NAV=0` 切回单沟模式定位问题 +> 5. CAN 协议层 (`snc_can_app.c`) 已冻结，**不要修改** diff --git a/Doc/SD日志方案.md b/Doc/SD日志方案.md new file mode 100644 index 0000000..afe58dc --- /dev/null +++ b/Doc/SD日志方案.md @@ -0,0 +1,391 @@ +# ARES SD 日志方案 + +## 1. 结论 + +对当前项目，最可行的一期方案是： + +- 用 `SDMMC1 + FatFs` 挂载 SD 卡。 +- 新增一个独立的 `logTask` 作为唯一写卡任务。 +- `navTask`、`monitorTask`、`imuTask` 等任务只负责把日志记录写入 RAM 队列，绝不直接 `f_write()`。 +- 日志格式采用“事件 + 低频快照”的文本 CSV，一期先保证稳定可读，不追求全量高频原始数据。 + +这个方案最适合当前仓库，因为： + +- 现有高频闭环在 `App/app_tasks.c` 的 `navTask` 中以 20ms 周期运行，不能被 SD 写卡阻塞。 +- 当前日志只有 `printf -> USB CDC`，没有 FatFs/SDMMC 基础设施。 +- 当前系统没有 RTC，日志只能基于 `HAL_GetTick()` 记录相对时间，文件名也不能依赖真实日期。 +- `CMakeLists.txt` 已经自动收集 `App/*.c`，新增 `App/log/*.c` 不需要手工改源文件列表。 + +## 2. 结合当前项目的约束 + +### 2.1 现有可直接复用的数据源 + +当前代码里已经有足够多的“运行状态”可用于日志： + +- 黑板快照：`App/Contract/robot_blackboard.h` +- 走廊观测：`CorridorObs_t`，定义在 `App/preproc/corridor_msgs.h` +- EKF 状态：`CorridorState_t`，定义在 `App/preproc/corridor_msgs.h` +- 赛道状态机输出：`GlobalNavOutput_t`，定义在 `App/nav/global_nav.h` +- 安全状态机输出：`SegFsmOutput_t`，定义在 `App/nav/segment_fsm.h` + +这意味着日志不需要额外造一套状态模型，直接复用现有结构即可。 + +### 2.2 当前不适合的做法 + +不建议直接把现在的 `printf` 改成“同时写 USB 和 SD”： + +- `printf` 现在走 `App/retarget.c` 的 `_write()`，底层是 USB CDC。 +- 如果把 `_write()` 直接改成写文件，会让所有任务都可能进入 FatFs，线程模型会变复杂。 +- 文本格式化和写卡延迟都可能拖慢闭环任务。 + +不建议一期就做“每 20ms 全量原始数据落盘”： + +- 对排障来说，状态转移、关键变量、故障瞬间比全量采样更有价值。 +- 高频写卡会更容易遇到 H7 的缓存一致性、FatFs 抖动、掉电损坏等问题。 + +## 3. 推荐总体架构 + +### 3.1 模块划分 + +建议新增目录： + +- `App/log/sd_log.h` +- `App/log/sd_log.c` +- `App/log/sd_log_port.c` 或 `App/log/sd_log_fs.c` + +职责划分： + +- `sd_log.c`：日志队列、记录封装、CSV 格式化、刷盘策略。 +- `sd_log_fs.c`：FatFs mount/open/write/sync/rotate。 +- `app_tasks.c`：在现有任务周期内调用 `SDLog_TryPush...()`。 + +### 3.2 任务模型 + +新增一个低优先级 `logTask`： + +- 优先级建议低于 `navTask` 和 `canTxTask` +- 周期不固定，阻塞等待日志队列 +- 它是系统里唯一允许调用 `f_mount/f_open/f_write/f_sync/f_close` 的任务 + +生产者任务： + +- `navTask`：产出导航快照、状态切换、故障事件 +- `monitorTask`：产出 CAN 在线状态、底盘诊断、里程计健康事件 +- 启动阶段：产出 boot 配置、固件版本、参数摘要 + +核心原则： + +- 生产者只入队，不阻塞 +- 队列满时允许丢弃低优先级快照，但不能卡住控制任务 +- 重要事件单独计数，后续在日志里补写 `drop_count` + +### 3.3 内存缓冲 + +一期建议用固定长度消息队列，而不是动态内存： + +- 例如 `osMessageQueue`，每条记录 192B 或 256B +- 队列深度 32 到 64 条 +- 总 RAM 占用约 8KB 到 16KB，可控 + +推荐记录结构： + +```c +typedef enum { + LOG_REC_BOOT = 0, + LOG_REC_HEALTH, + LOG_REC_NAV_SNAPSHOT, + LOG_REC_STAGE_EVENT, + LOG_REC_FAULT, +} LogRecordType_t; + +typedef struct { + uint32_t tick_ms; + uint16_t type; + uint16_t len; + char text[224]; +} LogRecord_t; +``` + +这里故意用“预格式化文本入队”，因为： + +- 一期更简单，便于直接在 SD 卡上看内容 +- `logTask` 只做批量写盘，不做复杂格式化 +- 后续如果需要高频日志，再把 `text` 改成二进制 payload + +## 4. 推荐日志内容 + +### 4.1 一期必须记录的内容 + +建议只做三类： + +1. 启动信息 +2. 周期性状态快照 +3. 异常/状态切换事件 + +### 4.2 启动信息 + +系统启动后创建文件，先写以下内容： + +- 固件名、编译日期时间 +- `USE_GLOBAL_NAV` 当前值 +- 关键参数摘要 +- SD 卡 mount 是否成功 +- 日志格式版本号 + +示例： + +```text +# ARES log v1 +# build=Apr 03 2026 12:00:00 +# mode=global_nav +# param.ctrl_v=0.20 +# param.safe_front_stop=0.10 +``` + +### 4.3 周期性状态快照 + +建议由 `navTask` 每 100ms 记录一次，不要每 20ms 都记。 + +推荐字段： + +- `tick_ms` +- `stage` / `stage_name` +- `seg_state` +- `request_corridor` +- `override_v` / `override_w` +- `safe_v` / `safe_w` +- `odom_vx` / `odom_wz` +- `e_y` / `e_th` / `s` / `conf` +- `d_front` / `d_back` +- `d_lf` / `d_lr` / `d_rf` / `d_rr` +- `valid_mask` +- `chassis_online` / `chassis_diag` + +推荐 CSV 行示例： + +```text +12340,SNAP,GNAV_CORRIDOR_TRACK,CORRIDOR,1,0.000,0.000,0.180,-0.120,0.182,0.01,-0.03,1.42,0.92,0.31,1.85,0.12,0.11,0.10,0.10,0x3F,1,0x00000000 +``` + +### 4.4 事件日志 + +以下情况必须立即记录，而不是等下一个 100ms 快照： + +- `GlobalNav` 阶段切换 +- `SegFsm` 状态切换 +- 进入 `GNAV_ERROR` +- 进入 `SEG_STATE_ESTOP` +- 底盘离线 / 恢复在线 +- SD 卡 mount 失败 / 写入失败 / 队列溢出 + +推荐示例： + +```text +12520,EVENT,GNAV_STAGE,GNAV_REACQUIRE->GNAV_CORRIDOR_TRACK +20840,EVENT,SEG_FSM,CORRIDOR->E-STOP,conf=0.18 +20900,FAULT,CHASSIS_OFFLINE,diag=0x00000004 +``` + +## 5. 文件组织与命名 + +### 5.1 文件命名 + +因为当前没有 RTC，不建议用日期命名。 + +推荐方式： + +- 目录：`/ARESLOG/` +- 文件名：`BOOT0001.CSV`、`BOOT0002.CSV`... + +创建流程： + +- 启动时扫描 `/ARESLOG` +- 找到最大序号 +- 新建下一个序号文件 + +### 5.2 文件轮转 + +建议单文件大小限制 2MB 到 4MB： + +- 超过阈值自动新建下一个文件 +- 文件轮转只在 `logTask` 内执行 + +按 100ms 一条快照估算： + +- 一条快照约 120B 到 220B +- 约 1.2KB/s 到 2.2KB/s +- 1 小时约 4MB 到 8MB + +所以 4MB 一卷是合理的。 + +## 6. 刷盘策略 + +### 6.1 建议策略 + +不要每写一行就 `f_sync()`，也不要一直不刷。 + +推荐折中： + +- RAM 累积 512B 或 1024B 后批量 `f_write()` +- 每 1000ms 做一次 `f_sync()` +- 重大故障事件后立即 `f_sync()` 一次 + +这样能兼顾三件事： + +- 降低 FatFs 抖动 +- 降低 SD 卡磨损 +- 掉电时最多丢失最近约 1 秒日志 + +### 6.2 关闭策略 + +如果系统有明确“任务结束/人工停机”流程，可在停机前调用： + +- `SDLog_Flush()` +- `SDLog_Close()` + +如果没有，就依赖周期性 `f_sync()`。 + +## 7. SDMMC1 硬件与驱动建议 + +### 7.1 一期推荐配置 + +建议先用最保守配置把链路跑通： + +- `SDMMC1` +- 1-bit 总线模式先 bring-up +- FatFs 先用默认配置 +- 先不用 DMA 优化 + +原因： + +- H7 上 SDMMC + DCache + DMA 的一致性问题很多 +- 一期日志吞吐量很低，1-bit 也足够 +- 先把 mount/open/write/read/sync 做稳定，比先追求 4-bit 吞吐更重要 + +稳定后再考虑： + +- 切到 4-bit 模式 +- 启用 DMA +- 做 32 字节对齐缓冲区和 cache clean/invalidate + +### 7.2 引脚建议 + +若 PCB 已引出标准 SDMMC1 引脚，可优先使用： + +- `PC8` `D0` +- `PC9` `D1` +- `PC10` `D2` +- `PC11` `D3` +- `PC12` `CK` +- `PD2` `CMD` + +从当前 `ARES.ioc` 看，这组引脚还没有被现有业务占用，但仍需要你结合原理图再确认一次。 + +### 7.3 热插拔建议 + +一期不建议做复杂热插拔。 + +建议： + +- 上电时检测并 mount +- 运行中默认认为卡一直在 +- 如果 `f_write()` 连续失败，则置 `sd_offline` 标志并停止写卡 +- 需要重新插卡时，用按键命令或调试命令手动 remount + +这样实现最小、风险最低。 + +## 8. 代码落点建议 + +### 8.1 CubeMX / 自动生成部分 + +需要改： + +- `ARES.ioc`：开启 `SDMMC1` +- `ARES.ioc`：开启 `FATFS` +- 生成对应 `sdmmc.c`、`fatfs.c`、中间件文件 + +### 8.2 手写业务代码 + +建议新增： + +- `App/log/sd_log.c` +- `App/log/sd_log.h` + +建议修改： + +- `Core/Src/freertos.c` + - 增加 `logTask` +- `App/app_tasks.c` + - `AppTasks_Init()` 中初始化日志模块 + - `AppTasks_RunNavTask_Impl()` 中每 100ms 打一次导航快照 + - `AppTasks_RunMonitorTask()` 中记录底盘在线/离线变化 +- `App/nav/global_nav.c` + - 在阶段切换点调用事件日志接口，或者暴露状态变化让 `navTask` 记录 +- `App/nav/segment_fsm.c` + - 在 `E-STOP/STOP/APPROACH` 切换时记录事件，或者暴露状态变化给上层记录 + +### 8.3 更推荐的接入方式 + +为了少改导航核心，我更推荐： + +- 在 `navTask` 内缓存“上一次 `stage` / `seg_state`” +- 若本周期发现变化，就从 `app_tasks.c` 发事件日志 + +这样可以减少对 `global_nav.c` 和 `segment_fsm.c` 的侵入。 + +## 9. 一期接口建议 + +建议暴露以下最小 API： + +```c +bool SDLog_Init(void); +bool SDLog_Start(void); +void SDLog_Task(void *argument); + +void SDLog_TryPushBoot(void); +void SDLog_TryPushHealth(uint32_t tick_ms, bool chassis_online, uint32_t diag); +void SDLog_TryPushNavSnapshot(uint32_t tick_ms, + const RobotBlackboard_t *board, + const CorridorObs_t *obs, + const CorridorState_t *state, + const GlobalNavOutput_t *gnav, + const SegFsmOutput_t *fsm); +void SDLog_TryPushEvent(const char *tag, const char *msg); +``` + +接口原则： + +- 全部 `TryPush` +- 失败直接返回，不阻塞调用方 +- 是否丢包由内部计数 + +## 10. 推荐实施顺序 + +建议按下面顺序推进，不要一步到位： + +1. 在 CubeMX 打开 `SDMMC1 + FatFs`，完成最小 mount/write/read 测试。 +2. 新建 `logTask`，只做固定字符串写卡，验证线程模型。 +3. 接入启动日志和健康日志。 +4. 接入 `navTask` 的 100ms 快照。 +5. 接入 `stage` 与 `seg_state` 变化事件。 +6. 最后再考虑 4-bit、DMA、热插拔、二进制高频日志。 + +## 11. 我对这个项目的最终建议 + +如果你的目标是“记录系统运行状况，便于复盘问题”，我建议你的一期目标定成： + +- 能稳定挂载 SD 卡 +- 能连续写 30 分钟以上不影响导航 +- 能在一份 CSV 里看见：导航阶段、安全状态、EKF 置信度、关键距离、底盘在线状态 +- 掉电最多损失 1 秒左右日志 + +不要把一期目标定成“全量原始数据黑匣子”。 + +对当前仓库，最小且正确的方案就是： + +- `SDMMC1 + FatFs` +- 单写者 `logTask` +- 100ms 状态快照 + 即时事件日志 +- CSV 文件 + 周期性 `f_sync()` + +这套方案实现量小，调试成本低，而且已经足够覆盖你现在最关心的“系统运行状况记录”。 diff --git a/Doc/code_review_report.md b/Doc/code_review_report.md new file mode 100644 index 0000000..7404e65 --- /dev/null +++ b/Doc/code_review_report.md @@ -0,0 +1,192 @@ +# 导航代码审查报告 + +日期: 2026-04-03 + +范围: +- `App/nav/global_nav.c` +- `App/nav/track_map.c` +- `App/nav/track_map.h` +- `App/robot_params.h` +- `App/app_tasks.c` +- `App/preproc/corridor_preproc.c` + +## 结论 + +当前版本的 S 型拓扑和左右转向表整体上与地图理解一致，没有再发现明显的左右方向写反问题。 + +主要风险集中在以下几类： +- 时间基准过度依赖 IMU 时间戳 +- 连续确认逻辑复用了同一帧 VL53 数据 +- 入场段过度依赖固定起始摆放位置 +- 阶段切换存在 1 个控制周期的旧命令残留 +- 连接段提前转向策略较激进 + +## Findings + +### 1. 高: IMU 时间戳卡住时，超时与里程都会冻结 + +位置: +- `App/nav/global_nav.c:430-445` +- `App/nav/global_nav.c:471` +- `App/nav/global_nav.c:628-630` +- `App/nav/global_nav.c:657-659` + +现象: +- `GlobalNav_Update()` 用 `board->imu_wz.timestamp_ms` 作为内部时间基准 +- `odom_distance_accum` 的积分和 `elapsed_ms` 的推进都依赖这个时间戳 + +风险: +- 如果 IMU 仍被判定为在线，但时间戳停更，导航状态机会继续输出控制命令 +- 同时阶段超时保护不会推进 +- 里程积分也不会推进 + +可能后果: +- `ENTRY_STRAIGHT`、`LINK_STRAIGHT`、`EXIT_STRAIGHT` 长时间不退出 +- 转向超时失效，机器人可能持续原地转 + +说明: +- 这是行为级问题，不是单纯的调参问题 +- 文档中也提到了该项仍是 TODO，但当前实现里确实已经构成运行风险 + +### 2. 高: “连续 N 拍确认”实际在重复消费同一帧 VL53 数据 + +位置: +- `App/app_tasks.c:285-286` +- `App/app_tasks.c:299-347` +- `App/nav/global_nav.c:516-523` +- `App/nav/global_nav.c:615-620` +- `App/robot_params.h:398-399` + +现象: +- 导航循环周期约为 `20ms` +- VL53 任务推送周期约为 `100ms` +- `REACQUIRE` 的连续 `5` 拍确认和 `LINK_STRAIGHT` 的连续 `2` 拍确认，都是按导航循环计数 + +风险: +- 同一帧 VL53 观测会被导航层重复读取多次 +- 于是“连续确认”并不等于“连续多个独立观测确认” + +可能后果: +- `REACQUIRE` 可能只靠 1 帧侧向数据就进入 `CORRIDOR_TRACK` +- 沟口检测的 2 拍确认也可能只是一帧瞬时失效被重复消费 + +说明: +- 这会削弱你现在新设计的联合判定可靠性 +- 当前问题核心不是阈值，而是采样独立性不足 + +### 3. 中: 入场段强依赖起始摆放位置，缺少几何确认 + +位置: +- `App/nav/global_nav.c:484-495` +- `App/robot_params.h:382-385` +- `App/app_tasks.c:302-306` +- `Doc/map.md:9` +- `Doc/map.md:53-59` + +现象: +- `ENTRY_STRAIGHT` 现在只用 `里程 >= 0.30m 或 超时` 进入第一次右转 +- 启动后直接 `GlobalNav_Start()`，没有专门的“出启动区口再开始计段”动作 + +风险: +- 这要求机器人初始位置必须比较稳定，且接近你假设的起跑点 + +可能后果: +- 如果车放在 100cm 深启动区内更靠后位置，可能在到达 `C1` 入口前就右转 +- 如果车放得更靠前，也可能转得偏晚 + +说明: +- 当前实现修掉了“侧墙始终有效导致误触发”的问题 +- 但引入了“对起点一致性要求很高”的新假设 + +### 4. 中: 阶段切换发生后，本周期仍可能执行旧阶段指令 + +位置: +- `App/nav/global_nav.c:242-266` +- `App/nav/global_nav.c:491-495` +- `App/nav/global_nav.c:623-625` +- `App/nav/global_nav.c:706-710` + +现象: +- 若本周期内先生成了旧阶段控制命令，再满足切段条件并 `transition_to()` +- `out->stage` 在函数末尾会更新成新阶段 +- 但本周期发出去的速度命令可能还是旧阶段的 + +风险: +- 状态显示与实际执行在一个周期内不完全一致 + +可能后果: +- 转向完成后多转一个控制周期 +- 直行段满足切换条件后，当拍仍会继续向前推进一小段 + +说明: +- 这通常是 20ms 量级的小偏差 +- 但在靠近入口边缘、转向容差较紧时会放大几何误差 + +### 5. 中: 连接段允许仅凭前激光位移提前触发下一次转向 + +位置: +- `App/nav/global_nav.c:590-625` +- `App/robot_params.h:406-409` +- `App/nav/track_map.h:36-37` + +现象: +- `LINK_STRAIGHT` 的逻辑是 `B || (A && C)` +- 其中 `A` 和 `B` 都采用 `0.70m * 0.85 = 0.595m` 作为触发阈值 +- 也就是前激光变化量到达约 `59.5cm` 就可直接触发转向 + +风险: +- 机器人可能在标称 `70cm` 沟间距之前约 `10.5cm` 就开始转向 + +可能后果: +- 若前激光初值记录稍晚、转出后航向略偏、或前激光看到的并非理想正对围栏面，可能提前转向 +- 提前量叠加转向半径误差后，可能更接近垄背边缘而非下一条沟中心 + +说明: +- 这不是硬 bug，更像策略上偏激进 +- 若实车转向余量很大，可能仍可工作；若几何余量小，则风险会明显上升 + +## 正向观察 + +### 1. 转向拓扑表当前与地图理解一致 + +位置: +- `App/nav/track_map.c:34-47` + +说明: +- `C1` 右转入、左转出 +- `C2` 左转入、右转出 +- 奇偶沟交替 +- `C6` 左转出场 + +这一版没有再看到此前那种第一条沟转向方向写反的问题。 + +### 2. 连接段已经避免把“贴围栏侧 VL53 常亮”当作入口触发 + +位置: +- `App/nav/global_nav.c:108-144` +- `App/nav/global_nav.c:557-625` + +说明: +- 你已经把“非围栏侧 VL53 沟口检测”显式建模出来 +- 同时结合前激光和里程计做联合判定 + +这比此前的 `side_walls_detected()` 直接触发要合理得多。 + +## 开放问题 + +1. 比赛摆车是否保证机器人车头在启动区出口附近，而不是启动区任意位置？ +2. 传感器黑板中的 `is_valid` 是否有基于时间戳的失效机制，还是生产者停更后仍可能保持有效？ +3. 连接段的目标是“到下一沟中心线再转”，还是“进入下一沟开口就允许转”？当前 0.85 容差会显著影响这个定义。 + +## 总体评价 + +当前版本比前一版明显更接近真实场地几何，尤其是： +- 地图方向理解正确 +- S 型左右转表正确 +- 连接段不再依赖错误的侧墙常亮判据 + +但如果从比赛稳定性角度看，当前还存在两个最值得优先处理的问题： +- 时间基准不能完全绑死在 IMU 时间戳上 +- 连续确认不能重复消费同一帧侧向观测 + +如果这两点不处理，现场表现会比较依赖传感器健康状态与偶然时序，稳定性风险较高。