code_drone-test —— 无人机视觉目标追踪与控制系统

最后对齐：2026-10-19。本文描述的是当前运行架构；历史设计与规格见 docs/。

---

0. 先看这一段（避免按旧设计干活）

本项目历史上存在两条感知路线，现状如下：

路线	组成	现状
运行链路（现行）	光电吊舱（表2-2 角度 + bit2） → pod_bridge → task_runner → MAVSDK offboard → PX4	✅ 已落地，SITL + 真吊舱在环均验证过
可选感知后端（备用）	stereo_yolo → target_tracker → stereo_ranger → visual_track_control	📦 已归档到 legacy/（175 条测试随包归档，默认不跑）

关键约定（2026-10-19 定稿）

• 吊舱取代双目 + YOLO，是唯一目标源；方位/LOS 来自吊舱脱靶量（角度直入）。
• 目标有效性 = 吊舱 bit2 + 遥测新鲜 + LOS 运动学一致性（pod_bridge/los_consistency.py）。 旧的 require_vision（我方视觉第二判据）已删除 —— 它指向的传感器已不在链路里。
• 距离通道尚未接入：distance_valid 恒 False → 控制律走 slow_approach（0.2 m/s 盲飞接近）。 规划中由激光雷达提供（低算力）；双目因 Pi 4 上 YOLO 开销大，暂不接入。
• 全部数值参数为占位值，标定回填前不得实飞。

权威文档：.workbuddy/memory/ARCHITECTURE.md（架构全貌与铁律）、 预定流程-操作指南.md（操作与待办）、工作进度.md（进度）、docs/（规格）。

---

1. 目录结构

code_drone-test/
├── utils/                          # Python 实现（主体）
│   ├── pod_bridge/                 # 吊舱 ↔ PX4 桥接（现行运行链路核心）
│   │   ├── bridge.py               #   PodBridge：L1/L2 运行时切换、目标门控、链路维持
│   │   ├── protocol.py             #   表2-1/2-2/3/4 帧编解码
│   │   ├── pod_serial.py           #   串口链路（115200 8N1）
│   │   ├── px4_gateway.py          #   MAVSDK 遥测订阅 + offboard 指令
│   │   ├── guidance.py             #   L2 地理引导律（飞到坐标）
│   │   ├── ground_link.py          #   地面站 UDP 坐标注入 + 命令队列
│   │   ├── los_consistency.py      #   目标有效性第二判据（LOS 运动学一致性）
│   │   ├── virtual_pod.py          #   虚拟吊舱（SITL/台架）
│   │   └── {run_bridge,listen_pod,pod_console,sim_fc,verify_px4}.py  # 台架工具
│   ├── task_runner/                # 任务级编排（现行）
│   │   ├── state_machine.py        #   WAIT→ARM_TAKEOFF→GOTO→SEARCH→TRACK→RTL
│   │   ├── runner.py               #   编排器（采集快照/动作/异常兜底）
│   │   ├── return_home.py          #   自定义返航（回起飞点→恢复航向→降落，不爬高）
│   │   ├── pod_control.py          #   吊舱识别/锁定指令（0x07/0x0F/0x0E/0x08）
│   │   └── log.py                  #   事件 JSONL + 遥测 CSV + 简报
│   ├── visual_track_control/       # 控制律 + MAVSDK 网关（被运行链路复用）
│   ├── sitl_support.py             # 入口共用：时钟/遥测订阅/连接准备/速率常量
│   ├── config/                     # 配置 JSON（pod_bridge.json 为主线配置）
│   ├── tests/                      # pytest（446 条，仅运行链路）
│   └── README.md                   # utils 索引
├── legacy/                         # 已归档：双目 + YOLO 可选感知后端（175 条测试）
│   ├── utils/                      #   stereo_yolo / stereo_ranger / target_tracker + demo + tests
│   ├── docs/                       #   三份双目规格
│   ├── models/                     #   YOLO 权重
│   └── README.md                   #   归档说明与运行方式
├── tools/                          # 临时/台架脚本（相机标定、视频预览；不参与运行链路）
├── ground_station/                 # 地面站页面（Flask：坐标注入/命令/状态/视频）
├── task_runner_sitl.py             # 任务全流程 SITL 入口（虚拟吊舱）
├── task_runner_real.py             # 任务全流程真机入口（真吊舱串口 + UDP 注入）
├── l1_sitl_test.py                 # L1 航向伺服 SITL 集成测试（只覆盖控制律段）
├── reference-c++/                  # C++ 参考实现（target_tracker / visual_track_control）
├── docs/                           # 规格与方案（权威来源）
└── .workbuddy/memory/              # 项目记忆：ARCHITECTURE.md / MEMORY.md

---

2. 运行链路（现行）

地面站 ──UDP 坐标/命令──▶ ground_link ──▶ TaskRunner（状态机）
                                            │
      ┌─────────────────────────────────────┼──────────────────────────────┐
      ▼                                     ▼                              ▼
  GOTO(L2 飞参考点) ──到点──▶ SEARCH(扫视) ──锁定──▶ TRACK(L1 缓慢靠近) ──超时──▶ RTL
                                                      │                        │
                                          吊舱表2-2 角度/bit2         自定义返航→垂直降落
                                                      ▲                        │
                                            PodBridge ──表3 姿态──▶ 吊舱      PX4 offboard
                                                      │
                                                    PX4 遥测 ◀── MAVSDK

• 状态链：WAIT → ARM_TAKEOFF → GOTO → SEARCH → TRACK → RTL （APPROACH 保留但已非主流程；TRACK 不再因"目标居中"退出）
• 参考点语义：地面坐标是搜索起点，不是终点；到点后转原地扫视。
• 缓慢靠近：复用 TrackingControlLaw 的无距离降级（no_distance_action=slow_approach，0.2 m/s）。
• 返航：不调 PX4 原生 return_to_launch()（会爬到 RTL_RETURN_ALT，SITL 默认 30 m）， 改为保持转场高度回起飞点 → 原地恢复初始航向 → 垂直降落。

---

3. 环境与快速开始

Python 环境（唯一口径）：conda env pytorch （D:\program file\conda\envs\pytorch\python.exe，Python 3.12；含 numpy/mavsdk/pytest/flask/pyserial）。

# 单元测试（621 条）
cd "D:/ProgramData/code_drone-test/utils"
"D:/program file/conda/envs/pytorch/python.exe" -m pytest -q

# 吊舱串口自检（只读；--detect 会下发 0x07）
"D:/program file/conda/envs/pytorch/python.exe" utils/pod_bridge/listen_pod.py COM5 --seconds 5

# S2 台架干跑：只接吊舱、不下发 offboard
"D:/program file/conda/envs/pytorch/python.exe" utils/pod_bridge/run_bridge.py --port COM5 --dry-run --duration 10

# 任务全流程 SITL（需 WSL 里 PX4 SITL）
"D:/program file/conda/envs/pytorch/python.exe" task_runner_sitl.py --conn udpin://0.0.0.0:14540

# 地面站页面
"D:/program file/conda/envs/pytorch/python.exe" ground_station/app.py --udp-port 14555 --log-dir logs

# 真机入口（真吊舱串口 + 地面站 UDP 注入 + PX4）
"D:/program file/conda/envs/pytorch/python.exe" task_runner_real.py --port COM5 \
        --conn udpin://0.0.0.0:14540 --target-type 1        # --land 改为就地降落

MAVSDK 连接串：本机 SITL 用 udpin://0.0.0.0:14540（WSL 镜像网络下可直接连通）。

---

4. 配置

主线配置：utils/config/pod_bridge.json（分组：pod_serial / px4 / link / guidance / heading_servo / tracking / ground_inject）。

• heading_servo：L1 开关、β/α 符号（S1 已标定 +1/−1）、 LOS 一致性判据（los_consistency_enabled / los_window_ms / los_min_yaw_change_deg / los_max_residual_deg）。
• tracking：内嵌整份控制律配置（各分组须齐全）；无距离时看 distance.no_distance_action 与 no_distance_approach_limit_mps。
• ground_inject：地面站 UDP 端口与坐标超龄门限；真机入口会用 timeout_ms 统一 guidance.target_stale_ms（避免"走走停停"）。

任务级参数在 task_runner_{sitl,real}.py 的 build_sm_config() / build_return_config()： 到达半径、搜索/追踪超时、扫视速率、返航速度与航向容差。均为占位值，须台架标定。

---

5. 测试

# 运行链路（446 条）
cd utils && "D:/program file/conda/envs/pytorch/python.exe" -m pytest -q

# 归档的可选后端（175 条，默认不跑）
cd legacy/utils && PYTHONPATH="../../utils" "D:/program file/conda/envs/pytorch/python.exe" -m pytest -q

组	条数	说明
visual_track_control	86	控制律（运行链路复用）
pod_bridge	208	协议/桥接/引导/注入/LOS 一致性
task_runner	122	状态机/编排器/自定义返航/日志/吊舱控制
ground_station	30	页面与日志解读
运行链路合计	446
（归档）stereo_ranger / stereo_yolo / target_tracker	175	在 legacy/，见 legacy/README.md

---

6. C++ 参考实现

reference-c++/ 是 target_tracker 与 visual_track_control 的参考实现（各自独立 CMake 子工程，含 gtest）。Python 侧为逐构件移植 + 行为修复，二者语义对齐。当前无 build 目录， 未复跑 cmake && ctest。构建方式：

cd reference-c++/<lib> && mkdir -p build && cd build && cmake .. && make && ctest

---

7. 许可

本项目遵循开源许可证协议。