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. 验证命令（实测所用）

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 条）