code-test-drone/工作进度.md

# 工作进度

> 更新日期：**2026-10-21**
> 项目：无人机视觉目标追踪与控制系统
> （`reference-c++/` C++ 参考实现 ＋ `utils/` Python 移植与新实现）
>
> 本文只记「做完了什么 / 卡在哪 / 下一步怎么走」。
> 架构全貌与必须遵守的铁律见 `.workbuddy/memory/ARCHITECTURE.md`；
> 「预定流程」全链路的操作指南与待办清单见 `预定流程-操作指南.md`；
> 权威规格见 `docs/` 下各技术路线 md（代码注释用 `§章节号` 反向引用）。
>
> ⚠ **架构口径（2026-10-19）**：光电吊舱已取代双目 + YOLO，成为**唯一目标源**；
> 双目三包已**归档到 `legacy/`**（见 §1.8），运行链路测试 446 条、归档测试 175 条。

---

## 1. 已完成工作

### 1.1 C++ → Python 移植（2 个模块）

| C++ 源工程 | Python 目标包 | 状态 |
|-----------|--------------|------|
| `reference-c++/target_tracker` | ~~`utils/target_tracker`~~ → **`legacy/utils/target_tracker`** | ✅ 完成（28 条用例，**已归档**） |
| `reference-c++/visual_track_control` | `utils/visual_track_control` | ✅ 完成（86 条，运行链路复用） |

- 逐构件对照移植：测试断言逐条搬运（`EXPECT_DOUBLE_EQ` → 精确 `==`，
  `EXPECT_NEAR` → `pytest.approx`）。
- `visual_track_control` 与追踪器**互不依赖**，唯一转换点是胶水层
  （胶水 demo 已归档到 `legacy/utils/demo_track_control.py`）。
- MAVLink 部分改为 **MAVSDK 语义映射**（offboard API，非字节透传），
  三分支映射保留 C++ A2 回归语义。
- 行为修复项（C1/C2/C4、A1/A2/A4、S6）已在测试文件头注明对应章节。

### 1.2 双目链路实现（非移植，按规格新写）—— 📦 已归档

> 2026-10-20 起三包与配套测试/demo/配置/权重/规格全部移到 `legacy/`（默认不跑测试）。
> 原因：吊舱已成为唯一目标源，这套链路无生产调用方；保留是为了日后接回"独立旁证/距离源"。
> 详见 `legacy/README.md`。

| Python 包 | 原位置 → 现位置 | 状态 |
|----------|---------|------|
| `stereo_ranger`（稀疏目标测距） | `utils/` → `legacy/utils/` | ✅ 完成（111 条，随包归档） |
| `stereo_yolo`（双目 YOLO 检测前端） | `utils/` → `legacy/utils/` | ✅ 完成（36 条，随包归档） |
| `target_tracker`（多目标追踪） | `utils/` → `legacy/utils/` | ✅ 完成（28 条，随包归档） |

- `stereo_ranger` 按规格五步流程实现，**纯标准库**，无第三方依赖；
  越界即有效性门、视差域权威，偏差项已在其包 README 记录。
- `stereo_yolo` 推理口径为 ultralytics 直跑 `.pt`，letterbox/解码/NMS/坐标回映均在库内完成；
  输出直接作为 `stereo_ranger` 的输入（仅复用其 `types.py` 纯值类型）。

### 1.3 吊舱 ↔ PX4 桥接与 L1 航向伺服（`utils/pod_bridge`）

- 协议桥接（09-22 起）：`protocol`（表2/表3 解析）、`px4_gateway`（offboard 速度闭环）、
  `pod_serial`、`guidance`（L2 地理引导）、`run_bridge.py --dry-run` 干跑入口。
- **L1 航向伺服（10-09 落地，路线 A1）**：吊舱角度直入——β/α 直接吃吊舱角度
  （配置双路 `beta_from_frame` / `beta_from_off_target`；本版未装电机，恒用脱靶量路径），
  复用 `TrackingControlLaw` 的无距离降级路径；`bridge.set_heading_servo_enabled()`
  支持运行时 L1/L2 切换（切换即 reset 清积分，防输出跳变）。
- **硬件结论（厂商答复 + 实测）**：本吊舱**不允许二次开发**；无激光 → **表4 永不出**，
  目标坐标改走地面注入；`0x0F` 自动锁定是常驻模式（`0x0E` 只停当次，`0x08` 才退出识别）；
  bit2 锁定位目标丢失后不复位 → **第二判据已改为 LOS 运动学一致性**
  （`pod_bridge/los_consistency.py`，2026-10-19 取代旧「我方视觉」方案）。
- **S1 台架标定已完成（10-09）**：`beta_sign=+1`、`alpha_sign=-1` 已回填
  `pod_bridge.json`，并有标定回归测试守护（`test_deployed_config_has_calibrated_signs`）。
- 配套工具：`virtual_pod`（模拟吊舱，无电机语义）、`verify_px4`（仅飞控验证，增量④）、
  `pod_sitl_test.py`（L1 SITL 全流程）。

### 1.4 预定流程全链路（`utils/task_runner`，10-12 落地；10-15 状态链重构；10-19 自定义返航 + LOS 判据）

「地面给坐标 → 天空飞向参考点 → 搜索 → 缓慢靠近 → 自定义返航」全链路，
**已在 PX4 SITL 端到端跑通**，并完成**真吊舱（COM5）+ SITL** 硬件在环验证：

状态链（10-15 重构后）：`WAIT → ARM_TAKEOFF → GOTO(L2) → SEARCH(原地扫视) → TRACK(L1 缓慢靠近) → RTL`

| 模块 | 文件 | 作用 |
|------|------|------|
| 任务状态机 | `utils/task_runner/state_machine.py` | 状态纯逻辑转移；参考点=搜索起点，搜索超时/追踪超时→RTL |
| 编排器 | `utils/task_runner/runner.py` | 状态机驱动 + 快照采集 + 状态动作（起飞/扫视/切律/返航）+ 异常兜底返航 |
| 返航控制 | `utils/task_runner/return_home.py` | 自定义返航：回起飞点 → 恢复初始航向 → 垂直降落（不爬高） |
| 日志模块 | `utils/task_runner/log.py` | 事件 JSONL + 遥测 CSV 双层结构化日志 |
| 吊舱控制 | `utils/task_runner/pod_control.py` | 真机吊舱识别/锁定指令（0x07/0x0F/0x0E/0x08，只下发一次） |
| 地面注入 | `utils/pod_bridge/ground_link.py` | UDP 接收地面站坐标（超龄判无效，替代表4） |
| 目标有效性 | `utils/pod_bridge/los_consistency.py` | **LOS 运动学一致性**第二判据（10-19 取代 `require_vision`） |
| SITL 入口 | `task_runner_sitl.py` | 仿真全流程（VirtualPod + 虚拟目标） |
| 真机入口 | `task_runner_real.py` | 真串口吊舱 + UDP 注入 + 吊舱控制 |
| 地面站 | `ground_station/` | 坐标注入 + 状态可视化（Flask） |

「缓慢靠近」由既有 `TrackingControlLaw` 的无距离降级通道实现：`distance.no_distance_action
= slow_approach` + `no_distance_approach_limit_mps=0.2`（SITL 实测 `vx_body` 稳定 0.2 m/s），
不需要新写控制律。

**自定义返航（10-19 新增）**：RTL 不再交给 PX4 原生 `return_to_launch()`（会先爬到
`RTL_RETURN_ALT=30m` 再降），改为 `utils/task_runner/return_home.py` 驱动：
`RETURN`（保持转场高度飞回起飞点）→ `YAW`（原地转回起飞时刻航向）→ `LAND`（垂直降落）。
SITL 实测：全程 4.91–5.06 m，距起飞点 16.1→1.6 m，航向恢复到 4° 内，高度 5.01→-0.03 m。
兜底（未起飞/无起飞点/转场超时/主循环异常）仍退回 PX4 原生 RTL；`--land` 就地降落。

**目标有效性第二判据（10-19 换成 LOS 一致性）**：吊舱取代双目后"我方视觉"不存在，
旧 `require_vision` 已删除（要么恒假、要么自证）。改为 `pod_bridge/los_consistency.py`：
本吊舱无电机、相机刚性固连 → 机头转 Δψ 时真实目标脱靶量必变 ≈−Δψ；以 `Δβ+Δψ`
（3s 窗口 / 20° 激励 / 8° 残差）判"角度是活的还是冻住的"，超限即 `tracked=False`
且 `pod_locked()` False（状态机随之退出 TRACK 重搜）。
⚠ 局限：只证"角度活着"，不证"是那个目标"；锁错物体但角度正常变化无法识别。

### 1.5 测试

**运行链路 446 条全部通过（实测，2026-10-20；归档包另 175 条）**：

| 测试组 | 用例数 | 覆盖内容 |
|--------|-------|---------|
| `tests/visual_track_control` | 86 | 对应 C++ gtest + L1 角度直入增量 |
| `tests/pod_bridge` | 208 | 协议/网关/桥接/L1 + virtual_pod/ground_link + 节流 + **LOS 一致性** |
| `tests/task_runner` | 122 | 状态机（含 SEARCH）/编排器/日志/吊舱控制/自定义返航 |
| `tests/ground_station` | 30 | 地面站页面与日志解读 |
| **运行链路合计** | **446** | 默认 `cd utils && pytest` 只跑这些 |
| （归档）`legacy/utils/tests/{stereo_ranger,stereo_yolo,target_tracker}` | 175 | 见 `legacy/README.md`，默认不跑 |

> **架构口径（2026-10-19 / 10-20）**：双目 + YOLO 三包已**归档到 `legacy/`**；
> 运行链路（`task_runner_*` / `pod_bridge`）不依赖它们 —— 吊舱是唯一目标源。
> 目标有效性第二判据为 **LOS 运动学一致性**（`pod_bridge/los_consistency.py`），
> `require_vision` 已删除。

- 归档的双目测试**不需要**真实图像、模型权重或硬件。
- ⚠ codegraph 报告 `AssociateDetections` / `SelectPrimary` 等“无测试”属调用图误报，勿据此判断。

### 1.6 仿真 / 运行入口

| 脚本 | 内容 | 结果 |
|------|------|------|
| `task_runner_sitl.py`（PX4 SITL） | 全链路 `goto→search→track→rtl` + 自定义返航 | ✅ 端到端跑通（10-19 复跑） |
| `task_runner_real.py`（真吊舱 COM5 + SITL 飞控） | 同上，真串口吊舱在环 | ✅ 无目标安全路径 + 强制锁定 TRACK 路径（10-19） |
| `l1_sitl_test.py`（PX4 SITL） | 只覆盖 L1 控制律闭环（原 `pod_sitl_test.py`） | ✅ SITL 实跑 |
| （归档）`legacy/utils/demo_*.py` | 双目/追踪演示 | 📦 移入 legacy，默认不跑 |

- 共享件 `now_ms` / `wait_until_ready` / `TelemetryHub` 等已从"测试脚本"拆到
  `utils/sitl_support.py`，两个入口不再 import 测试文件。
- 吊舱硬件在环实测（10-19，COM5/CH340）：表2-2 **60.0 Hz**、`self_check=0x00`；
  表3 20Hz 下发不打断上行；表2-1 指令帧校验和与协议样例一致。
  无目标时 `status1=0x00` 恒定（bit2=0）→ 全链路走"搜索 20s 超时→返航"安全路径。
  ⚠ **仍未验证**：吊舱真实目标识别（bit2 0→1），需视场内放目标。

### 1.7 文档与索引

- `docs/` 现有 7 份（3 份双目规格已归档到 `legacy/docs/`）：视觉跟踪控制律技术路线、
  视觉追踪技术路线、物理追踪思路、吊舱解算验证方案（模拟飞控喂表3）、树莓派 4B 部署注意事项、
  **L1 航向伺服实现方案**、光电吊舱控制协议 G 版-PB（docx）。
- 根目录：`预定流程.md`（需求）+ `预定流程-操作指南.md`（操作指南与待办，链路权威）+ `README.md`（总览）。
- `legacy/README.md`（归档说明）、`tools/README.md`（临时脚本说明）。
- `.workbuddy/memory/ARCHITECTURE.md` + `MEMORY.md`：完整架构参考与精简铁律（已对齐新架构）。
- `.codegraph/` 已于 10-20 重建（旧索引过期，清理后重新索引）。
- 相机标定脚本已移到 `tools/`；标定结果归档在 `legacy/stereo_calib_params.npz`。

### 1.8 仓库结构精简（2026-10-20 完成，2026-10-21 提交）

**动机**：子项目堆积了大量文件，其中双目/YOLO 一套在"吊舱取代双目"后已无生产调用方，
旧架构脚本与生成物也混在根目录，影响阅读与使用。

**做了什么**：

| 处理 | 内容 |
|---|---|
| 归档 `legacy/` | `stereo_yolo` / `stereo_ranger` / `target_tracker` 三包 + 175 条测试 + 3 个 demo + `stereo_*.json` + 素材 + `models/yolo26n-drone.pt` + 三份双目规格 + `stereo_calib_params.npz` |
| 删除 | `test.py`（旧架构双目闭环，未实跑）、`trajectory_plot.png`、`新建 文本文档.txt`、空目录、全部 `__pycache__` / `.pytest_cache` |
| 移入 `tools/` | `camera.py` / `camera_test.py` / `video_test.py`（台架相机脚本） |
| 拆分改名 | `pod_sitl_test.py` → `utils/sitl_support.py`（入口共享件）+ `l1_sitl_test.py`（L1 测试）；两个入口不再 import 测试文件 |
| 从索引剔除 | 59 个历史误跟踪的 `.pyc`（已被 `.gitignore` 覆盖） |
| 新增 `.gitignore` | `__pycache__/`、`.pytest_cache/`、`logs/`、`.codegraph/`、`.workbuddy/`、`.idea/`、`legacy/models/*.pt` 等 |
| 依赖清单 | `requirements.txt` 按实际 import 重写（去 `ultralytics`/`matplotlib`，补 `flask`）；`requirements-pod.txt` 更新条数 |
| 文档 | 所有现行文档同步新路径/新计数（README、操作指南、ARCHITECTURE、MEMORY、utils/README、L1 方案、树莓派部署）；新增 `legacy/README.md`、`tools/README.md` |
| codegraph | 删除过期索引并重建（143 文件 / 2667 节点 / 6470 边） |

**结果**：

- 根目录从 18 个文件减到 **10 个**（3 个入口/测试脚本 + 4 份 md + 2 份 requirements + `.gitignore`）
- `utils/` 从 6 个包 + 3 个 demo 减到 **4 个包 + `sitl_support.py`**（体积 3.39MB → 1.78MB）
- 测试口径：运行链路 **446 passed**；归档 **175 passed**（`cd legacy/utils && PYTHONPATH="../../utils" pytest`）
- 运行链路代码对已归档包的引用：**0 处**

---

## 2. 未完成工作

### 2.1 真机增量验证（P0，实飞前必须）

按 `预定流程-操作指南.md` §2.4 逐级进行，每级过了再进下一级：
② 串口回环 → ③ 仅吊舱干跑（验 β→yaw_rate 符号/量级）→ ④ 仅飞控（`verify_px4`）→
⑤ 地面联调（`task_runner_real.py`）。

### 2.2 数值参数未标定（阻塞飞行决策）

- **状态机**（`build_sm_config()`）：`arrival_radius_m` / `goto_timeout_s` / `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` 及各类超时均为占位。
- **控制律**：`heading.gain`、`guidance.speed_kp/yaw_kp` 为占位
  （`beta_sign`/`alpha_sign` 已按 S1 标定回填；LOS 判据阈值 `los_min_yaw_change_deg` /
  `los_max_residual_deg` 亦待台架实测调整）。
- **标定回填前不得实飞**。
- （归档）`legacy/utils/config/stereo_ranger.json` 几何参数为占位；若日后接回双目，
  解禁路径见 `legacy/docs/双目相机模组到位作业指导.md` P0→P6（标定未走完）。

### 2.3 任务级状态机规格对齐

- 「预定流程」版状态机已实现并 SITL + 真吊舱在环跑通（`WAIT→ARM_TAKEOFF→GOTO→SEARCH→
  TRACK→RTL`，2026-10-19 口径）；
- 但 `docs/物理追踪思路.md` 引用的 S5~S8 版规格（`docs/状态机设计.md` 等）仍缺失，
  两套状态集是否对齐/合并需总项目侧确认（见 §3.1）。

## 3. 阻塞项与风险

### 3.1 本仓库缺失 4 份被引用的上游规格

`docs/物理追踪思路.md` 引用了下列文档，但它们在**本仓库不存在**（应在总项目或飞控工程侧）：

| 被引文档 | 被引内容 | 影响 |
|---------|---------|------|
| `docs/状态机设计.md` | S6/S7/S8 切换守卫、§4.2 目标丢失转移、§7 策略接口 | S5~S8 版状态机与「预定流程」版对齐 |
| `docs/感知与避障设计.md` | 标定流程、稳定锁定、雷达关联、降级原则 | 阻塞 S8 障碍接管与雷达联动 |
| `docs/系统架构设计.md` | 数据流、控制单出口、时间与控制周期 | 影响状态机与控制律的接线方式 |
| `docs/需求分析.md` | FR-034/FR-044/FR-051、NFR-007 | 需求追溯无法本地闭环 |

另 `docs/开发进度.md`（总项目开发进度）亦被引用，本仓库没有。

### 3.2 C++ 侧未复跑

`reference-c++/target_tracker` 与 `visual_track_control` 下均**无 `build/` 目录**，
未构建、未执行 ctest。C++ 测试用例文件齐全（11 个），
需先 `cmake .. && make && ctest`。若以 Python 侧为验证基准，此点可暂不处理。

### 3.3 环境口径（已统一，曾不一致）

**现行唯一口径**：conda env `pytorch`（`D:\program file\conda\envs\pytorch\python.exe`），
pytest 9.1.1 可用，全量 621 passed 直接跑。
> 历史：曾一度认为该环境缺 pytest、改走隔离 venv `drone-test`。该结论**已过时**，
> 隔离 venv 不再需要；文中若见旧 venv 路径一律以 conda `pytorch` 为准。

### 3.4 真机安全红线

标定回填前**不得实飞**；S3 起必须：不装桨 / 空旷场地 / RC 随时可切手动 / 有人值守。

### 3.5 帧周期算力不可外推（归档项）

`stereo_yolo` 实测帧周期 25~35 ms 为台架开发机数据，与 30 fps 预算同量级，
结论不可外推到机载平台（**这也是当前不接双目、距离优先用雷达的原因之一**）。
该包已归档到 `legacy/`。

---

## 4. 下一步（建议顺序）

1. **接距离源（激光雷达）**：给 `PodBridge` 加距离注入点 → 填 `distance_valid` →
   打开 `d_exp_m` 距离 PID，并把 TRACK 的"到点"判据从纯超时改为距离判据。
   （低算力、收益最大；双目暂不接。）
2. **真机目标识别验证**：视场内放目标（人/无人机）+ 0x07，确认 bit2 0→1
   与 TRACK 真实闭环；顺带复核 LOS 判据阈值是否合适。
3. **占位参数标定回填**：状态机 6 项 + 返航 6 项 + L1/L2 增益 + LOS 两阈值。
4. P1：摄像头接真视频流；`TaskStateConfig`/`ReturnConfig` 外置 JSON；地面站与 runner 实时打通。
5. P2：C++ `cmake && ctest`；codegraph 定期 `sync`（本次已重建）。
6. 相机模组到位后按作业指导走 P0→P6（脚本在 `tools/`，文档在 `legacy/docs/`）；
   确认 §3.1 四份上游规格可获取位置。

---

## 5. 验证命令（实测所用）

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

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

# 归档的可选后端测试（175 条，按需）
cd "D:/ProgramData/code_drone-test/legacy/utils"
PYTHONPATH="../../utils" "$PY" -m pytest -q

# 吊舱台架 / SITL 全流程 / 地面站 / 真机：见 预定流程-操作指南.md §五「一键命令汇总」
```

---

## 6. 一页状态汇总

| 工作项 | 状态 | 备注 |
|--------|------|------|
| C++ `target_tracker` → Python | ✅ 完成 | 28 条用例；**已归档 legacy/** |
| C++ `visual_track_control` → Python | ✅ 完成 | 86 条（含 L1 增量）；运行链路复用 |
| `stereo_ranger` 双目测距 | 📦 归档（10-20） | 111 条随包；几何参数待标定 |
| `stereo_yolo` 检测前端 | 📦 归档（10-20） | 36 条随包；真实素材待联调 |
| 三路 demo 仿真验证 | 📦 归档（10-20） | 移入 `legacy/utils/` |
| `pod_bridge` 桥接 + L1 航向伺服 | ✅ 完成（10-09） | S1 符号已标定；增益待标 |
| **预定流程全链路（task_runner）** | ✅ **SITL 跑通（10-12）；10-15 重构；10-19 真吊舱在环** | 状态链含 SEARCH + 自定义返航 |
| LOS 运动学一致性判据 | ✅ 完成（10-19） | 取代 `require_vision`；阈值待台架调整 |
| 自定义返航（回点+复位航向+垂直降落） | ✅ SITL 验证（10-19） | 全程 4.91~5.06 m，不爬高 |
| 真机增量验证 ①②③③b | ✅ 已过 | 串口链路/干跑/真吊舱+SITL 全流程 |
| 真机增量验证 ④⑤（仅飞控 / 实飞地面联调） | ❌ 未开始 | P0；实飞前必须 |
| 吊舱**真实目标识别**（bit2 0→1） | ❌ 未验证 | 需视场内放目标 |
| 距离源接入（激光雷达） | ❌ 未开始 | 打开距离 PID 与到点判据 |
| 状态机/返航/L1/L2 占位参数标定 | ❌ 未开始 | 回填前不得实飞 |
| S5~S8 版状态机（物理追踪思路） | ⚠ 规格缺失 | 待对齐（§3.1） |
| 相机模组标定 P0~P6 | 📦 归档 | 脚本移到 `tools/`，标定数据在 `legacy/` |
| C++ 侧构建与 ctest | ⚠ 未执行 | 无 build 目录 |
| ~~conda 环境回填 pytest~~ | ✅ 已解决 | 运行链路测试统一走 conda `pytorch`（446 条） |