精简仓库结构并增强任务链路：归档双目可选后端、新增LOS判据与自定义返航

归档 legacy/：stereo_yolo/stereo_ranger/target_tracker 三包 + 175 条测试 + demo/配置/权重/三份规格；运行链路不再依赖，默认 pytest 446 条，归档测试按需运行。目标有效性第二判据改为 LOS 运动学一致性（pod_bridge/los_consistency.py），删除 require_vision；pod_locked() 叠加该判据，状态机随之后退重搜。自定义返航（task_runner/return_home.py）：保持转场高度回起飞点 → 恢复初始航向 → 垂直降落，不再爬 RTL_RETURN_ALT；未起飞/无起飞点/超时/异常仍退回 PX4 原生 RTL。任务链安全兜底：主循环异常→紧急返航、stop 幂等、landing_stop 保留遥测、in_air 流复用+超时、解锁/起飞与吊舱识别仅执行一次、rtl_timeout 上限。入口共享件拆到 utils/sitl_support.py；pod_sitl_test.py→l1_sitl_test.py；删除旧架构 test.py；相机脚本移入 tools/。新增 .gitignore 并剔除历史误跟踪的 59 个 .pyc；requirements 按实际依赖重写；文档全面对齐，新增 legacy/README.md 与 tools/README.md。
2026-10-21 14:48:41 +08:00
parent 53fdd3b13f
commit df91b1110e
159 changed files with 6121 additions and 1617 deletions
@@ -1,164 +1,182 @@
+# code_drone-test —— 无人机视觉目标追踪与控制系统

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

-本项目包含一个视觉目标追踪与控制系统的 C++ 参考实现及其 Python 移植版本，用于无人机对移动目标的追踪与控制。
+---

-## 项目结构
+## 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-test-drone/
-├── reference-c++/           # C++ 参考实现
-│   ├── target_tracker/      # 目标追踪库（独立子工程）
-│   │   ├── include/         # 头文件目录
-│   │   ├── src/             # 源文件目录
-│   │   └── tests/           # 单元测试
-│   └── visual_track_control/# 视觉追踪控制库
-│       ├── include/         # 头文件目录
-│       ├── src/             # 源文件目录
-│       └── tests/           # 单元测试
-└── utils/                   # Python 移植
-    ├── target_tracker/      # 目标追踪（Python 版）
-    ├── visual_track_control/# 追踪控制（Python 版）
-    └── tests/               # 单元测试
+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
 ```

-## 主要模块
+---

-### target_tracker - 目标追踪库
+## 2. 运行链路（现行）

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

- **KalmanBoxFilter**: 8维卡尔曼滤波器（位置、速度）
- **Tracklet**: 目标轨迹管理与状态更新
- **TargetTracker**: 主追踪器，包含关联、选举逻辑
+- **状态链**：`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），
+  改为保持转场高度回起飞点 → 原地恢复初始航向 → 垂直降落。

-核心特性：
- 贪婪关联算法进行检测匹配
- 主目标选举三级回退机制
- 支持 Coast（纯预测）与 Lost（丢失）状态管理
+---

-### visual_track_control - 视觉追踪控制库
+## 3. 环境与快速开始

-基于追踪结果生成无人机控制指令：
+**Python 环境（唯一口径）**：conda env `pytorch`
+（`D:\program file\conda\envs\pytorch\python.exe`，Python 3.12；含 numpy/mavsdk/pytest/flask/pyserial）。

- **PIDController**: 距离 PID 控制
- **TrackingControlLaw**: 追踪控制律
- **MavlinkCommandGateway**: MAVLink 指令网关
- **PixelToAngle**: 像素到角度转换
- **StalenessChecker**: 数据时效性检查
- **SlewRateLimiter**: 速率限制器
-
-支持多种控制模式：
- **Rate 模式**: 角速率控制
- **Position 模式**: 绝对航向控制
- **距离控制**: 接近/后退策略
-
-## 环境要求
-
-### C++ 版本
- CMake 3.10+
- C++17 编译器
- MAVLink 库（MAVSDK）
-
-### Python 版本
- Python 3.12+
- 依赖：`numpy`, `pymavlink`, `mavsdk`
-
-## 安装
-
-### Python 环境
 ```bash
-pip install -r requirements.txt
+# 单元测试（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 改为就地降落
 ```

-### C++ 构建
+> 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. 测试
+
 ```bash
-cd reference-c++/target_tracker
-mkdir build && cd build
-cmake ..
-make
+# 运行链路（446 条）
+cd utils && "D:/program file/conda/envs/pytorch/python.exe" -m pytest -q

-cd ../../visual_track_control
-mkdir build && cd build
-cmake ..
-make
+# 归档的可选后端（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`。构建方式：

-### Python 测试
 ```bash
-cd utils
-pytest
+cd reference-c++/<lib> && mkdir -p build && cd build && cmake .. && make && ctest
 ```

-### C++ 测试
-```bash
-cd reference-c++/target_tracker/build
-ctest
+---

-cd ../../visual_track_control/build
-ctest
-```
-
-## 配置
-
-C++ 与 Python 版本均使用 JSON 格式配置文件：
-
-```json
-{
-    "control": {
-        "frequency_hz": 20.0,
-        "visual_stale_ms": 200,
-        "radar_stale_ms": 500,
-        "attitude_stale_ms": 300
-    },
-    "camera": {
-        "image_width": 1920,
-        "image_height": 1080,
-        "fov_h_deg": 60.0,
-        "fov_v_deg": 35.0
-    },
-    "heading": {
-        "mode": "rate",
-        "gain": 1.0,
-        "yaw_rate_limit_dps": 30.0,
-        "yaw_slew_limit_dps2": 60.0
-    },
-    "distance": {
-        "d_exp_m": 10.0,
-        "kp": 0.5,
-        "ki": 0.05,
-        "kd": 0.1,
-        "integral_limit": 2.0
-    }
-}
-```
-
-## 使用示例
-
-### Python 追踪演示
-```bash
-cd utils
-python demo_track_control.py
-```
-
-### C++ 追踪演示
-```bash
-cd reference-c++/visual_track_control/build
-./track_control_demo
-```
-
-## 移植说明
-
-Python 版本与 C++ 版的接口差异请参考各子目录的 README：
- [target_tracker Python 版](./utils/target_tracker/README.md)
- [visual_track_control Python 版](./utils/visual_track_control/README.md)
-
-## 许可证
+## 7. 许可

 本项目遵循开源许可证协议。
-
-## 贡献指南
-
-欢迎提交 Issue 和 Pull Request 改进本项目。