# 预定流程 · 操作指南与待办事项

> 更新日期：**2026-10-19**
> 覆盖：任务级状态机全链路（`utils/task_runner/`）＋ 吊舱桥接与 LOS 判据 ＋ 地面站页面与控制命令通道 ＋ 真机/仿真入口
>
> 本链路对应 `预定流程.md` 的「地面给坐标 → 天空解锁/接受坐标引导/搜索/缓慢靠近」，
> 已在 **PX4 SITL 端到端跑通**，并完成**真吊舱（COM5）+ SITL** 硬件在环验证。
> 架构与铁律见 `.workbuddy/memory/MEMORY.md` / `ARCHITECTURE.md`。
>
> ⚠ 架构口径：吊舱取代双目 + YOLO 成为**唯一目标源**；`require_vision` 已删除，
> 第二判据改为 **LOS 运动学一致性**（见 §2.6）。

---

## 一、本次落地了什么

> **2026-10-15 按实机作业预期重构状态链**：参考点 = 搜索起点（不再是终点），
> 到点后原地扫视搜索，锁定后按 `slow_approach` **缓慢靠近**，超时返航。
>
> 新状态链：`WAIT → ARM_TAKEOFF → GOTO(L2 飞向参考点) → SEARCH(原地扫视) → TRACK(L1 缓慢靠近) → RTL`

| 模块 | 文件 | 作用 |
|------|------|------|
| 任务状态机 | `utils/task_runner/state_machine.py` | 状态纯逻辑转移：到点→SEARCH 扫视；搜索超时/追踪超时→RTL；TRACK 丢目标→回 SEARCH |
| 编排器 | `utils/task_runner/runner.py` | 状态机驱动 + 采集快照 + 状态动作（起飞/扫视/切律/返航）+ **主循环异常兜底返航** |
| 日志模块 | `utils/task_runner/log.py` | 事件 JSONL + 遥测 CSV（机器日志）＋ 简洁任务简报 `task_brief.log` |
| 吊舱控制 | `utils/task_runner/pod_control.py` | 真机吊舱识别/锁定指令（0x07/0x0F/0x0E/0x08）；识别开 **只下发一次**（防重入自激） |
| 地面注入 | `utils/pod_bridge/ground_link.py` | UDP 接收地面站坐标（超龄判无效） |
| SITL 入口 | `task_runner_sitl.py` | 仿真全流程（VirtualPod + 虚拟目标） |
| 真机入口 | `task_runner_real.py` | 真串口吊舱 + UDP 注入 + 吊舱控制 |
| 地面站 | `ground_station/app.py` + `templates/index.html` | 坐标注入 + 任务/吊舱控制 + 状态可视化（Flask） |
| 命令通道 | `ground_link.py`（队列）+ `runner.py`（分派）+ `app.py`（`/api/command`） | 页面 `{"cmd":…}` → UDP 14555 → runner 逐拍分派 |

控制律本体复用既有 `visual_track_control.TrackingControlLaw`（L1 角度直入）与
`pod_bridge.GuidanceLaw`（L2 地理引导），**运行时通过 `bridge.set_heading_servo_enabled()` 切换**。

**「缓慢靠近」不需要新写控制律**：既有 `TrackingControlLaw` 的无距离降级通道已实现——
`distance.no_distance_action = "slow_approach"` + `no_distance_approach_limit_mps = 0.2`。
SITL 实测 TRACK 段 `vx_body` 稳定 **0.2 m/s**，且经加速度限幅平滑起步。

---

## 二、操作指南

### 2.1 纯软测试（无硬件，最快验证）

```bash
cd "D:/ProgramData/code_drone-test/utils"
"D:/program file/conda/envs/pytorch/python.exe" -m pytest -q
# 预期：446 passed（2026-10-20，运行链路；归档 175 条另计）
```

> 环境口径：项目默认 Python 环境为 conda env `pytorch`（含 pytest/flask/mavsdk/pyserial），
> 运行与测试统一使用；「conda 缺 pytest，测试走隔离 venv」的旧口径已废止。

### 2.2 SITL 仿真全流程（独立模式，内置虚拟目标自动注入坐标）

前置：WSL 里启动 PX4 SITL（监听 14540）。

```bash
# 1. WSL 内启动 PX4 SITL
cd ~/PX4-Autopilot && make px4_sitl gz_x500

# 2. Windows 侧跑全流程（内置虚拟目标 home 北30m/东15m）
cd "D:/ProgramData/code_drone-test"
"D:/program file/conda/envs/pytorch/python.exe" task_runner_sitl.py --conn udpin://0.0.0.0:14540
```

预期：`wait → arm_takeoff → goto → search → track → rtl`。

> **返航已改为自定义流程（2026-10-19）**：不再调 PX4 原生 `return_to_launch()`。
> 原生 RTL 会先爬到 `RTL_RETURN_ALT`（本 SITL 实测 **30 m**）、再降到
> `RTL_DESCEND_ALT`（10 m）后落地，即"飞很高再降落"。自定义流程改为：
> **保持转场高度（取当前高度与起飞高度的较大者，绝不额外爬高）飞回起飞点 →
> 原地把机头转回起飞时刻航向 → 垂直降落**。SITL 实测全程高度 4.91–5.06 m。
>
> 参数在 `task_runner_{sitl,real}.py` 的 `build_return_config()`（`ReturnConfig`）；
> 兜底：未起飞/无起飞点/转场超时/主循环异常 → 退回 PX4 原生 RTL；
> `--land` → 就地降落（不返航）。

### 2.3 SITL 仿真（地面站联动模式，坐标由地面站 UDP 注入）

```bash
# 终端 A：无人机（--ground-station 表示坐标走 UDP 接收，不再内置自动注入）
"D:/program file/conda/envs/pytorch/python.exe" task_runner_sitl.py --conn udpin://0.0.0.0:14540 --ground-station

# 终端 B：地面站页面（--rtsp-url 可选：接入吊舱实时视频，断流自动重连）
"D:/program file/conda/envs/pytorch/python.exe" ground_station/app.py --udp-port 14555 --log-dir logs --rtsp-url rtsp://192.168.144.119:8554/video
# 浏览器打开 http://127.0.0.1:5000，输入坐标点「注入坐标」（或勾选「持续注入」自动重发）
```

⚠ 地面站需**持续**发坐标（每 0.5~1s 一次），否则 `ground_inject.timeout_ms`（默认 5s）
超龄后 GOTO 判「坐标失效」返航。页面勾选「持续注入」即可（**服务端后台线程**每 0.5s 重发，
切窗口/最小化不影响；实测浏览器会节流后台标签页的 JS 定时器 0.5s→约 1 次/分钟，故不能放页面侧）；
取消勾选或关闭地面站程序才停止。

### 2.4 地面站页面控制功能（2026-10-14 新增）

页面除坐标注入外，还支持以下控制（均走**同一 UDP 端口** 14555，消息 `{"cmd": "<name>"}`，
由 `GroundLink` 入队、`TaskRunner` 主循环逐拍分派；未知命令记警告忽略）：

| 区块 | 控件 | 命令 | 语义 |
|------|------|------|------|
| 链路目标 | 目标徽标 + 下拉选择 + 「切换目标」 | — | 坐标/命令的 UDP 目标地址（默认「本地回环」127.0.0.1:14555 跑仿真；可选本机网卡或自定义 IP，运行时切换，2026-10-14 新增） |
| 任务日志 | 滚动事件列表 | — | 阶段切换 / 地面站命令 / 飞控状态文本（STATUSTEXT，警告按级别着色）实时显示（读 `/api/state`，2026-10-14 新增） |
| 坐标注入 | 坐标形式（经纬度 / 局部北东 m；局部可选原点） | — | 局部按选定原点换算为经纬度后注入——「起飞点(home)」= 本次任务运行段起始位置（≈起飞点），「当前坐标」= 注入时刻无人机位置；页面显示换算结果与所用原点（2026-10-14 新增） |
| 坐标注入 | 「持续注入」勾选 | — | 服务端后台线程每 0.5s 重发当前坐标（替代循环脚本，与页面可见性无关） |
| 吊舱视频 | 「吊舱视频」面板 | — | 服务端 `--rtsp-url` 拉 RTSP → MJPEG 推流（浏览器原生显示）；断流自动重连 |
| 任务控制 | 请求返航 RTL | `rtl` | `runner.request_rtl()`：人工返航，任何状态立即转 RTL |
| 任务控制 | 紧急终止 | `emergency` | `runner.trigger_emergency()`：最高优先级立即返航 |
| 吊舱控制 | 开识别 | `pod_detect_on` | 0x07(目标类型)+0x0F（对齐 pod_console 按键 d） |
| 吊舱控制 | 自动锁定 | `pod_auto_lock` | 0x0F+0x10（对齐按键 a，识别已开时可用） |
| 吊舱控制 | 停跟踪 | `pod_stop_track` | 0x0E：仅解当前锁定，0x0F 常驻自动锁定仍生效（对齐按键 u） |
| 吊舱控制 | 关识别 | `pod_detect_off` | 0x0E+0x08：彻底退出识别（对齐按键 s） |
| 吊舱控制 | 归中 | `pod_center` | 0x2B（对齐按键 c） |

说明：RTL/紧急在 SITL 可直接验证。**注意 `rtl` 命令触发的是自定义返航**
（回起飞点→恢复初始航向→垂直降落，不爬高；见 §2.2 说明），不是 PX4 原生 RTL。
吊舱命令在 SITL（VirtualPod 无控制口）会被忽略并记警告，
真机经 runner → 吊舱串口下发（2026-10-19 台架实测：0x2B/0x07/0x0F/0x10/0x0E/0x08 全达真吊舱、校验和正确）。

### 2.5 真机联调（增量流程，标定前不得实飞）

按顺序逐级验证，每级过了再进下一级：

| 级 | 动作 | 命令 | 通过判据 |
|----|------|------|---------|
| ① 纯软单测 | 全量 pytest | `pytest -q` | 446 passed（2026-10-20） |
| ② 串口回环 | 吊舱自环 | 台架工具 | 收发一致 |
| ③ 仅吊舱 | 干跑（不发 offboard） | `python utils/pod_bridge/run_bridge.py --config utils/config/pod_bridge.json --port COM5 --dry-run` | β→yaw_rate 符号/量级正确 |
| ③b 吊舱控制联测 | SITL 飞控 + 真吊舱直连本机（TTL 转 USB） | `python task_runner_real.py --port COM5 --conn udpin://0.0.0.0:14540 --log-dir logs/x` ＋ 页面吊舱按钮 | 命令记录无失败、吊舱按指令响应 |
| ④ 仅飞控 | 飞控遥测/offboard | — | 遥测新鲜、offboard 可启停 |
| ⑤ 地面联调 | 真机全流程 | `python task_runner_real.py --target-type 1` | 完整走完 |

**2026-10-19 真吊舱在环实测结论**（COM5 / CH340，SITL 飞控）：

| 项 | 结果 |
|----|------|
| 表2-2 上行 | **60.0 Hz**，6s 359 帧；`self_check=0x00` 无告警 |
| 表3 下行 | 20Hz 持续下发时上行仍稳 60.0 Hz，**下行不打断上行** |
| 表2-1 指令 | 0x2B/0x07/0x0F/0x10/0x0E/0x08 均下发成功，帧校验和与协议样例一致（0x2B→`a6`） |
| 状态位 | 视场内无目标时 `status1=0x00` 恒定（**bit2 恒 0 = 搜索态**，符合预期） |
| 脱靶量 | 搜索态恒 `(0.00, 0.00)` —— 与文档「丢失表现为 0」一致 |
| 自身 IMU | `att_roll/att_pitch` 恒 0；`frame_*` 恒 0（本版无电机，符合厂商确认） |
| 全流程（无目标） | `arm_takeoff → goto → search(20s 超时) → rtl → 落地` ✅ |
| 全流程（强制锁定混合） | `goto → search → track(20s) → rtl`；TRACK 段 `vx` 0.05→**0.2 m/s**，距离 29.5→26.2 m ✅ |
| 扫视实测 | SEARCH 20s 内机头 yaw 跨度 **65°**，水平距离 26.6–29.9 m **无漂移**，vz 恒 0 ✅ |

⚠ **仍未验证**：吊舱真实目标识别（bit2 由 0→1）。需视场内放置目标（人或无人机）
并下发 0x07，才能验证「搜索发现目标→锁定→缓慢靠近」的真实闭环。

真机全流程前置：
1. 填 `utils/config/pod_bridge.json` 的 `pod_serial.port`（如 `/dev/ttyAMA0`）、`px4.connection_string`；
2. 地面站往 `ground_inject.port`（默认 14555）持续发 `{"lat":..,"lon":..,"alt":..}`。

真机关键命令参数：
```bash
python task_runner_real.py --target-type 1        # 1=无人机，0=人/车
python task_runner_real.py --port /dev/ttyAMA0    # 覆盖吊舱串口
python task_runner_real.py --conn serial:///dev/ttyS1:57600   # 覆盖飞控连接串
```

### 2.6 硬件铁律（真机必读，来自厂商确认 + 实测）

- 🔴 **本吊舱无激光 → 表4 目标经纬度永不出**，目标坐标必须走地面注入（`ground_link`），
  `guidance` 吃注入坐标而非表4。
- **0x0F「发现目标自动锁定」是常驻模式**，`0x0E` 只停当前这次跟踪、不退出自动锁定；
  只有 `0x08 目标识别关` 才真正退出识别。
- **bit2 锁定位在目标丢失后不复位**，且脱靶量无无效标志位（丢失表现为 0）——单靠 bit2
  会朝过期坐标继续飞。**第二判据已改为 LOS 运动学一致性**（`pod_bridge/los_consistency.py`，
  2026-10-19 取代旧的 `require_vision`）：本吊舱无电机、相机刚性固连，机头转 Δψ 时真实
  目标的脱靶量必变 ≈−Δψ；机头已明显转动而脱靶量不动 → 判丢（触发退出 TRACK 重搜）。
  ⚠ 它只能证明「角度是活的」，不能证明「是那个目标」。
- **自动解锁（`--auto-unlock`）在本设备禁用**（表4 恒不发导致误解锁），解锁一律用 `0x0E`：
  台架走 `pod_console` 按键 `u`，地面站页面走「停跟踪」按钮（等效通道）。

---

## 三、待办事项（按优先级）

### P0（实飞前必须）

- [ ] **真机增量验证**：按 §2.5 的 ④⑤ 继续（①②③③b 已于 2026-10-19 通过）。
- [ ] **吊舱真实目标识别验证**：视场内放目标（人/无人机）+ 0x07，确认 bit2 0→1 与
  TRACK 真实闭环；这是目前唯一没走通的一环（其余均为强制锁定模拟）。
- [ ] **状态机参数标定**：`task_runner_real.py` 的 `build_sm_config()` 中
  `arrival_radius_m`（到参考点转搜索的半径）、`search_timeout_s`、`track_timeout_s`、
  `sweep_rate_dps` / `sweep_reverse_s`（扫视）、`takeoff_altitude_m` 全是占位值。
- [ ] **返航参数标定**：`build_return_config()` 的 `speed_mps`、`arrival_radius_m`、
  `yaw_align_tolerance_deg`、`alt_kp`、`timeout_s` 均为占位值。
- [ ] **L1/L2 增益标定**：`pod_bridge.json` 里 `heading.gain`、`guidance.speed_kp/yaw_kp`
  等仍为占位，`beta_sign`/`alpha_sign` 已按 S1 标定（+1/-1），其余待标定。
- [ ] **LOS 判据阈值标定**：`los_window_ms=3000`、`los_min_yaw_change_deg=20`、
  `los_max_residual_deg=8` 需按真机噪声与目标机动特性复核（过紧误判丢目标、过松漏判幽灵）。

### P1（落地后尽快）

- [x] **摄像头可视化接真视频流**（2026-10-14 完成）：服务端 RTSP→MJPEG 接入页面「吊舱视频」面板
  （`--rtsp-url`；断流自动重连，拔插网线自愈已实测）；追踪框叠加未做（吊舱 OSD 已烧录脱靶量）。
- [ ] **接入距离源（激光雷达，优先）**：给 `PodBridge` 加距离注入点 → 填 `distance_valid` →
  打开 `d_exp_m` 距离 PID，并把 TRACK 的「到点」判据从纯超时改为距离判据。
  代价低（串口读数），直接改善"贴近"；**双目暂不接**（Pi 4 上 YOLO 开销大）。
- [ ] **任务级配置 JSON 化**：`TaskStateConfig` 与 `ReturnConfig` 目前是 dataclass 默认值 +
  脚本内构造，应外置到配置（铁律「配置全外置」）。
- [ ] **地面站与 runner 状态实时打通**：当前地面站读日志文件（秒级延迟），可改为 runner
  直接 UDP 上报遥测到地面站（低延迟）。

### P2（工程收尾）

- [x] **`.gitignore` 已补齐**（10-20）：`logs/`、`__pycache__/`、`.pytest_cache/`、`.codegraph/`、`.workbuddy/`、`legacy/models/*.pt` 等。
- [x] **conda `pytorch` 回填 pytest**（pytest 9.1.1 可用；2026-10-20 运行链路 446 通过，
  运行与测试统一走 conda 环境）。
- [ ] **C++ 侧构建验证**：`reference-c++/target_tracker` 与 `visual_track_control` 无
  `build/`，若以 C++ 为验收基准需 `cmake .. && make && ctest` 跑 11 个测试。
- [ ] **codegraph 重新索引**：当前索引早于 10-15/10-19 重构（行号偏移），需 `codegraph sync`。

---

## 四、关键配置速查（`utils/config/pod_bridge.json`）

| 段 | 关键字段 | 说明 |
|----|---------|------|
| `pod_serial` | `port` | 吊舱串口设备号（真机 `/dev/ttyAMA0`，USB 转串口常见 `COM5`/`/dev/ttyUSB0`） |
| `px4` | `connection_string` | 飞控连接串（SITL `udpin://0.0.0.0:14540`，真机串口） |
| `link` | `require_laser_valid` | 无激光吊舱必须 `false`（本机恒 `false`） |
| `heading_servo` | `enabled` / `beta_sign` / `alpha_sign` | L1 开关 + 符号（±1，已标定 +1/-1） |
| `heading_servo` | `los_consistency_enabled` / `los_window_ms` / `los_min_yaw_change_deg` / `los_max_residual_deg` | **目标有效性第二判据**（LOS 运动学一致性，2026-10-19 取代 `require_vision`） |
| `ground_inject` | `enabled` / `port` / `timeout_ms` | 地面坐标注入开关 / UDP 端口 / 超龄门限；**真机入口会用它统一 `guidance.target_stale_ms`** |

> 任务级与返航参数不在 JSON 内，见入口脚本的 `build_sm_config()` / `build_return_config()`
> （JSON 化仍在 P1 待办）。

### 4.1 链路负荷与遥测速率（2026-10-15）

遥测速率**不是越高越好**：PX4 的消息速率由最后一次 `SET_MESSAGE_INTERVAL` 决定，
多个订阅方用不同值会互相覆盖；速率越高 MAVSDK 回调队列压力越大。

| 流 | 速率 | 消费者/依据 |
|----|------|------------|
| `attitude_euler` | 20 Hz | 控制律（`attitude_stale_ms=300`） |
| `position` | 10 Hz | 起飞到位 / 到参考点距离 |
| `position_velocity_ned` | 10 Hz | 仅虚拟吊舱几何（SITL） |
| `gps_info` / `velocity_ned` / `in_air` | 2 Hz | 状态量，变化极慢 |

速率集中定义在 `utils/sitl_support.py` 的 `RATE_*` 常量（唯一来源，各订阅方共用）。

**任务日志落盘策略**：事件（阶段切换/命令/告警）**即时 flush**（页面实时性不受影响）；
遥测 CSV 按 `TaskLogConfig.telemetry_flush_ms`（默认 1000ms）节流落盘 —— 主循环
20Hz 若每行同步 flush，等于每秒 20 次磁盘 IO，在树莓派 SD 卡上会阻塞事件循环。
`close()` 做最后 flush，正常退出不丢数据。

---

## 五、一键命令汇总

```bash
PY="D:/program file/conda/envs/pytorch/python.exe"

# 运行链路测试（446 条）
cd "D:/ProgramData/code_drone-test/utils" && "$PY" -m pytest -q

# 吊舱串口自检 / S2 干跑
"$PY" utils/pod_bridge/listen_pod.py COM5 --seconds 5
"$PY" utils/pod_bridge/run_bridge.py --port COM5 --dry-run --duration 10

# SITL 独立全流程
"$PY" task_runner_sitl.py --conn udpin://0.0.0.0:14540

# SITL 地面站联动
"$PY" task_runner_sitl.py --conn udpin://0.0.0.0:14540 --ground-station
"$PY" ground_station/app.py --udp-port 14555 --log-dir logs --rtsp-url rtsp://192.168.144.119:8554/video

# 真机全流程（真吊舱串口 + UDP 注入 + PX4）
"$PY" task_runner_real.py --port COM5 --conn udpin://0.0.0.0:14540 --target-type 1
"$PY" task_runner_real.py --port /dev/ttyAMA0 --target-type 1            # 树莓派
"$PY" task_runner_real.py --port COM5 --land                             # 收尾就地降落
```

> 真机入口的 `rtl`/超时/异常收尾都走**自定义返航**（回起飞点→恢复初始航向→垂直降落）；
> 仅"未起飞/无起飞点/转场超时/主循环异常"退回 PX4 原生 RTL。