精简仓库结构并增强任务链路：归档双目可选后端、新增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。

.gitignore

@@ -0,0 +1,24 @@
+# Qoder local settings (personal MCP config)
+.qoder/settings.local.json
+
+# ---- Python 生成物 ----
+__pycache__/
+*.py[cod]
+.pytest_cache/
+*.egg-info/
+
+# ---- 运行产物（可重跑生成）----
+logs/
+trajectory_plot.png
+l1_sitl_trajectory.csv
+pod_sitl_trajectory.csv
+
+# ---- 大文件 / 外部数据 ----
+legacy/models/*.pt
+stereo_calib_params.npz
+stereo_calib_images/
+
+# ---- 本地工具与索引 ----
+.codegraph/
+.idea/
+.workbuddy/

README.md

@@ -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/
+code_drone-test/
-├── reference-c++/           # C++ 参考实现
+├── utils/                          # Python 实现（主体）
-│   ├── target_tracker/      # 目标追踪库（独立子工程）
+│   ├── pod_bridge/                 # 吊舱 ↔ PX4 桥接（现行运行链路核心）
-│   │   ├── include/         # 头文件目录
+│   │   ├── bridge.py               #   PodBridge：L1/L2 运行时切换、目标门控、链路维持
-│   │   ├── src/             # 源文件目录
+│   │   ├── protocol.py             #   表2-1/2-2/3/4 帧编解码
-│   │   └── tests/           # 单元测试
+│   │   ├── pod_serial.py           #   串口链路（115200 8N1）
-│   └── visual_track_control/# 视觉追踪控制库
+│   │   ├── px4_gateway.py          #   MAVSDK 遥测订阅 + offboard 指令
-│       ├── include/         # 头文件目录
+│   │   ├── guidance.py             #   L2 地理引导律（飞到坐标）
-│       ├── src/             # 源文件目录
+│   │   ├── ground_link.py          #   地面站 UDP 坐标注入 + 命令队列
-│       └── tests/           # 单元测试
+│   │   ├── los_consistency.py      #   目标有效性第二判据（LOS 运动学一致性）
-└── utils/                   # Python 移植
+│   │   ├── virtual_pod.py          #   虚拟吊舱（SITL/台架）
-    ├── target_tracker/      # 目标追踪（Python 版）
+│   │   └── {run_bridge,listen_pod,pod_console,sim_fc,verify_px4}.py  # 台架工具
-    ├── visual_track_control/# 追踪控制（Python 版）
+│   ├── task_runner/                # 任务级编排（现行）
-    └── tests/               # 单元测试
+│   │   ├── 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维卡尔曼滤波器（位置、速度）
+- **状态链**：`WAIT → ARM_TAKEOFF → GOTO → SEARCH → TRACK → RTL`
-- **Tracklet**: 目标轨迹管理与状态更新
+  （`APPROACH` 保留但已非主流程；`TRACK` 不再因"目标居中"退出）
-- **TargetTracker**: 主追踪器，包含关联、选举逻辑
+- **参考点语义**：地面坐标是**搜索起点**，不是终点；到点后转原地扫视。
+- **缓慢靠近**：复用 `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
+# 运行链路（446 条）
-mkdir build && cd build
+cd utils && "D:/program file/conda/envs/pytorch/python.exe" -m pytest -q
-cmake ..
-make
 
-cd ../../visual_track_control
+# 归档的可选后端（175 条，默认不跑）
-mkdir build && cd build
+cd legacy/utils && PYTHONPATH="../../utils" "D:/program file/conda/envs/pytorch/python.exe" -m pytest -q
-cmake ..
-make
 ```
 
-## 测试
+| 组 | 条数 | 说明 |
+|---|---|---|
+| `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
+cd reference-c++/<lib> && mkdir -p build && cd build && cmake .. && make && ctest
-pytest
 ```
 
-### C++ 测试
+---
-```bash
-cd reference-c++/target_tracker/build
-ctest
 
-cd ../../visual_track_control/build
+## 7. 许可
-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)
-
-## 许可证
 
 本项目遵循开源许可证协议。
-
-## 贡献指南
-
-欢迎提交 Issue 和 Pull Request 改进本项目。

docs/L1航向伺服实现方案.md

@@ -1,8 +1,11 @@
-# L1 航向伺服落地方案（无距离，树莓派 / Python）
+# L1 航向伺服落地方案（无距离，树莓派 / Python）
 
 > 目标：**先把"只修正飞行方向、不需要距离"的部分落地**，速度-距离闭环（L2）滞后。
 > 基于结论：本吊舱无激光 → 表4 永不出，且不允许二次开发（部署文档 §10.0 / §10）。
-> 日期：2026-10-09　状态：**S0 已实现（路线 A1 + L1 胶水层），全量 396 passed；符号待台架 S1 标定**
+> 日期：2026-10-09 起，**2026-10-19 更新**　状态：
+> **S0/S1/S2 + SITL + 真吊舱在环已验证**（全量 621 passed）。
+> 后续变更：第二判据 `require_vision` → **LOS 运动学一致性**（§2.3/§6）；
+> 距离源由「双目」改为规划中的「激光雷达」（§7）。
 
 ---
 
@@ -114,7 +117,20 @@ L1 的 `tracked` 判据按阶段递进：
 | 阶段 | 判据 |
 |---|---|
 | 台架（S1~S2） | bit2 锁定 **且** 遥测未超龄（现有 `_pod_locked()`）+ **人工在旁观察**，随时按键 `u` 解锁 |
-| 接入视觉后（S3+） | bit2 **且** 我方 `stereo_yolo`/`target_tracker` 有目标（**第二判据**，必做） |
+| ~~接入视觉后（S3+）~~ | ~~bit2 **且** 我方 `stereo_yolo`/`target_tracker` 有目标~~ |
+| **现行（2026-10-19 起）** | bit2 **且** 遥测未超龄 **且 LOS 运动学一致性**（`pod_bridge/los_consistency.py`） |
+
+> ⚠ **2026-10-19 架构变更**：后续决定**吊舱取代双目 + YOLO 作为唯一目标源**，
+> 因此原「我方视觉第二判据」(`require_vision` / `vision_target_fn`) 已**删除** ——
+> 它要么恒 False（链路永远进不了 TRACK），要么指向吊舱自身（自证，零信息量）。
+>
+> 替代判据：本吊舱无电机、相机刚性固连机体，机头转 Δψ 时真实目标的脱靶量必变
+> ≈−Δψ。故用 `residual = Δβ + Δψ`（3s 窗口，激励门槛 20°、残差上限 8°）判断
+> 「角度是活的还是冻住的」。超限 → `tracked=False` 且 `pod_locked()` 也返回 False
+> （状态机会退出 TRACK 重新搜索，而不是只让控制律单方面刹车）。
+>
+> **局限**：它只能证明「角度是活的」，不能证明「是那个目标」。若吊舱锁错了物体
+> 但角度正常变化，本判据无法识别 —— 那属于识别问题，需另接独立传感器。
 
 ---
 
@@ -149,7 +165,12 @@ L1 的 `tracked` 判据按阶段递进：
   "alpha_from_frame": false,
   "alpha_from_off_target": true,
   "vertical_enabled": true,    // 决策3：无人机 L1 开垂直（vz 由 tracking.vertical 压低）
-  "require_vision": false      // true = 需我方视觉第二判据（S3 起开，§2.3）
+
+  // 第二判据：LOS 运动学一致性（2026-10-19 取代 require_vision，见 §2.3）
+  "los_consistency_enabled": true,   // false → 退回"只信 bit2"
+  "los_window_ms": 3000,             // 比较窗口
+  "los_min_yaw_change_deg": 20.0,    // 窗口内机头至少转这么多才判定（激励门槛）
+  "los_max_residual_deg": 8.0        // |Δβ+Δψ| 上限，超过判不一致
 }
 ```
 
@@ -187,10 +208,10 @@ L1 的 `tracked` 判据按阶段递进：
 | 风险 | 判据 / 对策 |
 |---|---|
 | **符号反了** → 反向转 | S1 标定 + S2 干跑必验；配置化而非硬编码 |
-| bit2 粘滞 → 丢失后仍转 | 第二判据（我方视觉）；台架期人工值守 |
+| bit2 粘滞 → 丢失后仍转 | **LOS 运动学一致性**（`pod_bridge/los_consistency.py`，2026-10-19 取代旧「我方视觉」方案）：机头转 Δψ 而脱靶量不动 → 判丢；台架期仍人工值守 |
 | 框架角定义不符（绝对而非相对机体） | S1 实测确认，不符则改用脱靶量积分等替代 |
 | 云台归中/搜索态框架角乱转 | 仅锁定态启用；非锁定 → `tracked=False` → BRAKE_HOVER |
-| 垂直通道撞地 | L1 强制 `vz_gain=0` |
+| 垂直通道撞地 | L1 强制 `vz_gain=0`（本机实际按决策3 开垂直但压低限速） |
 | 测距噪声（L2 才有） | L2 滞后，且**不用连续 PID**，改分段限速 |
 
 ---
@@ -200,7 +221,8 @@ L1 的 `tracked` 判据按阶段递进：
 L1 落地时就留好口子，将来 L2 只是"换个输入源"，不动结构：
 
 - `ControlSnapshot.distance_valid` / `target_distance_m` / `distance_time_ms` 字段
-  **现在就按 `False` 填全**，L2 时换成 `stereo_ranger` 的 `forward_distance_m`；
+  **现在就按 `False` 填全**；将来接**激光雷达**距离源后填入（原计划的
+  `stereo_ranger.forward_distance_m` 已随"吊舱取代双目"一并搁置）；
 - `bridge.step()` 按配置在 `TrackingControlLaw`（L1）与 `GuidanceLaw`（L2）间切换；
 - `guidance.py` 的输入接口保持不变，等有标定过的距离源再启用。
 
@@ -224,18 +246,22 @@ L1 落地时就留好口子，将来 L2 只是"换个输入源"，不动结构
 | 2 | `visual_track_control/tracking_control_law.py` | ✅ 第④步角度直入优先，否则回退像素 |
 | 3 | `visual_track_control/config.py` | ✅ 拆出 `load_from_dict()` 供 bridge 内嵌复用 |
 | 4 | `pod_bridge/config.py` | ✅ 加 `HeadingServoConfig` + 内嵌 `TrackingConfig`，严格校验符号(±1) |
-| 5 | `pod_bridge/bridge.py` | ✅ `_build_snapshot()`；`step()` 按 enabled 切 L1/L2；`_dispatch` 对 DEGRADED_HOLD 补发刹车保心跳；`vision_target_fn` 注入点(§2.3 第二判据) |
+| 5 | `pod_bridge/bridge.py` | ✅ `_build_snapshot()`；`step()` 按 enabled 切 L1/L2；`_dispatch` 对 DEGRADED_HOLD 补发刹车保心跳；~~`vision_target_fn` 注入点~~ → **已删除，改为 LOS 一致性判据**（`los_consistency.py`，§2.3） |
 | 6 | `config/pod_bridge.json` | ✅ 落 L1 首飞值（slow_approach/0.2、垂直压低、无电机 → 仅脱靶量） |
 | 7 | `tests/` | ✅ 新增 24 条（vtc 5 + config 9 + bridge L1 10）；全量 **396 passed** |
 | 8 | `pod_bridge/run_bridge.py` | ✅ 闭环 CLI 入口：`--dry-run`(S2 只接吊舱/不发 offboard/打印 β→yaw_rate) 与实飞(S3+，`PodBridge.connect`+信号优雅停机)；配套 `run_bridge_test.py` 5 条，全量 **401 passed** |
 | 9 | `pod_bridge/bridge.py:run()` | ✅ 循环周期改按生效律取频率（L1=tracking.control，L2=guidance），修 L1 live 频率错配 |
 | 10 | `pod_bridge/virtual_pod.py` | ✅ 模拟吊舱（SITL/台架工具）：按无电机语义合成表2-2（框架角恒0、无激光、不可见→bit2=0+脱靶量归0），符号与 S1 标定一致 |
-| 11 | `pod_sitl_test.py`（根目录） | ✅ L1 SITL 全流程（test.py 吊舱版）：解锁→起飞→锚点→offboard→L1 闭环(VirtualPod→PodBridge→控制律)→丢目标→刹车→RTL→落地→上锁，轨迹落 CSV；飞行编排不塞进 run_bridge（保持其纯 offboard 闭环入口） |
+| 11 | `l1_sitl_test.py`（根目录，原 `pod_sitl_test.py`） | ✅ L1 SITL 全流程：解锁→起飞→锚点→offboard→L1 闭环(VirtualPod→PodBridge→控制律)→丢目标→刹车→RTL→落地→上锁，轨迹落 CSV；飞行编排不塞进 run_bridge（保持其纯 offboard 闭环入口） |
 | 12 | `tests/pod_bridge/virtual_pod*_test.py` | ✅ 15 条：S1 符号回归 + VirtualPod×PodBridge×控制律集成（右转/下降/慢速前近/丢目标刹车）；全量 **417 passed** |
+| 13 | `pod_bridge/los_consistency.py`（2026-10-19） | ✅ 目标有效性第二判据：LOS 运动学一致性（Δβ+Δψ，3s 窗口/20° 激励/8° 残差）；`pod_locked()` 叠加该判据；配套 `los_consistency_test.py` 12 条 + 桥接层 3 条 |
 
 **待办（非代码，台架）**：~~S1 标定~~ **已完成（2026-10-09）**：目标在左下时 `off_target_x/y`
 均为负 → 吊舱 X 轴右正（与 β 同向）`beta_sign=+1`；Y 轴上正（与 α 下正相反）
 `alpha_sign=-1`（已回填 `pod_bridge.json`，并加标定回归测试守住）。厂商确认本版
-**电机无用** → `beta_from_frame/alpha_from_frame` 恒 false。剩 **S2 干跑**
+**电机无用** → `beta_from_frame/alpha_from_frame` 恒 false。
-`run_bridge.py --dry-run` 复核方向与量级（目标偏左→yaw_rate 负、居中→趋 0），并标定 `heading.gain`；
+
-之后 **SITL 全流程** `python pod_sitl_test.py`（需 PX4 SITL，本机尚未实跑）→ S3 不装桨接飞控。
+**已完成的后续验证（2026-10-19）**：S2 干跑（`run_bridge.py --dry-run`，真吊舱 COM5，20Hz
+闭环无异常）、SITL 全流程（`l1_sitl_test.py` 与 `task_runner_sitl.py` 均实跑）、
+真吊舱在环全流程（`task_runner_real.py --port COM5`，无目标时走"搜索超时→返航"安全路径）。
+剩：**`heading.gain` 标定**、**吊舱真实目标识别（bit2 0→1）**、S3 不装桨接飞控。

docs/树莓派4B部署注意事项.md

@@ -1,4 +1,4 @@
-# pod_bridge 移交树莓派 4B 部署注意事项
+# pod_bridge 移交树莓派 4B 部署注意事项
 
 > 目标平台:树莓派 4B + **Raspberry Pi OS 官方镜像(64 位,Bookworm / Trixie)**。
 > (2026-09-23 由 Ubuntu 24.04 换回官方镜像:Ubuntu 镜像上串口配置与
@@ -48,7 +48,7 @@ pytest      # 仅上机自测需要,部署运行可省
 
 - **mavsdk 4.x 是全新原生 API 重写,与经典 gRPC API 完全不兼容**(2026-09-24
   树莓派实测):没有 `mavsdk.offboard` / `System` / `telemetry`,本仓库全部
-  飞控代码(Px4Gateway / MavsdkCommandGateway / test.py)都是经典 API,
+  飞控代码(Px4Gateway / MavsdkCommandGateway / 各入口脚本)都是经典 API,
   装 4.x 直接 `LegacyModuleError`。**必须锁 `mavsdk<4`**(最新 3.17.4,
   aarch64 官方 wheel `manylinux2014_aarch64`,PyPI 已核对);
   32 位 armv7l 支持不稳定——这是必须 64 位镜像的主因。
@@ -66,7 +66,7 @@ pytest      # 仅上机自测需要,部署运行可省
 
 - 连接串写法(`pod_bridge.json` 的 `px4.connection_string`):
   - Pi 串口直连飞控 TELEM:`serial:///dev/ttyUSB0:57600`(波特率按飞控侧设置);
-  - 经数传/仿真:`udp://:14540`。
+  - 经数传/仿真:`udpin://0.0.0.0:14540`(本机 SITL 实测可用;`udp://:14540` 亦可,语义为监听该端口)。
 - PX4 侧需确认 TELEM 口已配 MAVLink 且 offboard 心跳未被占用。
 
 ## 4. 串口硬件(吊舱侧,最容易翻车)
@@ -251,26 +251,28 @@ python -m pod_bridge.sim_fc /dev/ttyAMA0 --detect person --auto-lock \
 
 | 受影响项 | 后果 |
 |---|---|
-| `guidance.py` 地理位置引导律 | **永久刹车**。桥接层 `bridge.py:169` 的 `target_valid` 依赖表4;表4 恒缺 → `_latest_target` 恒 None → `GuidanceLaw.update()` 每周期走 `_brake("载机位置或目标不可用/超龄")`。安全方向正确(降级刹车、不会乱飞),但**吊舱表4 这条引导路径在本设备上等于不存在** |
+| `guidance.py` 地理位置引导律 | **表4 路径在本设备上恒不可用**(`target_valid` 恒假)。**现行做法:坐标走地面站 UDP 注入**(`ground_link` → `ground_target_fn`),`guidance` 吃注入坐标;表4 仅作回退 |
-| 目标距离/坐标来源 | 必须改由**我方自有链路**提供:`stereo_yolo` 检测 → `stereo_ranger` 双目视距。吊舱只当作"稳定指向 + 表2-2 脱靶量"的伺服/传感器用 |
+| 目标距离/坐标来源 | **坐标 = 地面站注入**(已落地)。**距离尚未接入**:规划由激光雷达提供(低算力);~~`stereo_yolo` 检测 → `stereo_ranger` 双目视距~~ 因 Pi 4 跑不动实时 YOLO,**暂不接入** |
 | `--auto-unlock`(§10.1) | **必然误触发,已实测禁用**:`last_tgt_s` 恒 None 被当成超时。见 §10.1 |
 | 后续选型 | 若要保留吊舱侧目标定位,只能换**带激光测距的吊舱型号**(且需确认激光能接入吊舱解算链路) |
 
-#### 架构该怎么办(分叉已选边)
+#### 架构该怎么办(2026-10-19 已定稿)
 
-既然距离无法回灌、表4 不会有,目标地理坐标就**必须由我方自己解算**:
+既然距离无法回灌、表4 不会有,目标地理坐标就**必须来自吊舱之外**。当时的设想是
+"双目自算坐标";实际实现改成了更简单的分工:
 
 ```
-stereo_yolo 检测 → stereo_ranger 双目视距(斜距/前向距离)
+目标**坐标**  ← 地面站 UDP 注入(ground_link)     ← 已落地
-                → 载机 GPS + 姿态(表3 我方已有) + 吊舱框架角(表2-2)
+目标**方位**  ← 吊舱表2-2 脱靶量(角度直入 L1)     ← 已落地
-                → **我方自己算出目标经纬度**,再喂给 guidance.py
+目标**距离**  ← 待接激光雷达(串口,低算力)        ← 未接
 ```
 
 吊舱在本设备上退化为:**伺服稳定指向 + 表2-2 脱靶量/框架角传感器**。
 它仍然很有用(稳像、跟得住、给得出视场角),只是不再提供坐标。
 
-> 这意味着 `guidance.py` 的输入要从"吊舱表4"换成"我方自算的目标经纬度",
+> ~~下文"stereo_yolo → stereo_ranger 自算经纬度再喂 guidance"的方案~~ 已被上述
-> `bridge.py:169` 的 `target_valid` 判据也要跟着改。**动手前先对齐方案再实现。**
+> 分工取代:`guidance.py` 的输入改为**注入坐标**(已实现),不需要我方自算经纬度。
+> 距离接入后,只影响控制律的 `distance_valid` 分支,不影响坐标来源。
 
 > 复测命令(`--json` 后是**输出产物路径**,非预置输入文件,建议给绝对路径):
 > ```
@@ -298,7 +300,9 @@ SEC 秒重复一次(与"假锁定"形成持续震荡:解锁 → 自动锁定 →
 2. 表4 超时(需先确认表4 存在);
 3. 锁定持续超过 T 秒且**从未**收到表4 → 判为假锁定,解锁一次并告警(不循环);
 4. 脱靶量恒为 0 持续 T 秒 —— 居中跟踪时也会为 0,**误杀风险高**,不推荐;
-5. 由我方视觉链路(stereo_yolo / target_tracker)交叉验证 —— 最可靠,待集成。
+5. ~~由我方视觉链路(stereo_yolo / target_tracker)交叉验证~~ → **已实行为 LOS 运动学一致性**
+   (`pod_bridge/los_consistency.py`,2026-10-19):无电机、刚性固连 → 机头转 Δψ 时脱靶量
+   必变 ≈−Δψ,不动即判丢。无需额外传感器;局限是只证"角度是活的",不证"是那个目标"。
 
 飞行期的正确行为是**刹车保护**(现有 `target_stale_ms` 已覆盖),与"解锁以便重新
 搜索"是两件事,不要混为一谈。

ground_station/app.py

@@ -1,30 +1,193 @@
 """ground_station/app.py —— 地面站页面（Flask，临时测试用）。
 
-预定流程 C 阶段：地面坐标输入 + 状态可视化配套页面。
+预定流程 C 阶段：地面坐标输入 + 任务/吊舱控制 + 状态可视化配套页面。
 
 功能：
-  * GET  /            地面站页面（坐标输入 + 状态面板 + 模拟视野）
+  * GET  /              地面站页面（坐标注入 + 任务控制 + 吊舱控制 + 状态面板 + 模拟视野）
-  * POST /api/inject  接收 {lat, lon, alt}，UDP 发到吊舱桥接的 ground_inject 端口
+  * GET  /px4           PX4 信息子页面（实时遥测 + 任务日志解读分页）
-                      （默认 14555，GroundLink.run() 接收）
+  * POST /api/inject    接收 {lat, lon, alt} 或局部 {north_m, east_m, alt}
-  * GET  /api/state   返回最新任务状态（读任务日志：事件最后状态 + 遥测最后一行）
+                        （局部按注入时刻无人机位置换算为经纬度），UDP 发到 ground_inject
+                        端口（默认 14555，GroundLink.run() 接收）；同时刷新持续注入内容
+  * POST /api/continuous 接收 {on: bool}，启停**服务端**持续注入（后台线程每 0.5s 重发）
+  * POST /api/command   接收 {cmd}（白名单：rtl/emergency/pod_*），同端口 UDP 下发，
+                        由 TaskRunner 主循环 drain_commands() 分派
+  * GET  /api/link     返回链路目标（坐标/命令发往的地址）+ 本机可选地址列表
+  * POST /api/link     {"host":.., "port":..} 切换链路目标（默认 127.0.0.1:14555）
+  * GET  /video_feed    MJPEG 实时视频（服务端 RTSP 拉流转推；未配置 --rtsp-url 时 503）
+  * GET  /api/state     返回最新任务状态（读任务日志：事件最后状态 + 遥测最后一行，
+                        附 continuous 标志与 video 状态）
+  * GET  /api/logs/telemetry  整段遥测 CSV → JSON（子页面图表用；只读）
+  * GET  /api/logs/events     整段事件 JSONL → JSON（子页面解读用；只读）
 
-与无人机 runner 解耦：本页面只做「发坐标（UDP）+ 读日志（只读）」，不侵入 runner。
+与无人机 runner 解耦：本页面只做「发 UDP（坐标/命令）+ 读日志（只读）」，不侵入 runner。
+持续注入放服务端的原因：浏览器会节流不可见标签页的 JS 定时器（实测 0.5s 被降到约
+1 次/分钟，注入超龄致任务误返航），服务端线程不受页面可见性影响。
 用法：
   python ground_station/app.py --udp-port 14555 --log-dir logs
+  python ground_station/app.py --rtsp-url rtsp://192.168.144.119:8554/video
 """
 
 import argparse
 import csv
 import json
+import math
 import os
 import socket
+import sys
+import threading
 from pathlib import Path
 
-from flask import Flask, jsonify, render_template, request
+from flask import (Flask, Response, jsonify, render_template, request,
+                   stream_with_context)
+
+_REPO_ROOT = Path(__file__).resolve().parent.parent   # 兼容「脚本直跑 / 包导入」双模式
+if str(_REPO_ROOT) not in sys.path:
+    sys.path.insert(0, str(_REPO_ROOT))
+
+from ground_station.video_source import RtspVideoSource  # noqa: E402
 
 _GROUND_UDP_PORT = 14555
 _LOG_DIR = "logs"
 
+# 日志解读接口的返回上限（超限返回最近 N 条，防内存/流量失控）
+_LOG_MAX_ROWS = 60000
+_LOG_MAX_EVENTS = 5000
+
+# 允许页面下发的控制命令（与 task_runner/runner.py 的 _POD_COMMAND_METHODS 对齐）
+_COMMAND_WHITELIST = frozenset({
+    "rtl", "emergency",
+    "pod_detect_on", "pod_auto_lock", "pod_stop_track",
+    "pod_detect_off", "pod_center",
+})
+
+
+class _LinkTarget:
+    """链路目标：坐标/命令 UDP 发往的地址（页面可运行时切换）。"""
+
+    def __init__(self, host: str, port: int) -> None:
+        self.host = host
+        self.port = int(port)
+
+
+def _local_ipv4_list() -> list:
+    """本机 IPv4 地址列表（供页面下拉选择；解析失败返回空列表）。"""
+    ips = []
+    try:
+        for info in socket.getaddrinfo(socket.gethostname(), None,
+                                       socket.AF_INET):
+            ip = info[4][0]
+            if ip not in ips:
+                ips.append(ip)
+    except OSError:
+        pass
+    return ips
+
+
+def _is_number(value) -> bool:
+    """数值（排除 bool）。"""
+    return isinstance(value, (int, float)) and not isinstance(value, bool)
+
+
+_DEG_LAT_M = 111320.0  # 局部平面近似的纬度米/度（与 task_runner_sitl 同口径）
+
+
+def _offset_to_latlon(lat0: float, lon0: float, north_m: float,
+                      east_m: float) -> tuple:
+    """北东偏移（米）→ 经纬度（局部平面近似，小范围够用）。"""
+    lat = lat0 + north_m / _DEG_LAT_M
+    lon = lon0 + east_m / (_DEG_LAT_M * math.cos(math.radians(lat0)))
+    return lat, lon
+
+
+def _session_start_position(log_dir: str):
+    """本次任务运行段（遥测最后一段）的首个有效定位 ≈ 起飞点/home。
+
+    运行段按 t_s 回退切分；取最后一段里第一条 pos_valid=True 的经纬度。
+    无日志 / 无有效定位 → (None, None)。
+    """
+    path = Path(log_dir) / "task_telemetry.csv"
+    try:
+        with open(path, encoding="utf-8", newline="") as f:
+            rows = list(csv.DictReader(f))
+    except OSError:
+        return None, None
+    start = 0
+    for i in range(1, len(rows)):
+        try:
+            if float(rows[i].get("t_s") or 0) < float(rows[i - 1].get("t_s") or 0):
+                start = i
+        except ValueError:
+            continue
+    for row in rows[start:]:
+        if (row.get("pos_valid") == "True" and row.get("latitude_deg")
+                and row.get("longitude_deg")):
+            try:
+                return float(row["latitude_deg"]), float(row["longitude_deg"])
+            except ValueError:
+                continue
+    return None, None
+
+
+def _send_udp(target: _LinkTarget, payload: dict) -> None:
+    """把 JSON 发到链路目标（坐标注入 / 持续注入 / 控制命令共用）。"""
+    data = json.dumps(payload, ensure_ascii=False)
+    with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s:
+        s.sendto(data.encode("utf-8"), (target.host, target.port))
+
+
+class _ContinuousInjector:
+    """服务端持续注入：后台线程按固定周期重发最近一次坐标。
+
+    页面 JS 定时器在标签页不可见时被浏览器节流（实测 0.5s → 约 1 次/分钟，
+    注入超龄致任务误返航），故持续注入放服务端；手动「注入坐标」同步刷新内容。
+    """
+
+    def __init__(self, target: _LinkTarget, interval_s: float = 0.5) -> None:
+        self._target = target
+        self._interval_s = max(0.05, float(interval_s))
+        self._lock = threading.Lock()
+        self._payload = None
+        self._stop = threading.Event()
+        self._thread = None
+
+    @property
+    def running(self) -> bool:
+        return self._thread is not None and self._thread.is_alive()
+
+    def update(self, payload: dict) -> None:
+        with self._lock:
+            self._payload = payload
+
+    def start(self) -> bool:
+        """开始持续注入；尚无坐标（未手动注入过）→ False。"""
+        with self._lock:
+            if self._payload is None:
+                return False
+        if self.running:
+            return True
+        self._stop.clear()
+        self._thread = threading.Thread(target=self._loop,
+                                        name="continuous-inject", daemon=True)
+        self._thread.start()
+        return True
+
+    def stop(self) -> None:
+        self._stop.set()
+        thread, self._thread = self._thread, None
+        if thread is not None:
+            thread.join(timeout=2.0)
+
+    def _loop(self) -> None:
+        while not self._stop.is_set():
+            with self._lock:
+                payload = self._payload
+            if payload is not None:
+                try:
+                    _send_udp(self._target, payload)
+                except OSError:
+                    pass  # 端口未监听等：静默重试
+            self._stop.wait(self._interval_s)
+
 
 def _read_last_line(path: Path):
     """读文件最后一行（tail，避免读全文）。"""
@@ -80,41 +243,197 @@ def _latest_state(log_dir: str) -> dict:
             pass
 
     return {"state": state, "telemetry": telemetry,
-            "events": events[-20:], "log_dir": str(log_path)}
+            "events": events[-40:], "log_dir": str(log_path)}
 
 
-def create_app(udp_port: int, log_dir: str) -> Flask:
+def create_app(udp_port: int, log_dir: str,
+               udp_host: str = "127.0.0.1",
+               inject_interval_s: float = 0.5,
+               rtsp_url: str = "",
+               video_width: int = 960,
+               video_fps: float = 15.0) -> Flask:
     app = Flask(__name__)
+    target = _LinkTarget(udp_host, udp_port)
+    injector = _ContinuousInjector(target, inject_interval_s)
+    video = None
+    if rtsp_url:
+        video = RtspVideoSource(rtsp_url, width=video_width, fps=video_fps)
+        video.start()
 
     @app.route("/")
     def index():
         return render_template("index.html", udp_port=udp_port)
 
+    @app.route("/px4")
+    def px4_page():
+        return render_template("px4.html")
+
+    @app.route("/api/logs/telemetry")
+    def logs_telemetry():
+        """整段遥测 CSV → JSON（fields + 行数组；前端负责分段/绘图）。只读。"""
+        path = Path(log_dir) / "task_telemetry.csv"
+        fields, rows = [], []
+        try:
+            with open(path, encoding="utf-8", newline="") as f:
+                for i, row in enumerate(csv.reader(f)):
+                    if i == 0:
+                        fields = row
+                    elif row:
+                        rows.append(row)
+        except OSError:
+            pass
+        total = len(rows)
+        if total > _LOG_MAX_ROWS:
+            rows = rows[-_LOG_MAX_ROWS:]
+        return jsonify({"fields": fields, "rows": rows,
+                        "total": total, "returned": len(rows)})
+
+    @app.route("/api/logs/events")
+    def logs_events():
+        """整段事件 JSONL → JSON 列表（前端负责分段/解读）。只读。"""
+        path = Path(log_dir) / "task_events.jsonl"
+        events = []
+        try:
+            with open(path, encoding="utf-8") as f:
+                for line in f:
+                    line = line.strip()
+                    if not line:
+                        continue
+                    try:
+                        events.append(json.loads(line))
+                    except json.JSONDecodeError:
+                        continue
+        except OSError:
+            pass
+        total = len(events)
+        if total > _LOG_MAX_EVENTS:
+            events = events[-_LOG_MAX_EVENTS:]
+        return jsonify({"events": events, "total": total})
+
     @app.route("/api/inject", methods=["POST"])
     def inject():
         data = request.get_json(silent=True) or {}
-        lat = data.get("lat")
+        try:
-        lon = data.get("lon")
+            alt = float(data.get("alt", 0.0) or 0.0)
+        except (TypeError, ValueError):
+            return jsonify({"ok": False, "error": "alt 必须是数值"}), 400
+
+        local = None
+        if "north_m" in data or "east_m" in data:
+            # 局部坐标：按选定原点换算为经纬度
+            #   current（默认）= 注入时刻无人机位置；home = 本次运行段起始位置（≈起飞点）
+            north, east = data.get("north_m"), data.get("east_m")
+            if not (_is_number(north) and _is_number(east)):
+                return jsonify({"ok": False,
+                                "error": "north_m/east_m 必须是数值"}), 400
+            origin_mode = data.get("origin", "current")
+            if origin_mode not in ("current", "home"):
+                return jsonify({"ok": False,
+                                "error": "origin 必须是 current 或 home"}), 400
+            if origin_mode == "home":
+                lat0, lon0 = _session_start_position(log_dir)
+                if lat0 is None:
+                    return jsonify({"ok": False,
+                                    "error": "无本次运行的有效起始位置（遥测缺失/"
+                                             "定位无效），无法按起飞点换算"}), 400
+            else:
+                tel = (_latest_state(log_dir).get("telemetry") or {})
+                lat0, lon0 = tel.get("latitude_deg"), tel.get("longitude_deg")
+                if tel.get("pos_valid") != "True" or not lat0 or not lon0:
+                    return jsonify({"ok": False,
+                                    "error": "无人机位置无效（无遥测/无 GPS），"
+                                             "无法换算局部坐标"}), 400
+                lat0, lon0 = float(lat0), float(lon0)
+            lat, lon = _offset_to_latlon(lat0, lon0, float(north), float(east))
+            local = {"north_m": float(north), "east_m": float(east),
+                     "origin": origin_mode,
+                     "origin_lat": lat0, "origin_lon": lon0}
+        else:
+            lat, lon = data.get("lat"), data.get("lon")
             if lat is None or lon is None:
-            return jsonify({"ok": False, "error": "需要 lat 和 lon"}), 400
+                return jsonify({"ok": False,
+                                "error": "需要 lat/lon 或 north_m/east_m"}), 400
             try:
                 lat = float(lat)
                 lon = float(lon)
             except (TypeError, ValueError):
-            return jsonify({"ok": False, "error": "lat/lon 必须是数值"}), 400
+                return jsonify({"ok": False,
-        alt = float(data.get("alt", 0.0) or 0.0)
+                                "error": "lat/lon 必须是数值"}), 400
 
-        payload = json.dumps({"lat": lat, "lon": lon, "alt": alt})
         try:
-            with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s:
+            _send_udp(target, {"lat": lat, "lon": lon, "alt": alt})
-                s.sendto(payload.encode("utf-8"), ("127.0.0.1", udp_port))
         except OSError as exc:
             return jsonify({"ok": False, "error": f"UDP 发送失败: {exc}"}), 500
-        return jsonify({"ok": True, "sent": {"lat": lat, "lon": lon, "alt": alt}})
+        injector.update({"lat": lat, "lon": lon, "alt": alt})  # 刷新持续注入内容
+        resp = {"ok": True, "sent": {"lat": lat, "lon": lon, "alt": alt}}
+        if local is not None:
+            resp["local"] = local
+        return jsonify(resp)
+
+    @app.route("/api/continuous", methods=["POST"])
+    def continuous():
+        data = request.get_json(silent=True) or {}
+        on = data.get("on")
+        if not isinstance(on, bool):
+            return jsonify({"ok": False, "error": "on 必须是布尔值"}), 400
+        if on and not injector.start():
+            return jsonify({"ok": False,
+                            "error": "尚未注入过坐标，请先点「注入坐标」"}), 400
+        if not on:
+            injector.stop()
+        return jsonify({"ok": True, "continuous": injector.running})
+
+    @app.route("/api/command", methods=["POST"])
+    def command():
+        data = request.get_json(silent=True) or {}
+        cmd = data.get("cmd")
+        if not isinstance(cmd, str) or cmd not in _COMMAND_WHITELIST:
+            return jsonify({"ok": False, "error": f"未知命令: {cmd!r}"}), 400
+        try:
+            _send_udp(target, {"cmd": cmd})
+        except OSError as exc:
+            return jsonify({"ok": False, "error": f"UDP 发送失败: {exc}"}), 500
+        return jsonify({"ok": True, "sent": {"cmd": cmd}})
+
+    @app.route("/api/link")
+    def link_get():
+        options = [{"label": "本地回环（仿真）", "host": "127.0.0.1"}]
+        for ip in _local_ipv4_list():
+            if not ip.startswith("127."):
+                options.append({"label": f"本机网卡 {ip}", "host": ip})
+        return jsonify({"host": target.host, "port": target.port,
+                        "options": options})
+
+    @app.route("/api/link", methods=["POST"])
+    def link_set():
+        data = request.get_json(silent=True) or {}
+        host = str(data.get("host") or "").strip()
+        port = data.get("port", target.port)
+        if not host or len(host) > 64 or any(c.isspace() for c in host):
+            return jsonify({"ok": False, "error": "host 不合法"}), 400
+        try:
+            port = int(port)
+        except (TypeError, ValueError):
+            return jsonify({"ok": False, "error": "port 必须是整数"}), 400
+        if not (1 <= port <= 65535):
+            return jsonify({"ok": False, "error": "port 超出范围"}), 400
+        target.host, target.port = host, port
+        return jsonify({"ok": True, "host": target.host, "port": target.port})
 
     @app.route("/api/state")
     def state():
-        return jsonify(_latest_state(log_dir))
+        j = _latest_state(log_dir)
+        j["continuous"] = injector.running
+        j["video"] = {"enabled": video is not None,
+                      "online": bool(video and video.online)}
+        return jsonify(j)
+
+    @app.route("/video_feed")
+    def video_feed():
+        if video is None:
+            return "未配置视频源（启动参数 --rtsp-url）", 503
+        return Response(stream_with_context(video.mjpeg_stream()),
+                        mimetype="multipart/x-mixed-replace; boundary=frame")
 
     return app
 
@@ -124,13 +443,27 @@ def main() -> int:
     ap.add_argument("--host", default="127.0.0.1")
     ap.add_argument("--port", type=int, default=5000, help="Flask 监听端口")
     ap.add_argument("--udp-port", type=int, default=_GROUND_UDP_PORT,
-                    help="坐标注入 UDP 端口（GroundLink 监听）")
+                    help="坐标/命令注入 UDP 端口（GroundLink 监听）")
+    ap.add_argument("--udp-host", default="127.0.0.1",
+                    help="坐标/命令注入目标地址（默认本机；真机可填机载/伴侣机 IP，"
+                         "页面也可运行时切换）")
     ap.add_argument("--log-dir", default=_LOG_DIR, help="任务日志目录")
+    ap.add_argument("--rtsp-url", default="",
+                    help="吊舱 RTSP 视频地址（如 rtsp://192.168.144.119:8554/video）；"
+                         "缺省则视频面板关闭")
+    ap.add_argument("--video-width", type=int, default=960,
+                    help="MJPEG 输出最大宽度（默认 960）")
+    ap.add_argument("--video-fps", type=float, default=15.0,
+                    help="MJPEG 编码帧率上限（默认 15）")
     args = ap.parse_args()
 
-    app = create_app(args.udp_port, args.log_dir)
+    app = create_app(args.udp_port, args.log_dir, udp_host=args.udp_host,
-    print(f"[地面站] http://{args.host}:{args.port} | UDP 注入 → 127.0.0.1:{args.udp_port}"
+                     rtsp_url=args.rtsp_url,
-          f" | 日志 → {args.log_dir}")
+                     video_width=args.video_width, video_fps=args.video_fps)
+    video_note = f" | 视频 ← {args.rtsp_url}" if args.rtsp_url else " | 视频未配置"
+    print(f"[地面站] http://{args.host}:{args.port} | UDP 注入 → "
+          f"{args.udp_host}:{args.udp_port}"
+          f" | 日志 → {args.log_dir}{video_note}")
     app.run(host=args.host, port=args.port, debug=False)
     return 0
 

ground_station/templates/index.html

@@ -23,6 +23,11 @@
   button { margin-top:12px; padding:9px 16px; border:none; border-radius:6px;
            background:var(--green); color:var(--green-ink); font-size:14px; cursor:pointer; }
   button:active { opacity:.85; }
+  button.sec { background:#5b6a5e; }
+  button.danger { background:var(--red); color:#fff; }
+  .btn-row { display:flex; flex-wrap:wrap; gap:8px; }
+  .btn-row button { margin-top:0; flex:1 1 auto; }
+  input[type=checkbox] { width:auto; }
   .kv { display:flex; justify-content:space-between; padding:5px 0;
         border-bottom:1px dashed var(--line); font-size:13px; }
   .kv:last-child { border-bottom:none; }
@@ -33,27 +38,97 @@
   .badge.warn { background:var(--red); color:#fff; }
   canvas { background:#0e1410; border-radius:8px; display:block; }
   .hint { color:var(--mut); font-size:12px; margin-top:8px; }
+  .nav { margin-top:8px; font-size:13px; }
+  .nav a { color:var(--green); text-decoration:none; margin-right:14px; }
+  .nav a.active { color:var(--ink); font-weight:600; text-decoration:underline; }
+  select { width:100%; padding:8px; border:1px solid var(--line); border-radius:6px;
+           font-size:14px; background:#fff; color:var(--ink); }
+  .badge.mut { background:#c9c3b2; color:#4a514a; }
+  .log-list { max-height:300px; overflow:auto; font-size:12.5px;
+              font-family:Consolas,"Courier New",monospace; border:1px solid var(--line);
+              border-radius:8px; padding:8px 10px; }
+  .log-line { padding:2px 0; border-bottom:1px dashed var(--line); word-break:break-all; }
+  .log-line:last-child { border-bottom:none; }
+  .log-t { color:var(--mut); margin-right:6px; }
+  .lv-state { color:var(--green); }
+  .lv-info { color:#3a6ea5; }
+  .lv-warn { color:#b06a2a; }
+  .lv-err { color:var(--red); }
 </style>
 </head>
 <body>
 <header>
   <h1>地面站 · 预定流程</h1>
-  <div class="sub">地面坐标注入 + 状态可视化（临时测试用）</div>
+  <div class="sub">坐标注入 + 任务/吊舱控制 + 状态可视化（临时测试用）</div>
+  <nav class="nav"><a class="active" href="/">主页面</a><a href="/px4">PX4 信息</a></nav>
 </header>
 
 <div class="wrap">
+  <div class="col">
+    <h2>链路目标 <span class="badge" id="link-badge">-</span></h2>
+    <label>坐标与命令经 UDP 发往此地址</label>
+    <select id="link-sel"></select>
+    <input id="link-custom" type="text" placeholder="自定义 IP，如 192.168.144.10" style="display:none;margin-top:8px">
+    <button onclick="applyLink()">切换目标</button>
+    <div class="hint" id="link-msg">默认「本地回环」= 本机跑仿真/runner；真机可切到机载或伴侣机 IP。</div>
+  </div>
+
   <div class="col">
     <h2>坐标注入</h2>
-    <label>纬度 lat</label>
+    <label>坐标形式</label>
+    <select id="coord-mode" onchange="applyCoordMode()">
+      <option value="geo">经纬度（绝对）</option>
+      <option value="local">局部坐标（相对原点，北/东 米）</option>
+    </select>
+    <div id="origin-row" style="display:none">
+      <label>局部原点</label>
+      <select id="origin-mode">
+        <option value="current">以当前坐标为原点（注入时刻无人机位置）</option>
+        <option value="home">以起飞点（home）为原点（本次运行起始位置）</option>
+      </select>
+    </div>
+    <label id="lbl-a">纬度 lat</label>
     <input id="lat" type="text" value="47.398241" placeholder="如 47.398241">
-    <label>经度 lon</label>
+    <label id="lbl-b">经度 lon</label>
     <input id="lon" type="text" value="8.546363" placeholder="如 8.546363">
     <label>高度 alt (m)</label>
     <input id="alt" type="text" value="0">
-    <button onclick="sendInject()">注入坐标</button>
+    <button id="inject-btn" onclick="sendInject()">注入坐标</button>
+    <div class="hint">局部坐标按选定原点换算（起飞前注入即为起飞点；「当前坐标」模式随注入时刻无人机位置变化）。</div>
+    <label style="display:flex;align-items:center;gap:6px;margin-top:10px">
+      <input type="checkbox" id="cont" onchange="toggleContinuous()">
+      持续注入（服务端每 0.5s 自动重发）
+    </label>
+    <div class="hint">坐标须持续注入：断流超过 5s 判失效（任务将返航）。持续注入由服务端执行，
+      切窗口/最小化不受影响；取消勾选或关闭地面站程序才停止。</div>
     <div class="hint" id="inject-msg"></div>
   </div>
 
+  <div class="col">
+    <h2>任务控制</h2>
+    <div class="btn-row">
+      <button onclick="sendCmd('rtl', '确认请求返航（RTL）？')">请求返航 RTL</button>
+      <button class="danger" onclick="sendCmd('emergency', '确认紧急终止（立即返航）？')">紧急终止</button>
+    </div>
+    <div class="hint">RTL = 人工返航请求；紧急 = 最高优先级立即返航（均经 UDP 下发到 runner）。</div>
+    <div class="hint" id="cmd-msg-t"></div>
+  </div>
+
+  <div class="col">
+    <h2>吊舱控制</h2>
+    <div class="btn-row">
+      <button class="sec" onclick="sendCmd('pod_detect_on')">开识别</button>
+      <button class="sec" onclick="sendCmd('pod_auto_lock')">自动锁定</button>
+      <button class="sec" onclick="sendCmd('pod_stop_track')">停跟踪</button>
+      <button class="sec" onclick="sendCmd('pod_detect_off')">关识别</button>
+      <button class="sec" onclick="sendCmd('pod_center')">归中</button>
+    </div>
+    <div class="hint">对齐 pod_console 按键：开识别 d(0x07+0x0F) / 自动锁定 a(0x0F+0x10) /
+      停跟踪 u(0x0E，仅解当前锁，0x0F 自动锁定仍常驻) / 关识别 s(0x0E+0x08，彻底退出) / 归中 c(0x2B)</div>
+    <div class="hint">真机经 runner → 吊舱串口下发；SITL 无吊舱控制口，命令将被忽略。</div>
+    <div class="hint" id="cmd-msg-p"></div>
+  </div>
+
   <div class="col">
     <h2>任务状态 <span class="badge" id="state">-</span></h2>
     <div class="kv"><span class="k">位置</span><span class="v" id="pos">-</span></div>
@@ -65,28 +140,108 @@
     <div class="hint" id="log-dir"></div>
   </div>
 
+  <div class="col" style="flex:1 1 400px">
+    <h2>任务日志 <span class="badge mut" id="log-count">0</span></h2>
+    <div class="log-list" id="log-list"><div class="log-line">加载中…</div></div>
+    <div class="hint">阶段切换 / 地面站命令 / 飞控状态（STATUSTEXT 警告）实时滚动</div>
+  </div>
+
   <div class="col">
     <h2>模拟视野（脱靶量）</h2>
     <canvas id="view" width="300" height="200"></canvas>
     <div class="hint">十字线 = 视野中心，红点 = 目标（按吊舱脱靶量映射）</div>
   </div>
+
+  <div class="col">
+    <h2>吊舱视频 <span class="badge" id="vid-badge">-</span></h2>
+    <img id="vid" alt="吊舱视频流" style="display:none; width:320px; border-radius:8px; background:#0e1410">
+    <div class="hint" id="vid-hint">检测中…</div>
+  </div>
 </div>
 
 <script>
 const FOV_H = 60, FOV_V = 35;
+let vidReloadAt = 0;
 
 function $(id){ return document.getElementById(id); }
 
+function applyCoordMode(){
+  const local = $("coord-mode").value === "local";
+  $("origin-row").style.display = local ? "block" : "none";
+  $("lbl-a").textContent = local ? "北向偏移 north (m)" : "纬度 lat";
+  $("lbl-b").textContent = local ? "东向偏移 east (m)" : "经度 lon";
+  $("lat").value = local ? "30" : "47.398241";
+  $("lon").value = local ? "15" : "8.546363";
+  $("lat").placeholder = local ? "如 30（北为正）" : "如 47.398241";
+  $("lon").placeholder = local ? "如 15（东为正）" : "如 8.546363";
+  $("inject-btn").textContent = local ? "注入局部坐标" : "注入坐标";
+}
+
 async function sendInject(){
-  const lat = $("lat").value, lon = $("lon").value, alt = $("alt").value || "0";
+  const local = $("coord-mode").value === "local";
+  const a = $("lat").value, b = $("lon").value, alt = $("alt").value || "0";
   const msg = $("inject-msg");
+  const body = local
+    ? {north_m: Number(a), east_m: Number(b), alt: Number(alt),
+       origin: $("origin-mode").value}
+    : {lat: Number(a), lon: Number(b), alt: Number(alt)};
   try {
     const r = await fetch("/api/inject", {
       method:"POST", headers:{"Content-Type":"application/json"},
-      body: JSON.stringify({lat:Number(lat), lon:Number(lon), alt:Number(alt)})
+      body: JSON.stringify(body)
     });
     const j = await r.json();
-    msg.textContent = j.ok ? `已注入 (${j.sent.lat}, ${j.sent.lon})` : `失败: ${j.error}`;
+    if (j.ok) {
+      if (local && j.local){
+        const orgLabel = j.local.origin === "home" ? "起飞点" : "当前坐标";
+        msg.textContent = `已注入（局部 北${j.local.north_m}m/东${j.local.east_m}m → `
+          + `${j.sent.lat.toFixed(6)}, ${j.sent.lon.toFixed(6)}；`
+          + `原点(${orgLabel}) ${j.local.origin_lat.toFixed(6)}, ${j.local.origin_lon.toFixed(6)}）`;
+      } else {
+        msg.textContent = `已注入 (${j.sent.lat}, ${j.sent.lon})`;
+      }
+      msg.style.color = "var(--green)";
+    } else {
+      msg.textContent = `失败: ${j.error}`;
+      msg.style.color = "var(--red)";
+    }
+  } catch(e){ msg.textContent = "请求失败: "+e; msg.style.color = "var(--red)"; }
+}
+
+async function toggleContinuous(){
+  const on = $("cont").checked;
+  const msg = $("inject-msg");
+  try {
+    const r = await fetch("/api/continuous", {
+      method:"POST", headers:{"Content-Type":"application/json"},
+      body: JSON.stringify({on})
+    });
+    const j = await r.json();
+    if (!j.ok) {
+      $("cont").checked = !on;          // 失败回滚勾选
+      msg.textContent = `失败: ${j.error}`;
+      msg.style.color = "var(--red)";
+      return;
+    }
+    msg.textContent = j.continuous ? "持续注入中（服务端发送，每 0.5s）" : "已停止持续注入";
+    msg.style.color = j.continuous ? "var(--green)" : "var(--mut)";
+  } catch(e){
+    $("cont").checked = !on;
+    msg.textContent = "请求失败: "+e;
+    msg.style.color = "var(--red)";
+  }
+}
+
+async function sendCmd(cmd, ask){
+  if (ask && !confirm(ask)) return;
+  const msg = (cmd==="rtl" || cmd==="emergency") ? $("cmd-msg-t") : $("cmd-msg-p");
+  try {
+    const r = await fetch("/api/command", {
+      method:"POST", headers:{"Content-Type":"application/json"},
+      body: JSON.stringify({cmd})
+    });
+    const j = await r.json();
+    msg.textContent = j.ok ? `已发送: ${j.sent.cmd}` : `失败: ${j.error}`;
     msg.style.color = j.ok ? "var(--green)" : "var(--red)";
   } catch(e){ msg.textContent = "请求失败: "+e; msg.style.color = "var(--red)"; }
 }
@@ -126,10 +281,142 @@ async function poll(){
     $("mode").textContent = t.mode || "-";
     $("cmd").textContent = t.yaw_rate_dps ? `${fmt(t.yaw_rate_dps,1)}°/s / ${fmt(t.vx_body_mps,2)} m/s` : "-";
     $("log-dir").textContent = "日志: " + (j.log_dir || "-");
+    // 持续注入开关与服务端同步（刷新/多标签时不失真）
+    if (typeof j.continuous === "boolean" && $("cont").checked !== j.continuous) {
+      $("cont").checked = j.continuous;
+      const m = $("inject-msg");
+      m.textContent = j.continuous ? "持续注入中（服务端发送，每 0.5s）" : "";
+      m.style.color = j.continuous ? "var(--green)" : "var(--mut)";
+    }
+    // 吊舱视频面板：状态徽标 + 断流自动重挂（限频 5s，避免打断重连）
+    const v = j.video || {};
+    const img = $("vid"), badge = $("vid-badge");
+    if (!v.enabled) {
+      img.style.display = "none";
+      badge.textContent = "未配置"; badge.className = "badge warn";
+      $("vid-hint").textContent = "服务端未配置视频源（--rtsp-url）";
+    } else {
+      img.style.display = "block";
+      badge.textContent = v.online ? "在线" : "断流";
+      badge.className = "badge" + (v.online ? "" : " warn");
+      $("vid-hint").textContent = v.online
+        ? "服务端 RTSP → MJPEG 实时画面"
+        : "视频流中断，自动重连中…";
+      const now = Date.now(), src = img.getAttribute("src");
+      if (!v.online) {
+        if (!src || now - vidReloadAt > 5000) { img.src = "/video_feed?t=" + now; vidReloadAt = now; }
+      } else if (!src) {
+        img.src = "/video_feed";
+      }
+    }
+    // 任务日志面板（阶段切换 / 地面站命令 / 飞控状态）
+    renderTaskLog(j.events || []);
     drawView(parseFloat(t.off_target_x_deg), parseFloat(t.off_target_y_deg));
   } catch(e){ /* 后端未起时静默 */ }
 }
 
+// ============================================================ 链路目标
+let linkPopulated = false;
+
+async function loadLink(){
+  try {
+    const r = await fetch("/api/link");
+    const j = await r.json();
+    $("link-badge").textContent = `${j.host}:${j.port}`;
+    if (linkPopulated) return;
+    const sel = $("link-sel");
+    sel.innerHTML = "";
+    for (const o of j.options){
+      const opt = document.createElement("option");
+      opt.value = o.host;
+      opt.textContent = `${o.label}（${o.host}）`;
+      sel.appendChild(opt);
+    }
+    const c = document.createElement("option");
+    c.value = "__custom__"; c.textContent = "自定义…";
+    sel.appendChild(c);
+    sel.value = j.host;
+    if (sel.value !== j.host){          // 当前目标不在列表 → 自定义项承载
+      sel.value = "__custom__";
+      $("link-custom").value = j.host;
+      $("link-custom").style.display = "block";
+    }
+    sel.onchange = () => {
+      $("link-custom").style.display = sel.value === "__custom__" ? "block" : "none";
+    };
+    linkPopulated = true;
+  } catch(e){ /* 后端未起时静默 */ }
+}
+
+async function applyLink(){
+  const sel = $("link-sel");
+  const host = sel.value === "__custom__" ? $("link-custom").value.trim() : sel.value;
+  const msg = $("link-msg");
+  try {
+    const r = await fetch("/api/link", {
+      method:"POST", headers:{"Content-Type":"application/json"},
+      body: JSON.stringify({host})
+    });
+    const j = await r.json();
+    msg.textContent = j.ok ? `已切换到 ${j.host}:${j.port}` : `失败: ${j.error}`;
+    msg.style.color = j.ok ? "var(--green)" : "var(--red)";
+    if (j.ok) $("link-badge").textContent = `${j.host}:${j.port}`;
+  } catch(e){ msg.textContent = "请求失败: " + e; msg.style.color = "var(--red)"; }
+}
+
+// ============================================================ 任务日志
+const STATE_CN = {
+  wait: "待命", arm_takeoff: "起飞", goto: "坐标引导", track: "追踪",
+  approach: "接近悬停", rtl: "返航"
+};
+let lastLogKey = "";
+
+function fmtT(t){
+  if (t === null || t === undefined) return "--:--.-";
+  const m = Math.floor(t / 60), s = t % 60;
+  return String(m).padStart(2, "0") + ":" + s.toFixed(1).padStart(4, "0");
+}
+
+function evLine(e){
+  if (e.type === "state"){
+    const a = STATE_CN[e.from_state] || e.from_state || "?";
+    const b = STATE_CN[e.to_state] || e.to_state || "?";
+    return { cls: "lv-state", txt: `阶段切换: ${a} → ${b}（${e.reason || ""}）` };
+  }
+  if (e.type === "command") return { cls: "lv-info", txt: `地面站命令: ${e.name}` };
+  if (e.type === "fc_status"){
+    const sev = String(e.severity || "").toUpperCase();
+    const cls = sev.includes("ERR") ? "lv-err" : (sev.includes("WARN") ? "lv-warn" : "lv-info");
+    return { cls, txt: `飞控 ${sev || "STATUS"}: ${e.text || ""}` };
+  }
+  if (e.type === "takeoff") return { cls: "lv-state", txt: `起飞至 ${e.altitude_m} m` };
+  if (e.type === "takeoff_failed") return { cls: "lv-err", txt: `起飞失败: ${e.error || ""}` };
+  if (e.type === "rtl") return { cls: "lv-state", txt: "开始返航" };
+  const parts = [];
+  for (const [k, v] of Object.entries(e)){
+    if (["type", "ts_ms", "t_s"].includes(k)) continue;
+    parts.push(`${k}=${v}`);
+  }
+  return { cls: "", txt: `${e.type || "事件"}: ${parts.join(", ")}` };
+}
+
+function renderTaskLog(evs){
+  const tail = evs.length ? evs[evs.length - 1] : null;
+  const key = evs.length + "|" + (tail ? `${tail.ts_ms}|${tail.type}` : "");
+  if (key === lastLogKey) return;       // 无变化不重建，避免打断滚动
+  lastLogKey = key;
+  const box = $("log-list");
+  box.innerHTML = evs.length
+    ? evs.map(e => {
+        const l = evLine(e);
+        return `<div class="log-line ${l.cls}"><span class="log-t">${fmtT(e.t_s)}</span>${l.txt}</div>`;
+      }).join("")
+    : '<div class="log-line">暂无事件（等待任务运行）</div>';
+  box.scrollTop = box.scrollHeight;
+  $("log-count").textContent = evs.length;
+}
+
+loadLink();
 poll();
 setInterval(poll, 1000);
 </script>

ground_station/templates/px4.html

@@ -0,0 +1,564 @@
+<!DOCTYPE html>
+<html lang="zh">
+<head>
+<meta charset="utf-8">
+<meta name="viewport" content="width=device-width, initial-scale=1">
+<title>地面站 · PX4 信息</title>
+<style>
+  :root { --bg:#f7f4ec; --card:#fffdf7; --ink:#2f3b2f; --mut:#6b756b;
+          --green:#2f5d46; --green-ink:#eaf3de; --line:#e2ddcf; --red:#b04a3a; }
+  * { box-sizing: border-box; }
+  body { margin:0; font-family:-apple-system,"Segoe UI","Microsoft YaHei",sans-serif;
+         background:var(--bg); color:var(--ink); }
+  header { padding:16px 24px; border-bottom:1px solid var(--line); background:var(--card); }
+  header h1 { margin:0; font-size:18px; font-weight:600; }
+  header .sub { color:var(--mut); font-size:12px; margin-top:2px; }
+  .nav { margin-top:8px; font-size:13px; }
+  .nav a { color:var(--green); text-decoration:none; margin-right:14px; }
+  .nav a.active { color:var(--ink); font-weight:600; text-decoration:underline; }
+  .wrap { padding:16px 24px; }
+  .tabs { display:flex; gap:8px; margin-bottom:14px; }
+  .tab { padding:8px 18px; border:1px solid var(--line); border-radius:8px 8px 0 0;
+         background:var(--card); color:var(--mut); font-size:14px; cursor:pointer; }
+  .tab.active { background:var(--green); color:var(--green-ink); border-color:var(--green);
+                font-weight:600; }
+  .panel { display:block; }
+  .hidden { display:none !important; }
+  .card { background:var(--card); border:1px solid var(--line); border-radius:10px;
+          padding:16px; margin-bottom:14px; }
+  .card h2 { margin:0 0 12px; font-size:14px; font-weight:600; color:var(--green); }
+  .badge { display:inline-block; padding:2px 10px; border-radius:12px; font-size:12px;
+           background:var(--green); color:var(--green-ink); }
+  .badge.warn { background:var(--red); color:#fff; }
+  .badge.mut { background:#c9c3b2; color:#4a514a; }
+  .hint { color:var(--mut); font-size:12px; margin-top:8px; }
+  table.kv-table { width:100%; border-collapse:collapse; font-size:13px; }
+  .kv-table td { padding:6px 4px; border-bottom:1px dashed var(--line);
+                 font-variant-numeric:tabular-nums; }
+  .kv-table td:first-child { color:var(--mut); width:150px; }
+  .kv-table tr:last-child td { border-bottom:none; }
+  .toolbar { display:flex; align-items:center; gap:10px; flex-wrap:wrap; }
+  select, button.btn { padding:7px 12px; border:1px solid var(--line); border-radius:6px;
+                       background:#fff; font-size:13px; color:var(--ink); }
+  button.btn { background:var(--green); color:var(--green-ink); border:none; cursor:pointer; }
+  button.btn.sec { background:#5b6a5e; }
+  button.btn:active { opacity:.85; }
+  .chips { display:flex; gap:8px; flex-wrap:wrap; margin:10px 0 4px; }
+  .chip { background:#efeadc; border:1px solid var(--line); border-radius:14px;
+          padding:4px 12px; font-size:12px; color:var(--ink);
+          font-variant-numeric:tabular-nums; }
+  .chip b { color:var(--green); }
+  .chart-grid { display:flex; flex-wrap:wrap; gap:12px; }
+  .chart-card { flex:1 1 340px; min-width:300px; background:#fffdf7;
+                border:1px solid var(--line); border-radius:8px; padding:6px; }
+  .chart-card canvas { width:100%; height:200px; display:block; }
+  canvas#timeline { width:100%; height:64px; display:block; background:#fffdf7;
+                    border:1px solid var(--line); border-radius:8px; }
+  .ev-wrap { max-height:320px; overflow:auto; border:1px solid var(--line);
+             border-radius:8px; }
+  table.ev-table { width:100%; border-collapse:collapse; font-size:12.5px; }
+  .ev-table th { position:sticky; top:0; background:#efeadc; text-align:left;
+                 padding:7px 8px; border-bottom:1px solid var(--line); z-index:1; }
+  .ev-table td { padding:6px 8px; border-bottom:1px dashed var(--line);
+                 font-variant-numeric:tabular-nums; }
+  .ev-table tr:last-child td { border-bottom:none; }
+  .tl-legend { display:flex; gap:10px; flex-wrap:wrap; font-size:12px; color:var(--mut);
+               margin-top:6px; }
+  .tl-legend span { display:inline-flex; align-items:center; gap:4px; }
+  .dot { width:10px; height:10px; border-radius:3px; display:inline-block; }
+  .loading { color:var(--mut); font-size:13px; }
+</style>
+</head>
+<body>
+<header>
+  <h1>地面站 · PX4 信息</h1>
+  <div class="sub">PX4 输出实时视图 + 任务日志解读</div>
+  <nav class="nav">
+    <a href="/">主页面</a><a class="active" href="/px4">PX4 信息</a>
+  </nav>
+</header>
+
+<div class="wrap">
+  <div class="tabs">
+    <button class="tab active" id="tabbtn-live" onclick="switchTab('live')">实时状态</button>
+    <button class="tab" id="tabbtn-logs" onclick="switchTab('logs')">日志解读</button>
+  </div>
+
+  <!-- ===================== 实时状态 ===================== -->
+  <section id="tab-live" class="panel">
+    <div class="card">
+      <h2>PX4 遥测 <span class="badge" id="live-state">-</span>
+        <span class="badge mut" id="live-inject">注入 -</span>
+        <span class="badge mut" id="live-video">视频 -</span>
+      </h2>
+      <table class="kv-table"><tbody id="live-body">
+        <tr><td colspan="2" class="loading">加载中…</td></tr>
+      </tbody></table>
+      <div class="hint" id="live-meta"></div>
+    </div>
+  </section>
+
+  <!-- ===================== 日志解读 ===================== -->
+  <section id="tab-logs" class="panel hidden">
+    <div class="card">
+      <h2>运行段选择</h2>
+      <div class="toolbar">
+        <select id="run-sel" onchange="renderRun()"></select>
+        <button class="btn sec" onclick="loadLogs(true)">刷新日志</button>
+        <span class="hint" id="logs-note">日志文件为多次运行追加，已按 t_s 重置自动分段。</span>
+      </div>
+      <div class="chips" id="kpis"></div>
+    </div>
+
+    <div class="card">
+      <h2>曲线</h2>
+      <div class="chart-grid">
+        <div class="chart-card"><canvas id="c-alt"></canvas></div>
+        <div class="chart-card"><canvas id="c-dist"></canvas></div>
+        <div class="chart-card"><canvas id="c-vel"></canvas></div>
+        <div class="chart-card"><canvas id="c-yaw"></canvas></div>
+        <div class="chart-card"><canvas id="c-off"></canvas></div>
+        <div class="chart-card"><canvas id="c-yawrate"></canvas></div>
+      </div>
+    </div>
+
+    <div class="card">
+      <h2>任务状态时间线</h2>
+      <canvas id="timeline"></canvas>
+      <div class="tl-legend" id="tl-legend"></div>
+    </div>
+
+    <div class="card">
+      <h2>事件表 <span class="badge mut" id="ev-count">0</span></h2>
+      <div class="ev-wrap">
+        <table class="ev-table">
+          <thead><tr><th>t_s</th><th>类型</th><th>详情</th></tr></thead>
+          <tbody id="ev-body"></tbody>
+        </table>
+      </div>
+    </div>
+  </section>
+</div>
+
+<script>
+// ============================================================ 通用
+function $(id){ return document.getElementById(id); }
+
+const STATE_COLORS = {
+  wait: "#8a8f8a", arm_takeoff: "#c9a227", goto: "#3a6ea5",
+  track: "#2f7d4f", approach: "#7a4fa3", rtl: "#b04a3a"
+};
+const STATE_LABELS = {
+  wait: "待命", arm_takeoff: "起飞", goto: "坐标引导", track: "追踪",
+  approach: "接近悬停", rtl: "返航"
+};
+const SERIES_COLORS = ["#2f6f4f", "#b04a3a", "#3a6ea5", "#c9a227", "#7a4fa3", "#0f766e"];
+
+// ============================================================ 分页
+let liveTimer = null;
+
+function switchTab(name){
+  for (const t of ["live", "logs"]){
+    $("tabbtn-" + t).classList.toggle("active", t === name);
+    $("tab-" + t).classList.toggle("hidden", t !== name);
+  }
+  if (name === "live"){ startLive(); }
+  else { stopLive(); if (!logsLoaded) loadLogs(false); }
+}
+
+// ============================================================ 实时状态
+const LIVE_FIELDS = [
+  ["state",            "任务状态",   v => v],
+  ["pos_valid",        "位置有效",   v => v === true ? "是" : (v === false ? "否" : "-")],
+  ["fix_type",         "定位类型 fix_type", v => v],
+  ["num_satellites",   "卫星数",     v => v],
+  ["latitude_deg",     "纬度",       v => num(v, 6)],
+  ["longitude_deg",    "经度",       v => num(v, 6)],
+  ["relative_altitude_m", "相对高度 (m)", v => num(v, 2)],
+  ["yaw_deg",          "航向 yaw (°)", v => num(v, 1)],
+  ["mode",             "飞行模式",   v => v || "-"],
+  ["target_injected",  "目标已注入", v => v === true ? "是" : (v === false ? "否" : "-")],
+  ["distance_to_target_m", "距目标 (m)", v => v === "inf" ? "∞" : num(v, 1)],
+  ["target_locked",    "吊舱锁定",   v => v === true ? "锁定" : (v === false ? "未锁定" : "-")],
+  ["off_target_x_deg", "脱靶 X (°)", v => num(v, 2)],
+  ["off_target_y_deg", "脱靶 Y (°)", v => num(v, 2)],
+  ["yaw_rate_dps",     "yaw_rate (°/s)", v => num(v, 1)],
+  ["vx_body_mps",      "vx 前向 (m/s)", v => num(v, 2)],
+  ["vz_mps",           "vz 垂速 (m/s)", v => num(v, 2)],
+];
+
+function num(v, d){
+  if (v === null || v === undefined || v === "" || v === "nan" || v === "inf") return v === "inf" ? "∞" : "-";
+  const n = Number(v);
+  return Number.isNaN(n) ? String(v) : n.toFixed(d);
+}
+
+async function pollLive(){
+  try {
+    const r = await fetch("/api/state");
+    const j = await r.json();
+    const t = j.telemetry || {};
+    const s = $("live-state");
+    const st = j.state || "unknown";
+    s.textContent = st + "（" + (STATE_LABELS[st] || "") + "）";
+    s.className = "badge" + (st === "rtl" || st === "wait" ? " warn" : "");
+    $("live-inject").textContent = "注入 " + (j.continuous ? "持续中" : "未持续");
+    $("live-inject").className = "badge" + (j.continuous ? "" : " mut");
+    const v = j.video || {};
+    $("live-video").textContent = "视频 " + (!v.enabled ? "未配置" : (v.online ? "在线" : "断流"));
+    $("live-video").className = "badge" + (!v.enabled || !v.online ? " mut" : "");
+
+    $("live-body").innerHTML = LIVE_FIELDS.map(([k, label, fmt]) => {
+      const raw = t[k];
+      // /api/state 的遥测值是 CSV 原始字符串："True"/"False" 归一化为布尔
+      const val0 = raw === "True" ? true : (raw === "False" ? false : raw);
+      const val = fmt(val0);
+      return `<tr><td>${label}</td><td>${val === undefined || val === null ? "-" : val}</td></tr>`;
+    }).join("");
+    $("live-meta").textContent =
+      `日志源: ${j.log_dir || "-"} · 刷新 ${new Date().toLocaleTimeString()}`
+      + `（数据为任务日志最新一行；runner 未运行时显示上次任务数据）`;
+  } catch(e){
+    $("live-meta").textContent = "后端未连接: " + e;
+  }
+}
+
+function startLive(){
+  pollLive();
+  if (liveTimer === null) liveTimer = setInterval(pollLive, 1000);
+}
+
+function stopLive(){
+  if (liveTimer !== null){ clearInterval(liveTimer); liveTimer = null; }
+}
+
+// ============================================================ 日志加载与分段
+let logsLoaded = false;
+let telSegs = [];     // 遥测分段（每段是对象数组）
+let evSegs = [];      // 事件分段
+let curTel = null, curEv = null;
+
+function parseVal(s){
+  if (s === "" || s === null || s === undefined) return null;
+  if (s === "True") return true;
+  if (s === "False") return false;
+  if (s === "inf") return "inf";
+  if (s === "-inf" || s === "nan") return null;
+  const n = Number(s);
+  return Number.isNaN(n) ? s : n;
+}
+
+function segmentize(arr, getT){
+  const segs = []; let cur = [];
+  for (const item of arr){
+    const t = getT(item);
+    if (cur.length && t < getT(cur[cur.length-1])){ segs.push(cur); cur = []; }
+    cur.push(item);
+  }
+  if (cur.length) segs.push(cur);
+  return segs;
+}
+
+// 事件分段：三重判据——t_s 回退（新会话）/「任务启动」标记（from==to）/
+// 上一段以 rtl 收尾（每次运行以返航事件结束；旧日志没有启动标记时靠这条切分）
+function segmentEvents(evs){
+  const segs = []; let cur = [];
+  for (const e of evs){
+    const prev = cur[cur.length - 1];
+    const boundary = !!prev && (
+      (e.t_s || 0) < (prev.t_s || 0)
+      || (e.type === "state" && e.from_state === e.to_state)
+      || prev.type === "rtl");
+    if (boundary){ segs.push(cur); cur = []; }
+    cur.push(e);
+  }
+  if (cur.length) segs.push(cur);
+  return segs;
+}
+
+async function loadLogs(force){
+  const note = $("logs-note");
+  if (logsLoaded && !force) return;
+  note.textContent = "加载中…";
+  try {
+    const [rt, re] = await Promise.all([
+      fetch("/api/logs/telemetry").then(r => r.json()),
+      fetch("/api/logs/events").then(r => r.json()),
+    ]);
+    const fields = rt.fields || [];
+    const rows = (rt.rows || []).map(r => {
+      const o = {};
+      fields.forEach((f, i) => { o[f] = parseVal(r[i]); });
+      return o;
+    });
+    telSegs = segmentize(rows, o => o.t_s || 0);
+    evSegs = segmentEvents(re.events || []);
+    logsLoaded = true;
+
+    const sel = $("run-sel");
+    sel.innerHTML = "";
+    for (let k = 1; k <= telSegs.length; k++){   // 最近在前（默认选中）
+      const seg = segFromEnd(telSegs, k);
+      const dur = seg.length ? seg[seg.length-1].t_s || 0 : 0;
+      const opt = document.createElement("option");
+      opt.value = k;   // k = 从最近数第 k 段
+      opt.textContent = (k === 1 ? "最近运行" : `倒数第 ${k} 次`)
+        + ` · ${dur.toFixed(1)}s · ${seg.length} 行`;
+      sel.appendChild(opt);
+    }
+    if (!telSegs.length){
+      note.textContent = "未找到遥测日志（logs/task_telemetry.csv）。";
+      return;
+    }
+    note.textContent = `共 ${telSegs.length} 段运行（文件追加 ${rt.total} 行）；`
+      + "事件按运行次序从最近配对，较旧段可能无事件。";
+    renderRun();
+  } catch(e){
+    note.textContent = "日志加载失败: " + e;
+  }
+}
+
+function segFromEnd(segs, k){ return segs[segs.length - k] || null; }
+
+function renderRun(){
+  const k = Number($("run-sel").value || 1);
+  curTel = segFromEnd(telSegs, k);
+  curEv = segFromEnd(evSegs, k);
+  if (!curTel) return;
+  renderKpis();
+  renderCharts();
+  renderTimeline();
+  renderEvents();
+}
+
+// ============================================================ KPI
+function renderKpis(){
+  const seg = curTel;
+  const dur = seg.length ? seg[seg.length-1].t_s || 0 : 0;
+  const alts = seg.map(r => r.relative_altitude_m).filter(v => typeof v === "number" && isFinite(v));
+  const dists = seg.map(r => r.distance_to_target_m)
+    .filter(v => typeof v === "number" && isFinite(v) && v > 0);
+  const maxAlt = alts.length ? Math.max(...alts) : null;
+  const minDist = dists.length ? Math.min(...dists) : null;
+
+  const states = [];
+  for (const r of seg){ const s = r.state; if (s && states[states.length-1] !== s) states.push(s); }
+  const seq = states.map(s => STATE_LABELS[s] || s).join(" → ");
+
+  $("kpis").innerHTML = [
+    `时长 <b>${dur.toFixed(1)}s</b>`,
+    `最大高度 <b>${maxAlt === null ? "-" : maxAlt.toFixed(1) + "m"}</b>`,
+    `最近距目标 <b>${minDist === null ? "-" : minDist.toFixed(1) + "m"}</b>`,
+    `遥测 <b>${seg.length}</b> 行`,
+    `事件 <b>${curEv ? curEv.length : 0}</b> 条`,
+    `状态链 <b>${seq || "-"}</b>`,
+  ].map(c => `<span class="chip">${c}</span>`).join("");
+}
+
+// ============================================================ 曲线图（纯 canvas）
+function niceTicks(lo, hi, n){
+  const span = hi - lo;
+  if (!(span > 0)) return [lo];
+  const step0 = span / n;
+  const mag = Math.pow(10, Math.floor(Math.log10(step0)));
+  const norm = step0 / mag;
+  let step = norm <= 1 ? 1 : norm <= 2 ? 2 : norm <= 5 ? 5 : 10;
+  step *= mag;
+  const out = [];
+  for (let v = Math.ceil(lo / step) * step; v <= hi + 1e-9; v += step) out.push(v);
+  return out;
+}
+
+function fmtTick(v){
+  const a = Math.abs(v);
+  if (a >= 10) return String(Math.round(v * 10) / 10);
+  if (a >= 1) return String(Math.round(v * 100) / 100);
+  return String(Math.round(v * 1000) / 1000);
+}
+
+function drawChart(cv, spec){
+  const dpr = window.devicePixelRatio || 1;
+  const W = cv.clientWidth || 340, H = cv.clientHeight || 200;
+  cv.width = Math.round(W * dpr); cv.height = Math.round(H * dpr);
+  const ctx = cv.getContext("2d");
+  ctx.setTransform(dpr, 0, 0, dpr, 0, 0);
+  ctx.clearRect(0, 0, W, H);
+  const L = 50, R = 12, T = 24, B = 24;
+  const pw = W - L - R, ph = H - T - B;
+
+  ctx.font = "12px sans-serif";
+  ctx.fillStyle = "#2f3b2f";
+  ctx.fillText(spec.title || "", 8, 15);
+
+  // 图例（右上，从右往左排）
+  let lx = W - R;
+  ctx.textAlign = "right";
+  for (let i = spec.series.length - 1; i >= 0; i--){
+    const s = spec.series[i];
+    const w = ctx.measureText(s.name).width;
+    ctx.fillStyle = s.color; ctx.fillRect(lx - w - 16, 7, 8, 8);
+    ctx.fillStyle = "#6b756b"; ctx.fillText(s.name, lx, 15);
+    lx -= w + 26;
+  }
+  ctx.textAlign = "left";
+
+  let xs = [], ys = [];
+  for (const s of spec.series) for (const p of s.points){
+    if (isFinite(p[0]) && isFinite(p[1])){ xs.push(p[0]); ys.push(p[1]); }
+  }
+  if (!xs.length){
+    ctx.fillStyle = "#6b756b";
+    ctx.fillText("无数据", L, T + ph / 2);
+    return;
+  }
+  let x0 = Math.min(...xs), x1 = Math.max(...xs);
+  if (x1 <= x0) x1 = x0 + 1;
+  let y0 = Math.min(...ys), y1 = Math.max(...ys);
+  if (y1 - y0 < 1e-9){ const m = Math.abs(y1) || 1; y0 -= m * 0.1; y1 += m * 0.1; }
+  else { const pad = (y1 - y0) * 0.08; y0 -= pad; y1 += pad; }
+
+  const xTo = x => L + (x - x0) / (x1 - x0) * pw;
+  const yTo = y => T + (1 - (y - y0) / (y1 - y0)) * ph;
+
+  ctx.lineWidth = 1;
+  ctx.strokeStyle = "#e2ddcf";
+  ctx.fillStyle = "#6b756b";
+  ctx.font = "11px sans-serif";
+  for (const yv of niceTicks(y0, y1, 5)){
+    const yy = yTo(yv);
+    ctx.beginPath(); ctx.moveTo(L, yy); ctx.lineTo(L + pw, yy); ctx.stroke();
+    ctx.fillText(fmtTick(yv), 6, yy + 3.5);
+  }
+  for (const xv of niceTicks(x0, x1, 4)){
+    const xx = xTo(xv);
+    ctx.beginPath(); ctx.moveTo(xx, T); ctx.lineTo(xx, T + ph); ctx.stroke();
+    const lbl = fmtTick(xv) + "s";
+    ctx.fillText(lbl, Math.min(Math.max(xx - 8, 2), W - R - ctx.measureText(lbl).width), H - 8);
+  }
+  ctx.strokeStyle = "#b9b4a3";
+  ctx.strokeRect(L, T, pw, ph);
+
+  for (const s of spec.series){
+    ctx.strokeStyle = s.color;
+    ctx.lineWidth = 1.6;
+    ctx.beginPath();
+    let started = false;
+    for (const p of s.points){
+      if (!isFinite(p[0]) || !isFinite(p[1])){ started = false; continue; }
+      const xx = xTo(p[0]), yy = yTo(p[1]);
+      if (!started){ ctx.moveTo(xx, yy); started = true; }
+      else ctx.lineTo(xx, yy);
+    }
+    ctx.stroke();
+  }
+}
+
+function seriesFrom(seg, key, label, colorIdx){
+  return { name: label, color: SERIES_COLORS[colorIdx % SERIES_COLORS.length],
+           points: seg.map(r => [r.t_s, r[key]]) };
+}
+
+function renderCharts(){
+  const seg = curTel;
+  drawChart($("c-alt"),  { title: "相对高度 (m)", series: [seriesFrom(seg, "relative_altitude_m", "alt", 0)] });
+  drawChart($("c-dist"), { title: "距目标 (m)",   series: [seriesFrom(seg, "distance_to_target_m", "dist", 1)] });
+  drawChart($("c-vel"),  { title: "机体系速度 (m/s)", series: [
+    seriesFrom(seg, "vx_body_mps", "vx", 0), seriesFrom(seg, "vz_mps", "vz", 1)] });
+  drawChart($("c-yaw"),  { title: "航向 yaw (°)", series: [seriesFrom(seg, "yaw_deg", "yaw", 2)] });
+  drawChart($("c-off"),  { title: "脱靶量 (°)", series: [
+    seriesFrom(seg, "off_target_x_deg", "off_x", 3), seriesFrom(seg, "off_target_y_deg", "off_y", 4)] });
+  drawChart($("c-yawrate"), { title: "yaw_rate (°/s)", series: [seriesFrom(seg, "yaw_rate_dps", "yaw_rate", 5)] });
+}
+
+// ============================================================ 状态时间线
+function transitionsOf(evSeg){
+  const out = [];
+  for (const e of (evSeg || [])){
+    if (e.type === "state" && e.to_state){
+      out.push({ t: e.t_s || 0, from: e.from_state, to: e.to_state, reason: e.reason || "" });
+    }
+  }
+  return out;
+}
+
+function renderTimeline(){
+  const cv = $("timeline");
+  const dpr = window.devicePixelRatio || 1;
+  const W = cv.clientWidth || 800, H = cv.clientHeight || 64;
+  cv.width = Math.round(W * dpr); cv.height = Math.round(H * dpr);
+  const ctx = cv.getContext("2d");
+  ctx.setTransform(dpr, 0, 0, dpr, 0, 0);
+  ctx.clearRect(0, 0, W, H);
+
+  const seg = curTel;
+  const tEnd = seg.length ? (seg[seg.length-1].t_s || 0) : 0;
+  const trs = transitionsOf(curEv);
+  const blocks = [];
+  let curFrom = 0;
+  let curState = trs.length ? (trs[0].from || "wait") : (seg[0] ? seg[0].state : "wait");
+  for (const tr of trs){
+    blocks.push({ from: curFrom, to: tr.t, state: curState });
+    curFrom = tr.t;
+    curState = tr.to;
+  }
+  blocks.push({ from: curFrom, to: Math.max(tEnd, curFrom + 0.1), state: curState });
+
+  const L = 8, R = 8, T = 6, ph = H - 2 * T;
+  const pw = W - L - R;
+  ctx.font = "11px sans-serif";
+  for (const b of blocks){
+    const x0 = L + (b.from / Math.max(tEnd, 0.1)) * pw;
+    const x1 = L + (b.to / Math.max(tEnd, 0.1)) * pw;
+    const w = Math.max(x1 - x0, 1);
+    ctx.fillStyle = STATE_COLORS[b.state] || "#555";
+    ctx.fillRect(x0, T, w, ph);
+    const label = STATE_LABELS[b.state] || b.state;
+    if (w > ctx.measureText(label).width + 10){
+      ctx.fillStyle = "#fff";
+      ctx.fillText(label, x0 + (w - ctx.measureText(label).width) / 2, T + ph / 2 + 4);
+    }
+  }
+  ctx.fillStyle = "#6b756b";
+  ctx.fillText(`0s`, L, H - 1);
+  const endLbl = `${tEnd.toFixed(1)}s`;
+  ctx.fillText(endLbl, W - R - ctx.measureText(endLbl).width, H - 1);
+
+  // 图例
+  const used = [...new Set(blocks.map(b => b.state))];
+  $("tl-legend").innerHTML = used.map(s =>
+    `<span><span class="dot" style="background:${STATE_COLORS[s] || "#555"}"></span>${STATE_LABELS[s] || s}</span>`
+  ).join("");
+}
+
+// ============================================================ 事件表
+function evSummary(e){
+  if (e.type === "state") return `${e.from_state} → ${e.to_state}（${e.reason || ""}）`;
+  if (e.type === "command") return `地面站命令: ${e.name}`;
+  if (e.type === "fc_status") return `飞控 ${e.severity || ""}: ${e.text || ""}`;
+  if (e.type === "takeoff") return `起飞至 ${e.altitude_m} m`;
+  if (e.type === "takeoff_failed") return `起飞失败: ${e.error || ""}`;
+  if (e.type === "rtl") return "开始返航";
+  const parts = [];
+  for (const [k, v] of Object.entries(e)){
+    if (["type", "ts_ms", "t_s"].includes(k)) continue;
+    parts.push(`${k}=${v}`);
+  }
+  return parts.join(", ");
+}
+
+function renderEvents(){
+  const evs = curEv || [];
+  $("ev-count").textContent = String(evs.length);
+  const rows = evs.slice(-300).map(e =>
+    `<tr><td>${(e.t_s ?? 0).toFixed(2)}</td><td>${e.type || "-"}</td><td>${evSummary(e)}</td></tr>`
+  );
+  $("ev-body").innerHTML = rows.length ? rows.join("")
+    : `<tr><td colspan="3" class="loading">本段无事件</td></tr>`;
+}
+
+// ============================================================ 启动
+startLive();
+window.addEventListener("resize", () => { if (logsLoaded && !$("tab-logs").classList.contains("hidden")) renderCharts(); });
+</script>
+</body>
+</html>

ground_station/video_source.py

@@ -0,0 +1,152 @@
+"""video_source.py —— 吊舱 RTSP 视频源（服务端拉流 → MJPEG 推流）。
+
+浏览器不能直连 RTSP，由本模块在服务端用 OpenCV 拉流、编码 JPEG 帧缓存，
+页面经 `/video_feed`（multipart/x-mixed-replace）显示实时画面。
+
+设计要点：
+  * cv2 延迟导入：模块 import 无副作用，无 OpenCV 环境也能 import；
+    capture_factory 可注入假采集器 → 单测不依赖真实摄像头/网络；
+  * 断线自动重连（读帧失败 → 释放 → 退避重试）；
+  * 抽帧节流 + 降分辨率：1080p60 全量转 JPEG 带宽/CPU 吃不消（默认 960 宽、≤15fps）。
+"""
+
+import logging
+import threading
+import time
+
+logger = logging.getLogger(__name__)
+
+_IDLE_RESEND_S = 2.0   # 无新帧时重发当前帧作保活（浏览器挂住连接）
+
+
+class RtspVideoSource:
+    """后台线程拉 RTSP，维护最新 JPEG 帧供 MJPEG 消费。"""
+
+    def __init__(self, url: str, width: int = 960, fps: float = 15.0,
+                 quality: int = 75, capture_factory=None) -> None:
+        self._url = url
+        self._width = int(width)
+        self._min_interval = 1.0 / max(1.0, float(fps))
+        self._quality = int(quality)
+        self._capture_factory = capture_factory
+        self._lock = threading.Lock()
+        self._frame = None            # 最新 JPEG bytes
+        self._frame_seq = 0
+        self._frame_at = 0.0          # 最新帧时间（monotonic）
+        self._stop = threading.Event()
+        self._thread = None
+
+    # --- 生命周期 -----------------------------------------------------------
+    def start(self) -> None:
+        if self._thread is not None and self._thread.is_alive():
+            return
+        self._stop.clear()
+        self._thread = threading.Thread(target=self._run, name="rtsp-video",
+                                        daemon=True)
+        self._thread.start()
+
+    def stop(self) -> None:
+        self._stop.set()
+        thread, self._thread = self._thread, None
+        if thread is not None:
+            thread.join(timeout=3.0)
+
+    @property
+    def online(self) -> bool:
+        """最近 2s 内有新帧 → 在线（用于页面状态显示）。"""
+        with self._lock:
+            return (self._frame is not None
+                    and (time.monotonic() - self._frame_at) < 2.0)
+
+    def latest(self) -> tuple[int, bytes | None]:
+        """返回 (seq, jpeg)；尚无帧时 (0, None)。"""
+        with self._lock:
+            return self._frame_seq, self._frame
+
+    def mjpeg_stream(self):
+        """MJPEG 生成器（multipart 分块）；客户端断开时随 GeneratorExit 终止。"""
+        last_seq = -1
+        last_send = 0.0
+        while not self._stop.is_set():
+            seq, jpeg = self.latest()
+            now = time.monotonic()
+            if jpeg is not None and (seq != last_seq
+                                     or now - last_send > _IDLE_RESEND_S):
+                last_seq = seq
+                last_send = now
+                yield (b"--frame\r\nContent-Type: image/jpeg\r\n"
+                       + f"Content-Length: {len(jpeg)}\r\n\r\n".encode("ascii")
+                       + jpeg + b"\r\n")
+            time.sleep(1.0 / 30.0)
+
+    # --- 拉流线程 -----------------------------------------------------------
+    def _open(self):
+        if self._capture_factory is not None:
+            return self._capture_factory()
+        import cv2  # 延迟导入：仅真正拉流时依赖 OpenCV
+        return cv2.VideoCapture(self._url, cv2.CAP_FFMPEG)
+
+    def _run(self) -> None:
+        import os
+        # stimeout：RTSP socket 超时（µs）。不设的话断网时单次 open 会阻塞 ~30s，
+        # 影响重连节奏（实测 2026-10-14）。
+        os.environ.setdefault("OPENCV_FFMPEG_CAPTURE_OPTIONS",
+                              "rtsp_transport;tcp|stimeout;5000000")
+        import cv2
+
+        cap = None
+        last_encode = 0.0
+        try:
+            while not self._stop.is_set():
+                if cap is None or not cap.isOpened():
+                    if cap is not None:
+                        try:
+                            cap.release()
+                        except Exception:
+                            pass
+                        cap = None
+                    cap = self._open()
+                    if not cap.isOpened():
+                        logger.warning("[Video] RTSP 打开失败，1s 后重试: %s",
+                                       self._url)
+                        self._stop.wait(1.0)
+                        continue
+                    logger.info("[Video] RTSP 已连接: %s", self._url)
+                    last_encode = 0.0
+
+                ok, frame = cap.read()
+                if not ok or frame is None:
+                    logger.warning("[Video] 读帧失败，重连")
+                    try:
+                        cap.release()
+                    except Exception:
+                        pass
+                    cap = None
+                    self._stop.wait(0.5)
+                    continue
+
+                now = time.monotonic()
+                if now - last_encode < self._min_interval:
+                    continue  # 抽帧节流：60fps 源 → ≤fps 编码
+                last_encode = now
+
+                h, w = frame.shape[:2]
+                if w > self._width:
+                    scale = self._width / w
+                    frame = cv2.resize(
+                        frame, (self._width, max(1, int(h * scale))),
+                        interpolation=cv2.INTER_AREA)
+                ok, buf = cv2.imencode(
+                    ".jpg", frame, [cv2.IMWRITE_JPEG_QUALITY, self._quality])
+                if not ok:
+                    continue
+                with self._lock:
+                    self._frame = buf.tobytes()
+                    self._frame_seq += 1
+                    self._frame_at = time.monotonic()
+        finally:
+            if cap is not None:
+                try:
+                    cap.release()
+                except Exception:
+                    pass

l1_sitl_test.py

@@ -1,5 +1,5 @@
-#!/usr/bin/env python3
+#!/usr/bin/env python3
-"""pod_sitl_test.py —— L1 航向伺服 SITL 全流程集成测试（test.py 的吊舱版）。
+"""l1_sitl_test.py —— L1 航向伺服 SITL 全流程集成测试。
 
 连接 PX4 SITL，流程：
   解锁 → 起飞 → 记录悬停锚点/航向 → 进 offboard
@@ -7,7 +7,9 @@
     → PodBridge → TrackingControlLaw 角度直入 → offboard 速度设定点）
   → 目标消失 → 判丢零速刹车 → 退出 offboard → RTL → 落地 → 上锁
 
-与 test.py 的差异：
+只覆盖 L1 控制律这一段；任务级全流程见根目录 `task_runner_sitl.py`。
+
+要点：
   * 走 L1 角度直入（吊舱脱靶量 off_target），无像素/检测框/追踪器；
   * 无距离（distance_valid=False）→ 距离通道按 no_distance_action 降级
     （slow_approach，0.2 m/s 极低限速）；
@@ -15,9 +17,11 @@
   * 符号使用部署配置（S1 标定：beta_sign=+1 / alpha_sign=-1），
     本脚本即对该标定的端到端复核。
 
+共享件（时钟/遥测订阅/连接准备）在 `utils/sitl_support.py`。
+
 运行前：启动 PX4 SITL（默认本机 14540）。用法：
-  python pod_sitl_test.py
+  python l1_sitl_test.py
-  python pod_sitl_test.py --conn udp://:14540 --config utils/config/pod_bridge.json
+  python l1_sitl_test.py --conn udpin://0.0.0.0:14540 --config utils/config/pod_bridge.json
 """
 
 import argparse
@@ -42,13 +46,14 @@ from pod_bridge.px4_gateway import Px4Gateway
 from pod_bridge.virtual_pod import (CarrierView, VirtualPod, VirtualTarget,
                                     VirtualTargetConfig)
 
-logger = logging.getLogger("pod_sitl")
+logger = logging.getLogger("l1_sitl")
 
 # ==================== 配置 ====================
 CONNECTION = "udpin://0.0.0.0:14540"
 CONFIG_PATH = _UTILS_DIR / "config" / "pod_bridge.json"
 TAKEOFF_ALTITUDE = 5.0            # 起飞高度 (m)
 
+
 # 虚拟目标：相对悬停锚点、按锚点机头朝向放置（前/右/下），随后水平圆弧运动。
 TARGET_AHEAD_M = 30.0
 TARGET_RIGHT_M = 15.0
@@ -61,96 +66,13 @@ TRACK_NORMAL_SECS = 20.0          # 目标保持可见时长（之后模拟丢
 TRACK_BRAKE_SECS = 5.0            # 判丢后零速刹车时长
 TRACK_TIMEOUT_SECS = 120.0        # 跟踪段整体安全超时
 STATUS_EVERY_TICKS = 10           # 20 Hz 下每 0.5 s 打印一行状态
-CSV_PATH = Path(__file__).resolve().parent / "pod_sitl_trajectory.csv"
+CSV_PATH = Path(__file__).resolve().parent / "l1_sitl_trajectory.csv"
 
 
-def now_ms() -> int:
-    return int(time.monotonic() * 1000.0)
 
-
+# 共享件（时钟/遥测订阅/连接准备）：utils/sitl_support.py
-class TelemetryHub:
+from sitl_support import (TelemetryHub, now_ms, print_flight_mode,
-    """后台订阅 NED 位置/姿态，供 VirtualPod 几何合成与锚点记录（同 test.py）。"""
+                           set_telemetry_rates, wait_until_ready)
-
-    def __init__(self) -> None:
-        self.yaw_rad = 0.0
-        self.north_m = 0.0
-        self.east_m = 0.0
-        self.down_m = 0.0
-        self.attitude_time_ms = 0
-        self._tasks: list = []
-
-    def start(self, drone) -> None:
-        self._tasks.append(asyncio.create_task(self._attitude_loop(drone)))
-        self._tasks.append(asyncio.create_task(self._position_loop(drone)))
-
-    async def _attitude_loop(self, drone) -> None:
-        async for euler in drone.telemetry.attitude_euler():
-            self.yaw_rad = math.radians(euler.yaw_deg)
-            self.attitude_time_ms = now_ms()
-
-    async def _position_loop(self, drone) -> None:
-        async for pos_vel in drone.telemetry.position_velocity_ned():
-            pos = pos_vel.position
-            self.north_m = pos.north_m
-            self.east_m = pos.east_m
-            self.down_m = pos.down_m
-
-    async def stop(self) -> None:
-        for task in self._tasks:
-            task.cancel()
-        for task in self._tasks:
-            try:
-                await task
-            except asyncio.CancelledError:
-                pass
-        self._tasks.clear()
-
-    def view(self) -> CarrierView:
-        return CarrierView(north_m=self.north_m, east_m=self.east_m,
-                           down_m=self.down_m, yaw_rad=self.yaw_rad)
-
-
-async def print_flight_mode(drone):
-    async for flight_mode in drone.telemetry.flight_mode():
-        print(f"[模式] 当前飞行模式: {flight_mode}")
-        return flight_mode
-
-
-async def wait_until_ready(drone):
-    """等待连接 + 位置估计就绪（各阶段限时 30 s，同 test.py）。"""
-    print("等待无人机连接...")
-    try:
-        async with asyncio.timeout(30.0):
-            async for state in drone.core.connection_state():
-                if state.is_connected:
-                    print("-- 已连接")
-                    break
-    except TimeoutError:
-        raise RuntimeError("30 s 内未建立 MAVSDK 连接（检查 SITL 是否在运行）") from None
-
-    print("等待全球位置估计就绪...")
-    try:
-        async with asyncio.timeout(30.0):
-            async for health in drone.telemetry.health():
-                if health.is_global_position_ok and health.is_home_position_ok:
-                    print("-- 位置估计 OK")
-                    break
-    except TimeoutError:
-        raise RuntimeError("30 s 内位置估计未就绪") from None
-
-
-async def set_telemetry_rates(drone):
-    """显式设定遥测流速率（失败沿用默认并告警，同 test.py）。"""
-    for stream, rate in (("position", 50.0),
-                         ("position_velocity_ned", 30.0),
-                         ("attitude_euler", 50.0),
-                         ("gps_info", 10.0),
-                         ("velocity_ned", 10.0),
-                         ("in_air", 10.0)):
-        try:
-            await getattr(drone.telemetry, f"set_rate_{stream}")(rate)
-        except Exception as e:
-            print(f"! 设置 {stream} 速率失败（沿用默认值）: {e}")
 
 
 async def takeoff(drone) -> bool:

legacy/README.md

@@ -0,0 +1,54 @@
+# legacy/ —— 已归档的「双目 + YOLO」可选感知后端
+
+> 归档日期：**2026-10-20**
+>
+> 自 2026-10-19 起，**光电吊舱取代双目 + YOLO，成为唯一目标源**。运行链路是
+> `pod_bridge` → `task_runner` → PX4（见仓库根 `README.md` 与
+> `.workbuddy/memory/ARCHITECTURE.md` §11）。
+>
+> 这里的内容**不参与运行链路，也不在默认 pytest 收集范围内**。保留原因：日后若要
+> 重新引入双目作为「独立身份旁证」或「距离源」，可从本目录恢复；对应的设计文档
+> 也一并归档。
+
+## 归档内容
+
+| 路径 | 原位置 | 说明 |
+|---|---|---|
+| `utils/stereo_yolo/` | `utils/stereo_yolo/` | 双目 YOLO 检测前端 |
+| `utils/stereo_ranger/` | `utils/stereo_ranger/` | 双目稀疏测距 |
+| `utils/target_tracker/` | `utils/target_tracker/` | 多目标追踪 + 主目标选举 |
+| `utils/tests/{stereo_yolo,stereo_ranger,target_tracker}/` | 同名 | 共 175 条单测 |
+| `utils/demo_*.py` | `utils/demo_*.py` | 三个演示（track_control / stereo_ranger / stereo_yolo） |
+| `utils/config/stereo_*.json` | `utils/config/` | 对应配置 |
+| `utils/assets/` | `utils/assets/` | demo 占位素材 |
+| `models/yolo26n-drone.pt` | `models/` | YOLO 权重（5.2MB） |
+| `docs/双目*.md` | `docs/` | 三份双目规格/作业指导 |
+| `stereo_calib_params.npz` | 仓库根 | 双目相机标定结果 |
+
+## 怎么跑归档测试（默认不跑）
+
+归档代码依赖 `utils/` 里的 `visual_track_control`（控制律仍在运行链路中），
+所以需要同时把两处加进 `PYTHONPATH`：
+
+```bash
+cd legacy/utils
+PYTHONPATH="../../utils" "D:/program file/conda/envs/pytorch/python.exe" -m pytest -q
+```
+
+预期 **175 passed**（111 stereo_ranger + 36 stereo_yolo + 28 target_tracker）。
+
+## 怎么跑归档 demo
+
+```bash
+cd "D:/ProgramData/code_drone-test"
+PYTHONPATH="utils;legacy/utils" "D:/program file/conda/envs/pytorch/python.exe" legacy/utils/demo_stereo_ranger.py
+```
+
+> `demo_stereo_yolo.py` 需要 `ultralytics` 与 `legacy/models/yolo26n-drone.pt`
+> （demo 内按 `legacy/` 为仓库根解析相对模型路径）。
+
+## 若要正式接回
+
+不要直接 `git mv` 回来就完事：先读 `ARCHITECTURE.md` §11.1 的约定 ——
+需要给 `PodBridge` 增加「距离源」与「独立旁证源」两个**显式注入点**，
+并重新评估 Pi 4 的算力预算（YOLO 双路推理在 Pi 4 上跑不动实时）。

legacy/docs/双目相机模组到位作业指导.md

@@ -0,0 +1,301 @@
+# 双目相机模组到位作业指导
+
+> **定位**：`docs/` 下三份规格（《双目测距技术路线》《双目YOLO目标识别需求说明》
+> 《视觉跟踪控制律技术路线》）回答"应该怎样、判据是什么"；本文回答"相机模组到货
+> 之后按什么顺序做什么、每步怎么判通过、结果回填到哪里"。**本文不新立判据**——
+> 所有通过标准一律引用规格原文并注明章节。
+>
+> **适用范围**：双目相机模组（含厂商标定数据）到货起，至《测距路线》§12.3 四条
+> 红线解除为止。飞控/机载平台到位后的工作属《视觉跟踪控制律技术路线》§8 阶段 4~6，
+> 本文只在阶段 6 交接。
+>
+> **编写状态（2026-09-14）**：仓库内几何参数全部为占位值
+> （`utils/config/stereo_ranger.json`）、demo 素材为 AI 合成图，本文判据**尚未在
+> 任何真实设备上执行过**。首次执行后请把实测偏差与踩坑回填本文，并去掉本行。
+
+---
+
+## 0. 一页速览
+
+| 阶段 | 内容 | 出口判据（出处） | 解除的阻塞 |
+|------|------|-----------------|-----------|
+| P0 | 开箱登记与数据清点 | 登记表（附录 A.1）填满；缺失项有替代方案 | 厂商数据可用性未知 |
+| P1 | 几何参数回填 | fail-fast 校验通过 + 新有效域覆盖任务包线（§9.1/§11.2） | 几何参数为占位 |
+| P2 | 校正质量核验 | 静态场景 \|Δy\| P95 ≤ 1 px（§12.1） | 极线对齐不可信 |
+| P3 | 真实素材采集入库 | 覆盖 5/10/15/20/25 m 档、元数据齐（§2.3） | 素材只有合成图 |
+| P4 | 测距真值验证 | 有效域内 δZ ≤ 2 m、趋势 ∝ Z²（§12.1） | 数值从未对过真值 |
+| P5 | 阈值整定 | §9 各表 ⚠ 全部替换为实测值（§12.2） | 阈值全部占位 |
+| P6 | 台架/SITL 联动 → 解禁评审 | 控制联动场景全覆盖（§12.1）；§12.3 四条逐条勾选 | 红线未解除 |
+
+依赖顺序：P0 → P1 → P2 → P3 → P4 → P5 → P6。P2 必须在 P1 之后（校正质量要在
+正确内参下判读），P4 必须在 P3 之后（没有真实素材就没有真值比对）。
+
+---
+
+## 1. 阶段 0：开箱登记与数据清点
+
+### 1.1 向厂商索取的清单
+
+| 数据 | 用途 | 缺失时的替代方案 | 落位 |
+|------|------|-----------------|------|
+| 校正后内参 `fx_px`/`fy_px`/`cx_px`/`cy_px` | §9.1 几何组，`Z = F·B/D` 的分母来源 | 自标定（张正友）；**P2 判据不达标时本就要提前做** | `utils/config/stereo_ranger.json` |
+| 基线 `baseline_m`（标称 + 实测公差） | §9.1；`F·B` 常量 | 卡尺/塞规实测，记录不确定度 | 同上 |
+| 畸变系数与校正映射（remap maps）或"已做校正"的书面确认 | §2.2 第 4 条：前端输出的是校正后图上的框 | 自标定 + 自建 remap 表 | 采集预处理（不在本仓） |
+| 出厂标定报告与不确定度 | P1/P2 的判据依据，**要存档** | 无报告 = 按未标定处理，走自标定 | 本文附录 A.1 |
+| 分辨率 / 帧率 / 像素格式 / 位深 | 原生坐标系与算力预算（《需求说明》§5） | — | 附录 A.1 + `stereo_yolo.json` 复核 |
+| 同步方式与触发接口（sbs / 双流 / 硬触发） | §2.3 同帧契约的形态选择 | sbs 优先（天然同曝光，§2.3） | 采集端设计 |
+| 机械图纸（安装接口、两目光轴平行度要求） | 安装外参、换机型（P6） | — | 附录 A.1 |
+
+### 1.2 到货当天建议实测
+
+1. **基线复核**：卡尺/图纸比对标称值，偏差 >1% 时以实测为准并记录（`F·B` 直接决定距离标定）；
+2. **同帧性验证**：拍快速横向移动的手/笔，sbs 形态应为同一时刻；双流形态量化两路时间窗抖动并记录（§2.3 要求"残余时序抖动应记录供误差评估"）；
+3. **分辨率与帧率实测**：跑满声称帧率，记录实际值与丢帧情况（供 P5/P6 的算力预算）；
+4. **图像质量目测**：左右目曝光/增益是否一致（不一致会让同一目标两目置信度与框边缘不对称，污染视差取点）。
+
+---
+
+## 2. 阶段 1：几何参数回填
+
+### 2.1 改哪些字段
+
+只动 `utils/config/stereo_ranger.json` 的 **5 个几何项**：
+
+```
+fx_px, fy_px, cx_px, cy_px   ← 厂商校正后内参（注意：fx=fy=F 是校正后口径）
+baseline_m                    ← 实测基线
+```
+
+`utils/config/stereo_yolo.json` **不含几何参数**（输出框天然位于输入图的原生
+坐标系，§4 配置一致性要求）——前端无需回填，只需复核 `imgsz` 与真实分辨率匹配。
+
+**视差域 `disparity_min_px` / `disparity_max_px` 不是标定数据**，是任务决策项：
+按任务近界 `Z_near` 反推 `disparity_max_px = F·B / Z_near`，按可测远界定
+`disparity_min_px`。回填内参后 `F·B` 变化，两者的距离语义会平移，**必须重算**。
+
+### 2.2 回填后必算的派生量（写进记录表）
+
+| 派生量 | 公式（代码属性） | 用途 |
+|--------|-----------------|------|
+| `F·B` | `fx_px × baseline_m`（`focal_baseline_px_m`） | 距离解算常量 |
+| `z_min_m` | `F·B / disparity_max_px` | 近界（只读派生） |
+| `z_max_m` | `F·B / disparity_min_px` | 远界（只读派生） |
+| `z_error_budget_max_m` | `sqrt(max_delta_z_m × F·B)` | 误差预算门对应的最远距离（§7.2，δD 基准固定 1 px） |
+
+以上四项**不是配置项**，由 `StereoConfig` 只读派生（v1.0 冻结：视差域权威，
+距离域派生，避免两处冗余互推舍入不一致，§6.4）。回填后手算一遍与程序输出比对，
+作为回填正确性的一次交叉验证。
+
+### 2.3 判据
+
+1. **fail-fast 校验通过**：JSON 必须含全部 17 个键、无未知键、类型正确
+   （`consistency_N_frames` / `consistency_M_frames` / `filter_reset_frames`
+   必须是整数，`3.5` 或 `true` 会被拒）；取值约束：`0 < disparity_min_px < disparity_max_px`、
+   `consistency_M_frames ≥ consistency_N_frames`、`0 < smooth_alpha ≤ 1`、
+   `cx_px`/`cy_px` 非负、其余数值项一律为正（以 `stereo_ranger/config.py` 为准）；
+2. **有效域覆盖任务包线**：新的 `z_min_m` / `z_max_m` / `z_error_budget_max_m`
+   与任务的距离区间逐项比对，**不达标时先改视差域/基线/机型（P6），不要迁就旧数值**；
+3. 记录回填前后的 diff 与批准人（附录 A.2）。
+
+### 2.4 连带修改清单（容易漏，会导致测试变红）
+
+| 受影响处 | 现象 | 处置 |
+|---------|------|------|
+| `utils/tests/stereo_ranger/config_test.py::test_load_from_json_matches_defaults` | **必失败**：该用例断言 JSON 内容与 `StereoConfig()` 占位默认值逐字段相等 | 改为"语义一致性"断言（域关系、派生恒等式、取值约束），或维护一份"当前标定值"副本单独比对；**不要**为了过测试而改 dataclass 默认值——默认值是单测的占位基准 |
+| `utils/tests/stereo_yolo/config_test.py::test_load_from_json_matches_defaults` | 同上（仅当改动 `stereo_yolo.json` 时） | 同上 |
+| `docs/双目测距技术路线.md` §9 各表的 ⚠ 标记 | 表格仍写占位值 | 回填为实测值并标注日期与来源（保持"以规格为准"） |
+| `utils/stereo_ranger/README.md`、`utils/stereo_yolo/README.md` 的占位值表 | 同上 | 同步，并撤掉"占位红线"字样（或改为"已标定"） |
+| `utils/demo_stereo_ranger.py` 的注释（`F·B=57.6 px·m`） | 注释过时 | 同步数值 |
+
+---
+
+## 3. 阶段 2：校正质量核验
+
+**判据（§12.1 校正质量）**：静态场景下，左右匹配框的 \|Δy\| 分布 **P95 ≤ 1 px**（⚠占位）。
+不达标 → **触发自标定提前**（§7 风险表第 7 项、《需求说明》§6）。
+
+### 3.1 建议方法
+
+1. 静态标靶（棋盘格或随机纹理板）摆在近/中/远 3 档（如 3 / 8 / 15 m），每档在画面
+   中心与四角共 5 个区域成像；
+2. 每处采 ≥50 帧，用**检测链路本身**（前端 → 两目框）统计 `|y1_L − y1_R|` 与
+   `|cy_L − cy_R|` 双口径的 P95；
+3. 交叉验证：若统计值接近 1 px，改用角点/纹理匹配口径（不依赖检测框边缘）复核，
+   避免把"检测边缘抖动"误判成"校正误差"。参考量级：本仓合成对（已知零校正误差）
+   在 1080p 下实测 \|Δy\| ≈ 0.2 px，即**检测链路自身就贡献约 0.2 px 噪声**。
+
+### 3.2 不达标时的排查顺序
+
+1. 内参/remap 是否张冠李戴（最常见：用原始图内参解释校正图）；
+2. 校正映射是否按原生分辨率生成（缩放过的 map 会引入亚像素偏置）；
+3. 两目光轴平行度/安装（机械问题，需厂商介入）；
+4. 仍不达标 → 自标定（P2 不达标即 v1.1 自标定提前，§7 风险表）。
+
+⚠ **工具缺口**：仓库目前**没有**该统计脚本，需新增（建议 `utils/tools/calib_check.py`，
+输入左右目图像目录，输出 \|Δy\| 直方图与 P95）。
+
+---
+
+## 4. 阶段 3：真实素材采集与入库
+
+### 4.1 采集清单
+
+| 档位 | 内容 | 用途 |
+|------|------|------|
+| 5 / 10 / 15 / 20 m | 静态标靶（或无人机目标静置），每档 ≥50 帧，手持激光测距仪同步记录真值 | P4 测距精度（§12.1） |
+| ≥25 m（域外档） | 同上 | 验证"域外不撒谎"：`valid=false` 且方位照常发布（§12.3 第 3 条） |
+| 横向快速移动 | 目标在画面内快速平移 | 帧同步核验（§12.1：无系统性虚假视差） |
+| 多目标 | 同画面 2 个同类目标 + 1 个非目标干扰 | P5 的 `isolation_gate_px` 整定 |
+| 丢失-恢复 | 目标被遮挡 ≥ `filter_reset_frames` 帧后重现 | P5 的 `filter_reset_frames` / `smooth_alpha` 整定 |
+
+### 4.2 命名与落位（建议）
+
+```
+utils/assets/stereo_yolo/<YYYYMMDD>/z05m_01_L.png
+                                      z05m_01_R.png    # 双流形态
+                                      z05m_01_sbs.png  # sbs 形态（推荐，§2.3）
+                                      z05m_01.json     # 元数据
+```
+
+元数据必填字段：拍摄时间、距离真值 + 测距仪型号、目标类型、光照/天气、曝光与增益、
+同步形态、分辨率、帧号范围、操作人。
+
+### 4.3 注意
+
+- **素材必须是校正后的图**（或同时保存原始图 + 本次使用的 remap 表）——否则 P4 的
+  误差里混进校正误差，无法归因（§2.2 第 4 条）；
+- 替换 `utils/assets/stereo_yolo/drone_sky.png` 只能换"单图冒烟"的输入；上面这套
+  目录结构需要配回放脚本（见 P4 工具缺口）。
+
+---
+
+## 5. 阶段 4：测距真值验证
+
+**判据（§12.1 测距精度）**：以**地面手持激光测距仪**为真值（D8：测距方向与 `Z`
+一致，不是斜距 `R`）；有效域内 **δZ ≤ 2 m**（⚠占位）；误差趋势符合 `δZ ∝ Z²`（§7.1）。
+
+### 5.1 方法与产物
+
+1. 每档 ≥50 组"前端 → `StereoRanger.update()`"输出，与真值比对，输出**误差-距离曲线**；
+2. 由曲线拟合实测 δD（视差噪声基准，当前固定 1 px 是占位，§12.2），
+   并用 `Z_budget = sqrt(max_delta_z_m × F·B / δD_meas)` 反推"预算门"应落在哪；
+3. 单独跑域外档，确认 `valid=false` 且 `center_mid_px` 仍发布（§7.3 域外不撒谎，
+   也不丢方位）；
+4. 结果记入附录 A.4。
+
+### 5.2 不通过时的归因顺序
+
+| 现象 | 首先怀疑 | 处置 |
+|------|---------|------|
+| 全档系统性偏置 | 内参/基线错、取点规则 | 复核 P1 回填值；复核 `x1` 取点（§6.1 同侧边缘） |
+| 误差随 Z² 放大且系数偏大 | 实测 δD 大于既定基准 | 收紧 `max_delta_z_m` 或缩小有效域；把实测 δD 写回规格（§12.2） |
+| 远场随机跳变 | 检测边缘抖动 + 小视差放大 | 属预期（§7.2），确认降级/门限按 §6.4 生效即可 |
+| 横向移动时出现虚假视差 | 帧同步 | 回到 P3 复核同步形态（§2.3） |
+
+⚠ **工具缺口**：仓库目前**没有**"真实素材回放"脚本（现 demo 的合成对是解析构造的），
+需新增（建议 `utils/tools/replay_stereo.py`，输入 4.2 的目录结构，输出逐帧记录
+与误差表）。
+
+---
+
+## 6. 阶段 5：阈值整定清单
+
+§9 表中每个 ⚠ 都要在本阶段被实测值替换。整定依据与建议方法：
+
+| 参数 | 占位值 | 整定输入 | 建议方法 |
+|------|-------|---------|---------|
+| `tau_y_px` | 2.0 | P2 的 \|Δy\| 分布 | 取 P95 的 1.5~2 倍留余量 |
+| `tau_h_ratio` | 0.25 | 真实框高相对差分布 | 同上口径 |
+| `disparity_min_px` | 3.0 | 可测远界需求 | 由任务远界反推（P1 已重算） |
+| `disparity_max_px` | 57.6 | 任务近界 | 由 `Z_near` 反推（P1 已重算） |
+| `max_delta_z_m` | 2.0 | P4 的误差曲线 | 由可接受的最大距离误差定；与 δD 实测联动（P4 第 2 步） |
+| `consistency_jump_px` | 3.0 | 静态场景视差噪声 P95 | 取噪声峰值的数倍：太小误判、太大漏检（§6.7） |
+| `consistency_N_frames` | 3 | 安全需求 + 误报率 | N/M 决定"异常后多久降级/判无效"（3/5 ≈ 0.17 s @30 fps），与消费侧的降级时延一起定 |
+| `consistency_M_frames` | 5 | 同上 | 必须 ≥ N |
+| `smooth_alpha` | 0.3 | P4 数据离线拟合 | 在"噪声抑制"与"群延迟"间取平衡（0.3 ≈ 3.3 帧群延迟） |
+| `jump_reset_m` | 5.0 | 真实跳变幅度观测 | 用目标切换/大机动场景标定 |
+| `filter_reset_frames` | 5 | 丢失-恢复场景 | 与消费侧 coast 策略配合（§6.5） |
+| `isolation_gate_px` | 50.0 | 多目标场景 | 由"同帧不同目标的最小像素间距"定，取小于该值的门限 |
+
+前端侧同步复核：`conf_threshold` / `iou_threshold` 与真实素材上的漏检/误检权衡；
+`imgsz` 与平台算力（P7）——**这两项不随标定变化，但要与检测质量一起整定**。
+
+---
+
+## 7. 阶段 6：台架/SITL 联动与解禁评审
+
+### 7.1 台架联动（判据见 §12.1 控制联动）
+
+回放/SITL 下跑通：距离闭环 ↔ 定速接近切换、`invalid` 持续降级路径、
+远场"方位闭环 + 定速接近"（D4）——场景覆盖要求见《视觉跟踪控制律技术路线》第 7 章。
+
+### 7.2 解禁检查表（§12.3 四条逐条勾选）
+
+- [ ] **红线 1**：§7/§9 中所有 ⚠ 数值已由实测/厂商数据确认，替换记录已归档（附录 A.2/A.3/A.4）；
+- [ ] **红线 2**：相机参数与阈值全部在配置文件中，代码内无写死（含 demo/工具脚本复核）；
+- [ ] **红线 3**：域外/失配确实输出 `valid = false`——合成回归（`pytest -q`）+ 真实素材（P4 域外档）两轮都有证据；
+- [ ] **红线 4**：地面真值验证完成并归档（P4 全档通过）。
+
+### 7.3 解禁之后仍未解禁的
+
+本文解除的是**测距/检测链路**的红线。实飞还要求控制律侧完成
+《视觉跟踪控制律技术路线》§8 阶段 6 的增益与限幅整定——两者都满足才可实飞。
+
+---
+
+## 附录 A：记录表模板
+
+### A.1 开箱登记
+
+| 项 | 值 |
+|----|----|
+| 型号 / 序列号 | |
+| 分辨率 / 帧率 / 像素格式 | |
+| 标称基线 / 实测基线（工具、公差） | |
+| 校正后内参（fx, fy, cx, cy） | |
+| 畸变系数 / remap 表版本 | |
+| 出厂标定报告编号与日期 | |
+| 同步形态（sbs / 双流 / 硬触发） | |
+| 到货日期 / 登记人 | |
+
+### A.2 参数回填记录
+
+| 字段 | 回填前 | 回填后 | 来源 | 日期 / 批准 |
+|------|-------|-------|------|------------|
+
+派生量复核（手算 vs 程序）：`F·B` = ＿＿＿，`z_min_m` = ＿＿＿，`z_max_m` = ＿＿＿，
+`z_error_budget_max_m` = ＿＿＿。
+
+### A.3 校正质量核验
+
+| 档位 | 区域 | 样本数 | \|Δy\| P95 | 判据 ≤1 px | 备注 |
+|------|------|-------|-----------|-----------|------|
+
+### A.4 测距真值验证
+
+| 距离真值 | 样本数 | Z 均值 | δZ | 判据 ≤2 m | valid 率 | 备注 |
+|---------|-------|-------|----|----------|---------|------|
+
+---
+
+## 附录 B：判据来源索引
+
+| 本文阶段 | 规格出处 |
+|---------|---------|
+| P1 回填与派生量 | 《测距路线》§9.1、§6.4、§11.2 |
+| P1 连带修改 | `utils/stereo_ranger/config.py`（fail-fast 规则）、两份 `config_test.py` |
+| P2 校正质量 | 《测距路线》§12.1、§7 风险表第 7 项；《需求说明》§6 |
+| P3 采集与同步 | 《需求说明》§2.3；《测距路线》§12.1 帧同步 |
+| P4 真值验证 | 《测距路线》§12.1、§7.1、§7.2、§7.3（D8 真值方向） |
+| P5 阈值整定 | 《测距路线》§9 全表、§12.2；《需求说明》§4 |
+| P6 联动与解禁 | 《测距路线》§12.1、§12.3；《视觉跟踪控制律技术路线》§7/§8 |
+
+## 附录 C：已知缺口（执行前需补齐）
+
+1. **工具脚本缺三件**（本文标注 ⚠）：校正质量统计（P2）、真实素材回放（P4）、
+   真值-误差曲线拟合（P4）。在它们就位前，P2/P4 只能手工抽样，判据不可复现；
+2. **外部参考文档缺档**：《测距路线》§12.2 把开放问题（P1 焦距规格、P2 出厂标定、
+   P3 帧同步形态、P6 换机型、P7 性能实测、P8 避障激光去留）的**权威清单**指向
+   《双目深度估计思路》第 15 章，该文档当前不在仓库中——需要它才能核对问题编号与
+   结论，请补回或改引他处；
+3. **P7（性能实测）不在本文范围**：它依赖机载/边缘平台，属平台工程文档。

legacy/utils/conftest.py

@@ -0,0 +1,19 @@
+"""pytest 根配置（归档目录专用）。
+
+归档的 175 条测试仍 import `stereo_yolo` / `stereo_ranger` / `target_tracker`
+（三者都在本目录的 utils/ 下），部分还复用运行链路的 `visual_track_control`
+（留在仓库根的 utils/）。因此这里把本目录的 utils/ 与仓库根的 utils/ 都加入
+sys.path，使 `cd legacy/utils && pytest` 可直接运行。
+
+默认（从仓库根 utils/ 跑）不会收集本目录，主链路测试不受影响。
+"""
+
+import sys
+from pathlib import Path
+
+_LEGACY_UTILS = Path(__file__).resolve().parent          # legacy/utils
+_REPO_UTILS = _LEGACY_UTILS.parents[1] / "utils"         # 仓库根/utils
+
+for _p in (_LEGACY_UTILS, _REPO_UTILS):
+    if str(_p) not in sys.path:
+        sys.path.insert(0, str(_p))

legacy/utils/pytest.ini

@@ -0,0 +1,6 @@
+[pytest]
+testpaths = tests
+python_files = *_test.py test_*.py
+; 与运行链路 utils/pytest.ini 同口径：各包存在同名测试文件（如 config_test.py），
+; importlib 模式避免按裸模块名导入时的 basename 冲突。
+addopts = --import-mode=importlib

requirements-pod.txt

@@ -1,6 +1,6 @@
 # requirements-pod.txt —— 树莓派 4B(Ubuntu 24.04)吊舱桥接最小依赖
-# 只装这几个,切勿在 Pi 上装全量 requirements.txt(torch/cv2 无意义且数百 MB)
+# 只装这几个，切勿在 Pi 上装全量 requirements.txt（cv2/torch 无意义且数百 MB）
 # 详见 docs/树莓派4B部署注意事项.md §2
-mavsdk<4    # PX4 遥测订阅 + offboard;⚠ 必须锁 <4:v4 是全新原生 API 重写,与本仓库经典 gRPC API 不兼容(最新 3.17.4 有 aarch64 wheel)
+mavsdk<4    # PX4 遥测订阅 + offboard；⚠ 必须锁 <4：v4 是全新原生 API 重写，与本仓库经典 gRPC API 不兼容（最新 3.17.4 有 aarch64 wheel）
 pyserial    # 吊舱串口(115200 8N1)
-pytest      # 仅上机自测(tests/pod_bridge 92 条)需要,部署运行可省
+pytest      # 仅上机自测(tests/pod_bridge 208 条)需要，部署运行可省

requirements.txt

@@ -1,21 +1,18 @@
-# code_drone-test Python 依赖
+# code_drone-test Python 依赖（运行链路）
-# 扫描自全仓库 .py 文件的实际 import(2026-09-22)
+# 按 utils/ + ground_station/ + tools/ 的实际 import 核对（2026-10-20 精简后）
 
-# ---- 运行时核心 ----
+# ---- 运行链路核心 ----
-numpy                   # target_tracker 卡尔曼滤波 / stereo_yolo sbs 拆分
+mavsdk<4                # ⚠ 锁 <4：v4 为全新原生 API，经典 gRPC API（System/offboard/telemetry）已移除
-opencv-python           # camera.py / camera_test.py / video_test.py / demo_stereo_yolo.py (cv2)
+pyserial                # 吊舱串口（pod_bridge.pod_serial，惰性导入）
 
-# ---- 检测推理(stereo_yolo,真实推理才加载,torch 为其依赖) ----
+# ---- 地面站页面 / 视频（ground_station.app + video_source）----
-ultralytics
+flask                   # 页面与 API
-
+opencv-python           # RTSP 拉流 / MJPEG 推流；tools/ 下相机脚本也用
-# ---- 飞控通信(MAVSDK offboard -> PX4) ----
+numpy                   # video_source / tools 图像处理
-mavsdk<4                # ⚠ 锁 <4:v4 为全新原生 API,经典 gRPC API(System/offboard/telemetry)已移除
-
-# ---- 吊舱串口(pod_bridge.pod_serial,惰性导入) ----
-pyserial
-
-# ---- 可视化(test.py 轨迹绘图) ----
-matplotlib
 
 # ---- 测试 ----
-pytest                  # utils/tests 全量单测(256 条)
+pytest                  # utils/tests 运行链路单测（446 条）
+
+# ---- 可选：已归档的感知后端（仅跑 legacy/ 时需要）----
+# ultralytics           # legacy/utils/stereo_yolo 真模型推理路径
+# torch                 # ultralytics 依赖

task_runner_real.py

@@ -1,27 +1,33 @@
-#!/usr/bin/env python3
+#!/usr/bin/env python3
 """task_runner_real.py —— 真机任务编排入口（真实吊舱串口 + 地面站 UDP 注入 + PX4）。
 
 与 task_runner_sitl.py（仿真）的差异：
   * 吊舱 = PodSerial（真实串口表2-2 遥测），非 VirtualPod；
   * 地面坐标 = GroundLink.run()（UDP 接收地面站 JSON），非 inject() 直接注入；
-  * 吊舱目标识别/锁定 = PodController（GOTO 进入发 0x07 识别 + 0x0F 自动锁定，
+  * 吊舱目标识别/锁定 = PodController（首次进入引导阶段发 0x07 识别 + 0x0F 自动锁定，
-    RTL 前发 0x0E 停止 + 0x08 识别关）；
+    RTL 前发 0x0E 停止 + 0x08 识别关；**只发一次**，重入不会重发打断跟踪）；
-  * PX4 连接 = cfg.px4.connection_string（真机串口/遥测），非 SITL 14540。
+  * PX4 连接 = cfg.px4.connection_string（真机串口/遥测），非 SITL 14540；
+  * 收尾 = RTL 返航（默认）或 `--land` 就地降落；
+  * SIGINT/SIGTERM 优雅停机：先请状态机转 RTL，再退出（不把飞机留在 offboard 上）。
+
+任务链（与 SITL 一致）：WAIT→ARM_TAKEOFF→GOTO(飞向参考点)→SEARCH(原地扫视)→TRACK(缓慢靠近)→RTL
 
 前置（真机联调增量：纯软单测 → 串口回环 → 仅吊舱 → 仅飞控 → 地面联调）：
   1. 填 utils/config/pod_bridge.json 的 pod_serial.port / px4.connection_string；
   2. 吊舱上电、串口连通（run_bridge.py --dry-run 已验证遥测）；
-  3. 地面站往 ground_inject.port（默认 14555）发 JSON 坐标
+  3. 地面站往 ground_inject.port（默认 14555）**持续**发 JSON 坐标
-     {"lat":..,"lon":..,"alt":..}（每 0.5~1s 一次）。
+     {"lat":..,"lon":..,"alt":..}（每 0.5~1s 一次）；停发会导致坐标超龄 → 返航。
 
 用法：
   python task_runner_real.py
   python task_runner_real.py --config utils/config/pod_bridge.json --target-type 1
+  python task_runner_real.py --port /dev/ttyAMA0 --land
 """
 
 import argparse
 import asyncio
 import logging
+import signal
 import sys
 import time
 from datetime import datetime
@@ -42,9 +48,10 @@ from pod_bridge.pod_serial import PodSerial
 from pod_bridge.px4_gateway import Px4Gateway
 from task_runner.log import TaskLogConfig, TaskLogger
 from task_runner.pod_control import PodController
+from task_runner.return_home import ReturnConfig
 from task_runner.runner import TaskRunner
 from task_runner.state_machine import TaskStateConfig, TaskStateMachine
-from pod_sitl_test import now_ms, print_flight_mode, set_telemetry_rates, wait_until_ready
+from sitl_support import now_ms, print_flight_mode, set_telemetry_rates, wait_until_ready
 
 logger = logging.getLogger("task_real")
 
@@ -57,25 +64,92 @@ def build_sm_config() -> TaskStateConfig:
     """真机状态机参数。⚠ 全部为占位值，须台架标定后回填，标定前不得实飞。"""
     return TaskStateConfig(
         takeoff_altitude_m=5.0,
-        switch_radius_m=30.0,      # GOTO→TRACK 切换半径（待标定）
+        arrival_radius_m=30.0,     # 到参考点半径 → 转搜索（待标定）
         goto_timeout_s=180.0,
-        track_timeout_s=180.0,
+        search_timeout_s=20.0,     # 搜索 20s 未发现目标 → 返航
-        center_hold_s=3.0,
+        track_timeout_s=20.0,      # 追踪/靠近段上限 → 返航（无距离，判不出"已到目标"）
-        center_threshold_deg=3.0,
+        center_threshold_deg=3.0,  # 仅日志/诊断
-        approach_hover_s=5.0,
+        sweep_rate_dps=15.0,       # 搜索扫视角速度（度/秒，待标定）
+        sweep_reverse_s=4.0,       # 每 4s 换向
         loop_frequency_hz=20.0,
     )
 
 
+def build_return_config() -> ReturnConfig:
+    """自定义返航参数：回起飞点 → 恢复初始航向 → 垂直降落（不额外爬高）。
+
+    ⚠ 速度为占位值，须按机体/场地标定。
+    """
+    return ReturnConfig(
+        enabled=True,
+        speed_mps=2.0,             # 转场水平速度
+        arrival_radius_m=3.0,      # 距起飞点该半径内视为到位
+        timeout_s=120.0,           # 转场超时 → PX4 RTL 兜底
+        yaw_kp=1.0,
+        yaw_rate_limit_dps=20.0,
+        align_kp=1.0,
+        align_rate_limit_dps=15.0,
+        yaw_align_tolerance_deg=5.0,
+        yaw_align_timeout_s=15.0,
+        alt_kp=0.5,
+        alt_limit_mps=0.5,
+    )
+
+
+def _sync_staleness_with_injection(cfg) -> None:
+    """把「坐标时效」口径统一到 `ground_inject.timeout_ms`。
+
+    ⚠ 这是 SITL 实测暴露的一个真机必踩的坑：`guidance.target_stale_ms` 默认 1000ms，
+    而地面站按 0.5~1s 上报 → 报文间隔稍有抖动（无线链路/页面节流）就会让引导律判
+    「目标超龄 → 零速刹车」，表现为飞机在路上走走停停；而任务层要等
+    `ground_inject.timeout_ms`（默认 5000ms）才判坐标失效。
+    SITL 入口是硬编码 5000ms 绕过的，真机这里改为**派生**，两个门限只有一个来源。
+    """
+    inject_ms = int(cfg.ground_inject.timeout_ms)
+    if cfg.guidance.target_stale_ms < inject_ms:
+        logger.info("[Task] guidance.target_stale_ms %d → %d（与 ground_inject 对齐）",
+                    cfg.guidance.target_stale_ms, inject_ms)
+        cfg.guidance.target_stale_ms = inject_ms
+
+
+def _install_signal_handlers(stop: asyncio.Event, runner: TaskRunner) -> None:
+    """SIGINT/SIGTERM → 请求返航并退出主循环（真机在树莓派上被 kill 也要安全收尾）。
+
+    只置位不抛异常：让 `runner.run()` 走正常的 RTL 分支（停机+返航+落地+上锁），
+    而不是让进程带着 offboard 速度指令消失。
+    """
+    loop = asyncio.get_running_loop()
+
+    def _handler(sig_name: str) -> None:
+        logger.warning("[Task] 收到 %s：请求返航并退出", sig_name)
+        runner.request_rtl()
+        stop.set()
+
+    for name in ("SIGINT", "SIGTERM"):
+        sig = getattr(signal, name, None)
+        if sig is None:
+            continue
+        try:
+            loop.add_signal_handler(sig, _handler, name)
+        except (NotImplementedError, RuntimeError, ValueError):
+            # Windows / 非主线程不支持：退化为 signal.signal（可能不生效）
+            try:
+                signal.signal(sig, lambda *_a, _n=name: _handler(_n))
+            except (ValueError, OSError):
+                pass
+
+
 async def run(args) -> int:
     cfg = load_from_json(args.config)
+    if args.port:
+        cfg.pod_serial.port = args.port  # 先应用覆盖，再校验（否则 --port 救不了空配置）
     if not cfg.pod_serial.port:
         print("! 未配置吊舱串口：在 pod_bridge.json 填 pod_serial.port，或用 --port 覆盖")
         return 2
-    if args.port:
-        cfg.pod_serial.port = args.port
     cfg.ground_inject.enabled = True  # 地面注入必须开（真机无激光，表4 永不出）
+    _sync_staleness_with_injection(cfg)  # 坐标时效口径统一（SITL 实测踩过的坑）
     sm_cfg = build_sm_config()
+    return_cfg = build_return_config()
 
     drone = System()
     await drone.connect(system_address=args.conn or cfg.px4.connection_string)
@@ -97,15 +171,24 @@ async def run(args) -> int:
     log_dir = args.log_dir or f"logs/real_{datetime.now().strftime('%Y%m%d_%H%M%S')}"
     task_log = TaskLogger(TaskLogConfig(dir=log_dir), now_ms_fn=now_ms)
     runner = TaskRunner(cfg, sm_cfg, px4, bridge, sm, ground, drone,
-                        now_ms_fn=now_ms, logger=task_log, pod_control=pod_control)
+                        now_ms_fn=now_ms, logger=task_log, pod_control=pod_control,
+                        land_instead_of_rtl=args.land, return_cfg=return_cfg)
 
     stop = asyncio.Event()
+    _install_signal_handlers(stop, runner)
     ground_task = asyncio.ensure_future(ground.run(stop))  # UDP 接收地面站坐标
     runner_task = asyncio.ensure_future(runner.run(stop))
 
     print(f"-- 启动真机任务全流程（吊舱 {cfg.pod_serial.port}，"
-          f"地面站 UDP {cfg.ground_inject.port}，目标类型 {args.target_type}）")
+          f"地面站 UDP {cfg.ground_inject.port}，目标类型 {args.target_type}，"
-    print(f"   等待地面站注入坐标（每 0.5~1s 一次 JSON）… 日志 → {log_dir}")
+          f"收尾 {'就地降落' if args.land else '返航'}）", flush=True)
+    print(f"   状态机: takeoff={sm_cfg.takeoff_altitude_m}m "
+          f"arrival_radius={sm_cfg.arrival_radius_m}m "
+          f"search={sm_cfg.search_timeout_s}s@{sm_cfg.sweep_rate_dps}°/s "
+          f"track_max={sm_cfg.track_timeout_s}s | "
+          f"坐标时效 ground_inject={cfg.ground_inject.timeout_ms}ms "
+          f"guidance={cfg.guidance.target_stale_ms}ms", flush=True)
+    print(f"   等待地面站注入坐标（每 0.5~1s 一次 JSON）… 日志 → {log_dir}", flush=True)
 
     started = time.monotonic()
     last_state = None
@@ -114,17 +197,20 @@ async def run(args) -> int:
             await asyncio.sleep(1.0)
             if runner.state is not last_state:
                 last_state = runner.state
-                print(f"[任务] 进入状态: {runner.state.value}")
+                print(f"[任务] 进入状态: {runner.state.value}", flush=True)
             if runner_task.done():
                 exc = runner_task.exception()
                 if exc is not None:
-                    print(f"! runner 异常退出: {type(exc).__name__}: {exc}")
+                    print(f"! runner 异常退出: {type(exc).__name__}: {exc}",
+                          flush=True)
                 break
+            if stop.is_set():
+                break  # 信号处理已请求退出，交给 finally 收尾
             if time.monotonic() - started > TASK_TIMEOUT_S:
-                print("! 全流程超时，强制返航")
+                print("! 全流程超时，强制返航", flush=True)
                 runner.request_rtl()
         if runner.rtl_done:
-            print("-- 任务完成（RTL 落地）")
+            print("-- 任务完成（落地）", flush=True)
     except asyncio.CancelledError:
         raise
     finally:
@@ -141,7 +227,7 @@ async def run(args) -> int:
             pass
 
     await print_flight_mode(drone)
-    print("\n===== 真机任务全流程完成 =====")
+    print("\n===== 真机任务全流程完成 =====", flush=True)
     return 0
 
 
@@ -153,11 +239,18 @@ def main() -> int:
     ap.add_argument("--target-type", type=int, default=1,
                     help="吊舱目标类型：0=人/车，1=无人机")
     ap.add_argument("--log-dir", default=None, help="日志输出目录（默认按时间戳）")
+    ap.add_argument("--land", action="store_true",
+                    help="收尾改为就地降落（默认返航 RTL）")
     args = ap.parse_args()
 
     logging.basicConfig(level=logging.INFO,
                         format="%(asctime)s.%(msecs)03d %(levelname)s %(message)s",
                         datefmt="%H:%M:%S")
+    # 真机常在树莓派上经 SSH 运行：stdout 非 tty 时块缓冲会让进度看不到，强制行缓冲
+    try:
+        sys.stdout.reconfigure(line_buffering=True)
+    except (AttributeError, ValueError):
+        pass
     return asyncio.run(run(args))
 
 

task_runner_sitl.py

@@ -1,19 +1,25 @@
 #!/usr/bin/env python3
 """task_runner_sitl.py —— 任务级状态机 SITL 全流程集成测试（预定流程.md 天空侧）。
 
-用状态机驱动完整流程：WAIT → ARM_TAKEOFF → GOTO(L2 坐标引导) → TRACK(L1 航向伺服)
+用状态机驱动完整流程：
-→ APPROACH(悬停) → RTL(返航落地)。相比 pod_sitl_test.py（固定场景、人工编排），
+  WAIT → ARM_TAKEOFF → GOTO(L2 飞向参考点) → SEARCH(原地扫视) → TRACK(L1 缓慢靠近)
-本脚本把「飞行编排 + 控制律切换」交给 TaskRunner 状态机统一驱动。
+  → RTL(自定义返航：回起飞点 → 恢复初始航向 → 垂直降落)
+
+相比 `l1_sitl_test.py`（只跑 L1 控制律闭环、人工编排），本脚本把「飞行编排 + 控制律切换」
+交给 TaskRunner 状态机统一驱动；返航也不再走 PX4 原生 RTL（那会先爬到
+RTL_RETURN_ALT 再降），而是自定义的低空返航（见 task_runner/return_home.py）。
 
 场景：
   * 起飞前注入虚拟目标坐标（模拟地面站给坐标）——目标 = home 北 30m / 东 15m；
   * VirtualPod 按无电机语义合成表2-2（框架角恒0、无激光、锁定=目标在视场内）；
   * 后台任务持续刷新注入坐标（模拟地面站持续上报），坐标始终新鲜；
-  * GOTO→TRACK 由状态机按「距目标 < switch_radius 且吊舱锁定」守卫切换。
+  * GOTO→SEARCH 按「距参考点 ≤ arrival_radius」切换；SEARCH→TRACK 按「目标锁定」
+    切换（含 LOS 运动学一致性判据，见 pod_bridge/los_consistency.py）。
 
 运行前：启动 PX4 SITL（默认本机 14540）。用法：
   python task_runner_sitl.py
-  python task_runner_sitl.py --conn udp://:14540 --config utils/config/pod_bridge.json
+  python task_runner_sitl.py --conn udpin://0.0.0.0:14540 \
+         --config utils/config/pod_bridge.json
 """
 
 import argparse
@@ -40,9 +46,10 @@ from pod_bridge.px4_gateway import Px4Gateway
 from pod_bridge.virtual_pod import (CarrierView, VirtualPod, VirtualTarget,
                                     VirtualTargetConfig)
 from task_runner.log import TaskLogConfig, TaskLogger
+from task_runner.return_home import ReturnConfig
 from task_runner.runner import TaskRunner
 from task_runner.state_machine import TaskStateConfig, TaskStateMachine
-from pod_sitl_test import (TelemetryHub, now_ms, print_flight_mode,
+from sitl_support import (TelemetryHub, now_ms, print_flight_mode,
                           set_telemetry_rates, wait_until_ready)
 
 logger = logging.getLogger("task_sitl")
@@ -83,16 +90,35 @@ def build_sm_config() -> TaskStateConfig:
     """SITL 状态机参数（占位值，仅用于仿真验证链路）。"""
     return TaskStateConfig(
         takeoff_altitude_m=5.0,
-        switch_radius_m=25.0,      # 目标在 30m，飞进 25m 内切 TRACK
+        arrival_radius_m=25.0,     # 目标在 30m，飞进 25m 内转搜索
         goto_timeout_s=60.0,
-        track_timeout_s=60.0,
+        search_timeout_s=20.0,     # 搜索 20s 未发现目标 → 返航
-        center_hold_s=3.0,
+        track_timeout_s=20.0,      # 追踪/靠近段上限 → 返航
-        center_threshold_deg=3.0,  # 脱靶量 < 3° 判居中
+        center_threshold_deg=3.0,  # 脱靶量 < 3° 判居中（仅日志/诊断）
-        approach_hover_s=5.0,      # 悬停 5s 后 RTL
+        sweep_rate_dps=15.0,       # 搜索扫视角速度（度/秒）
+        sweep_reverse_s=4.0,       # 每 4s 换向
         loop_frequency_hz=20.0,
     )
 
 
+def build_return_config() -> ReturnConfig:
+    """SITL 自定义返航参数（与真机同口径，仅用于仿真验证）。"""
+    return ReturnConfig(
+        enabled=True,
+        speed_mps=2.0,
+        arrival_radius_m=3.0,
+        timeout_s=120.0,
+        yaw_kp=1.0,
+        yaw_rate_limit_dps=20.0,
+        align_kp=1.0,
+        align_rate_limit_dps=15.0,
+        yaw_align_tolerance_deg=5.0,
+        yaw_align_timeout_s=15.0,
+        alt_kp=0.5,
+        alt_limit_mps=0.5,
+    )
+
+
 async def inject_loop(ground: GroundLink, lat: float, lon: float,
                       stop: asyncio.Event) -> None:
     """后台持续刷新注入坐标（模拟地面站持续上报，保坐标始终新鲜）。"""
@@ -111,6 +137,7 @@ async def run(args) -> int:
     cfg.ground_inject.enabled = True  # 启用地面注入（骨架期 JSON 默认关）
     cfg.guidance.target_stale_ms = 5000  # 注入坐标超时放宽（表4 默认 1000 对低频注入太紧）
     sm_cfg = build_sm_config()
+    return_cfg = build_return_config()
 
     drone = System()
     await drone.connect(system_address=args.conn)
@@ -146,7 +173,7 @@ async def run(args) -> int:
     sm = TaskStateMachine(sm_cfg)
     task_log = TaskLogger(TaskLogConfig(), now_ms_fn=now_ms)
     runner = TaskRunner(cfg, sm_cfg, px4, bridge, sm, ground, drone,
-                        now_ms_fn=now_ms, logger=task_log)
+                        now_ms_fn=now_ms, logger=task_log, return_cfg=return_cfg)
 
     stop = asyncio.Event()
     if args.ground_station:
@@ -160,9 +187,9 @@ async def run(args) -> int:
     if args.ground_station:
         print(f"   [地面站模式] 等待 UDP {cfg.ground_inject.port} 注入坐标…")
     print(f"   状态机: takeoff={sm_cfg.takeoff_altitude_m}m "
-          f"switch_radius={sm_cfg.switch_radius_m}m "
+          f"arrival_radius={sm_cfg.arrival_radius_m}m "
-          f"center={sm_cfg.center_threshold_deg}°/{sm_cfg.center_hold_s}s "
+          f"search={sm_cfg.search_timeout_s}s@{sm_cfg.sweep_rate_dps}°/s "
-          f"hover={sm_cfg.approach_hover_s}s")
+          f"track_max={sm_cfg.track_timeout_s}s")
 
     # 状态打印 + 总超时保护
     started = time.monotonic()

test.py

@@ -1,836 +0,0 @@
-#!/usr/bin/env python3
-"""
-PX4 SITL Offboard 视觉跟踪集成测试 (MAVSDK-Python)
-连接: udpin://0.0.0.0:14540
-流程:
-  解锁 → 起飞 → 切换 Offboard（速度设定点）
-  → 视觉跟踪闭环 20 Hz（追踪器 → 控制律 → offboard 网关）
-  → 目标消失 → Coast 预测 → 判丢零速刹车 → 退出 Offboard → RTL → 落地 → 上锁
-适用于 PX4 1.17
-
-YOLO 检测未接入：VirtualTarget 用无人机真实位置/航向做针孔反投影生成
-检测框与双目目标距离（几何一致闭环），无人机会实际飞向该虚拟目标。
-"""
-
-import asyncio
-import logging
-import math
-import sys
-import time
-from pathlib import Path
-
-import matplotlib
-import matplotlib.pyplot as plt
-from matplotlib import font_manager
-from matplotlib.ticker import MaxNLocator
-
-
-def _setup_chinese_font() -> str | None:
-    """为 matplotlib 配置可用的中文字体，避免中文标题/图例乱码。
-
-    优先从字体文件注册，再按 family 名称匹配（Linux Noto/WQY、
-    Windows 雅黑/黑体、macOS 苹方等）。返回实际字体名；失败返回 None。
-    """
-    # 1) 直接按路径注册（TTC/TTF）
-    path_candidates = [
-        "/usr/share/fonts/opentype/noto/NotoSansCJK-Regular.ttc",
-        "/usr/share/fonts/opentype/noto/NotoSerifCJK-Regular.ttc",
-        "/usr/share/fonts/truetype/droid/DroidSansFallbackFull.ttf",
-        "/usr/share/fonts/truetype/wqy/wqy-microhei.ttc",
-        "/usr/share/fonts/truetype/wqy/wqy-zenhei.ttc",
-        "C:/Windows/Fonts/msyh.ttc",
-        "C:/Windows/Fonts/msyhbd.ttc",
-        "C:/Windows/Fonts/simhei.ttf",
-        "/System/Library/Fonts/PingFang.ttc",
-        "/System/Library/Fonts/STHeiti Light.ttc",
-    ]
-    chosen = None
-    for path in path_candidates:
-        if not Path(path).is_file():
-            continue
-        try:
-            font_manager.fontManager.addfont(path)
-            prop = font_manager.FontProperties(fname=path)
-            chosen = prop.get_name()
-            break
-        except Exception:
-            continue
-
-    # 2) 按已注册 family 名称匹配
-    if chosen is None:
-        name_candidates = [
-            "Noto Sans CJK SC",
-            "Noto Sans CJK JP",
-            "Noto Serif CJK SC",
-            "Noto Serif CJK JP",
-            "Droid Sans Fallback",
-            "WenQuanYi Micro Hei",
-            "WenQuanYi Zen Hei",
-            "Source Han Sans SC",
-            "Source Han Sans CN",
-            "Microsoft YaHei",
-            "SimHei",
-            "PingFang SC",
-            "Heiti SC",
-            "Arial Unicode MS",
-        ]
-        available = {f.name for f in font_manager.fontManager.ttflist}
-        for name in name_candidates:
-            if name in available:
-                chosen = name
-                break
-
-    if chosen:
-        matplotlib.rcParams["font.sans-serif"] = [chosen, "DejaVu Sans"]
-        matplotlib.rcParams["font.family"] = "sans-serif"
-        # 解决坐标轴负号显示为方块的问题
-        matplotlib.rcParams["axes.unicode_minus"] = False
-    return chosen
-
-
-_CHINESE_FONT = _setup_chinese_font()
-
-_UTILS_DIR = Path(__file__).resolve().parent / "utils"
-if str(_UTILS_DIR) not in sys.path:
-    sys.path.insert(0, str(_UTILS_DIR))
-
-from mavsdk import System
-from mavsdk.action import ActionError
-
-from utils.target_tracker import Detection, FrameShape, TargetTracker, TrackerConfig
-from utils.demo_track_control import track_result_to_state
-from utils.visual_track_control.config import load_from_json
-from utils.visual_track_control.mavsdk_gateway import MavsdkCommandGateway
-from utils.visual_track_control.tracking_control_law import TrackingControlLaw
-from utils.visual_track_control.types import ControlMode, ControlOutput, ControlSnapshot
-
-logger = logging.getLogger("test")
-
-# ==================== 配置 ====================
-CONNECTION = "udpin://0.0.0.0:14540"   # 推荐写法，也可用 "udp://127.0.0.1:14540"
-TAKEOFF_ALTITUDE = 5.0                 # 起飞高度 (m)
-
-TRACK_CONFIG_PATH = _UTILS_DIR / "config" / "track_control.json"
-TRACK_MAX_LOST_FRAMES = 60             # 追踪器 Coast 窗口（20 Hz 下 = 3 s）
-TRACK_NORMAL_SECS = 20.0               # 虚拟目标保持可见时长（之后进入丢失场景）
-TRACK_BRAKE_SECS = 5.0                 # 判丢后零速刹车时长
-TRACK_TIMEOUT_SECS = 120.0             # 跟踪段整体安全超时
-STATUS_EVERY_TICKS = 10                # 每 0.5 s 打印一行状态
-PLOT_TRAJECTORY = True                 # 跟踪结束后绘制无人机与虚拟目标轨迹
-TRAJECTORY_SAVE_PATH = Path(__file__).resolve().parent / "trajectory_plot.png"
-
-# 虚拟目标：相对悬停锚点、按锚点机头朝向放置的机体系偏移
-# （前方 AHEAD / 右方 RIGHT，横偏用于激发航向通道）。
-# 默认与悬停点同高：地面目标场景下垂直通道会持续下降趋近目标高度，
-# SITL 中有触发 PX4 着陆检测的风险；TARGET_DOWN_M 取正值（目标更低，
-# 如 +5.0 = 地面）即可切换为地面目标场景。
-TARGET_AHEAD_M = 30.0
-TARGET_RIGHT_M = 15.0
-TARGET_DOWN_M = 0.0
-MOCK_BOX_W = 120.0                     # 模拟检测框尺寸 (px)
-MOCK_BOX_H = 160.0
-MOCK_CONFIDENCE = 0.9
-MOCK_TARGET_SPEED_MPS = 1.0            # 圆弧切向速度 (m/s)
-TARGET_ARC_RADIUS_M = 12.0             # 圆弧半径 (m)
-# 旋转方向：+1 = NED 水平面逆时针（从北向东转），-1 = 顺时针
-TARGET_ARC_CCW = 1
-
-
-def now_ms() -> int:
-    return int(time.monotonic() * 1000.0)
-
-
-def plot_trajectories(
-    drone_traj: list,
-    target_traj: list,
-    save_path=None,
-) -> None:
-    """绘制无人机与虚拟目标的 NED 轨迹。
-
-    参数:
-        drone_traj / target_traj: 列表元素为 (t_s, north_m, east_m, down_m)
-        save_path: 若给定则保存图片，否则仅弹窗显示
-    """
-    if not drone_traj and not target_traj:
-        print("! 无轨迹数据，跳过绘图")
-        return
-
-    if _CHINESE_FONT is None:
-        print("! 未找到中文字体，图中中文可能显示为方框。"
-              "可安装 fonts-noto-cjk 或文泉驿，或改用英文标签。")
-    else:
-        # 再次确保本进程内 rc 生效（部分后端会重置）
-        matplotlib.rcParams["font.sans-serif"] = [_CHINESE_FONT, "DejaVu Sans"]
-        matplotlib.rcParams["axes.unicode_minus"] = False
-
-    fig, axes = plt.subplots(1, 2, figsize=(12, 5))
-
-    # ---- 左图：水平面 N-E 轨迹（俯视图）----
-    ax = axes[0]
-    if drone_traj:
-        dn = [p[1] for p in drone_traj]
-        de = [p[2] for p in drone_traj]
-        ax.plot(de, dn, "b-", linewidth=1.5, label="无人机")
-        ax.scatter(de[0], dn[0], c="blue", s=60, marker="o", zorder=5, label="无人机起点")
-        ax.scatter(de[-1], dn[-1], c="blue", s=80, marker="x", zorder=5, label="无人机终点")
-    if target_traj:
-        tn = [p[1] for p in target_traj]
-        te = [p[2] for p in target_traj]
-        ax.plot(te, tn, "r--", linewidth=1.5, label="虚拟目标")
-        ax.scatter(te[0], tn[0], c="red", s=60, marker="o", zorder=5, label="目标起点")
-        ax.scatter(te[-1], tn[-1], c="red", s=80, marker="x", zorder=5, label="目标终点")
-    ax.set_xlabel("East (m)")
-    ax.set_ylabel("North (m)")
-    ax.set_title("水平轨迹 (NED 俯视)")
-    ax.set_aspect("equal", adjustable="datalim")
-    ax.grid(True, linestyle="--", alpha=0.6)
-    ax.legend(loc="best", fontsize=8)
-    ax.xaxis.set_major_locator(MaxNLocator(nbins=6))
-    ax.yaxis.set_major_locator(MaxNLocator(nbins=6))
-
-    # ---- 右图：高度随时间（Down 取负号显示为高度）----
-    ax2 = axes[1]
-    if drone_traj:
-        dt = [p[0] for p in drone_traj]
-        d_alt = [-p[3] for p in drone_traj]  # NED down → 高度
-        ax2.plot(dt, d_alt, "b-", linewidth=1.5, label="无人机高度")
-    if target_traj:
-        tt = [p[0] for p in target_traj]
-        t_alt = [-p[3] for p in target_traj]
-        ax2.plot(tt, t_alt, "r--", linewidth=1.5, label="目标高度")
-    ax2.set_xlabel("时间 (s)")
-    ax2.set_ylabel("高度 (m)")
-    ax2.set_title("高度-时间曲线")
-    ax2.grid(True, linestyle="--", alpha=0.6)
-    ax2.legend(loc="best", fontsize=8)
-
-    fig.suptitle("视觉跟踪段轨迹", fontsize=12)
-    fig.tight_layout()
-
-    if save_path is not None:
-        fig.savefig(str(save_path), dpi=150, bbox_inches="tight")
-        print(f"-- 轨迹图已保存: {save_path}")
-    try:
-        plt.show()
-    except Exception as e:
-        print(f"! 无法弹窗显示图像（无图形界面？）: {e}")
-    finally:
-        plt.close(fig)
-
-
-class VirtualTarget:
-    """几何一致虚拟目标 —— YOLO 检测与双目测距的替身。
-
-    目标初始位置：
-        以起飞后的悬停锚点为参考，
-        按起飞时机头方向放置：
-            前方 TARGET_AHEAD_M
-            右方 TARGET_RIGHT_M
-            下方 TARGET_DOWN_M
-
-    目标运动（水平圆弧）：
-        跟踪开始时刻 t=0，目标以切向速度 MOCK_TARGET_SPEED_MPS、
-        半径 TARGET_ARC_RADIUS_M 在 NED 水平面内做匀速圆周运动。
-
-        圆心：初始位置沿“起飞时右方”偏移半径 R（目标从圆心左侧/右侧
-        起转，初始切向近似沿机头前方），高度保持不变。
-
-        角速度 ω = v / R，方向由 TARGET_ARC_CCW 控制（+1 逆时针）。
-
-    每次 observe() 调用时，根据经过的时间实时更新目标的
-    NED 世界坐标，然后再进行针孔投影。
-
-    相机假定与机体固连、无视差、忽略 roll/pitch；
-    仅使用 yaw 将世界 NED 坐标转换到机体系。
-    """
-
-    def __init__(
-        self,
-        cfg,
-        anchor_ned: tuple[float, float, float],
-        anchor_yaw_rad: float,
-    ) -> None:
-
-        # ============================================================
-        # 1. 根据起飞后的锚点 + 起飞时航向，计算目标初始位置
-        # ============================================================
-
-        cos_y = math.cos(anchor_yaw_rad)
-        sin_y = math.sin(anchor_yaw_rad)
-
-        # 机体系：
-        #   前方 = (cos(yaw), sin(yaw))
-        #   右方 = (-sin(yaw), cos(yaw))
-
-        self._start_n = (
-            anchor_ned[0]
-            + TARGET_AHEAD_M * cos_y
-            - TARGET_RIGHT_M * sin_y
-        )
-
-        self._start_e = (
-            anchor_ned[1]
-            + TARGET_AHEAD_M * sin_y
-            + TARGET_RIGHT_M * cos_y
-        )
-
-        self._start_d = (
-            anchor_ned[2]
-            + TARGET_DOWN_M
-        )
-
-        # ============================================================
-        # 2. 当前目标位置
-        # ============================================================
-
-        self._pos_n = self._start_n
-        self._pos_e = self._start_e
-        self._pos_d = self._start_d
-
-        # ============================================================
-        # 3. 圆弧参数
-        #
-        # 圆心取在初始点沿“起飞时右方”偏移 R 处，使 t=0 时目标
-        # 位于圆心左侧（相对机头），初始切向大致沿机头前方：
-        #
-        #   右方单位向量: (-sin(yaw), cos(yaw))
-        #   圆心 = start + R * 右方
-        #   初始相位：从圆心指向 start 的向量 = -R * 右方
-        # ============================================================
-
-        self._radius_m = max(0.1, float(TARGET_ARC_RADIUS_M))
-        self._omega = (
-            float(MOCK_TARGET_SPEED_MPS) / self._radius_m
-        ) * (1.0 if TARGET_ARC_CCW >= 0 else -1.0)
-
-        right_n = -sin_y
-        right_e = cos_y
-
-        self._center_n = self._start_n + self._radius_m * right_n
-        self._center_e = self._start_e + self._radius_m * right_e
-
-        # 初始角度：圆心 → 起点 在 NE 平面的方位角
-        # atan2(E, N) 与常见航向定义一致
-        self._theta0 = math.atan2(
-            self._start_e - self._center_e,
-            self._start_n - self._center_n,
-        )
-
-        self._move_yaw_rad = anchor_yaw_rad  # 仅作参考/日志
-
-        # ============================================================
-        # 4. 记录目标运动开始时间
-        # ============================================================
-
-        self._start_time = time.monotonic()
-
-        # ============================================================
-        # 5. 相机参数
-        # ============================================================
-
-        cam = cfg.camera
-
-        self._img_w = float(cam.image_width)
-        self._img_h = float(cam.image_height)
-
-        # 针孔模型焦距（像素）：
-        #
-        #     f = W / (2 * tan(FOV_h / 2))
-        #
-        self._f_px = self._img_w / (
-            2.0 * math.tan(
-                math.radians(cam.fov_h_deg) / 2.0
-            )
-        )
-
-        # ============================================================
-        # 6. 目标可见状态
-        # ============================================================
-
-        self._visible = True
-
-    @property
-    def visible(self) -> bool:
-        """返回目标当前是否可见。"""
-        return self._visible
-
-    def set_visible(self, visible: bool) -> None:
-        """设置目标是否可见。"""
-        self._visible = visible
-
-    def update_position(self) -> None:
-        """根据经过的时间更新目标世界坐标（水平匀速圆弧）。
-
-        θ(t) = θ0 + ω * t
-        N = center_N + R * cos(θ)
-        E = center_E + R * sin(θ)
-        D 保持初始高度不变。
-
-        切向速度 v = |ω| * R = MOCK_TARGET_SPEED_MPS。
-        """
-
-        elapsed_s = time.monotonic() - self._start_time
-        elapsed_s = max(0.0, elapsed_s)
-
-        theta = self._theta0 + self._omega * elapsed_s
-
-        self._pos_n = self._center_n + self._radius_m * math.cos(theta)
-        self._pos_e = self._center_e + self._radius_m * math.sin(theta)
-        self._pos_d = self._start_d
-
-    def observe(
-        self,
-        ned_n: float,
-        ned_e: float,
-        ned_d: float,
-        yaw_rad: float,
-    ):
-        """返回：
-
-            (detections, target_distance_m, distance_valid)
-
-        不可见时：
-
-            ([], 0.0, False)
-
-        每次调用都会先更新虚拟目标位置，
-        然后根据无人机当前位置和当前 yaw
-        计算目标在相机画面中的投影。
-        """
-
-        # ============================================================
-        # 1. 始终更新目标真实世界坐标（即使“不可见”也继续运动，
-        #    便于轨迹记录；仅检测输出被屏蔽）
-        # ============================================================
-
-        self.update_position()
-
-        # ============================================================
-        # 2. 目标不可见 → 不返回检测框
-        # ============================================================
-
-        if not self._visible:
-            return [], 0.0, False
-
-        # ============================================================
-        # 3. 目标相对于无人机的 NED 坐标
-        # ============================================================
-
-        rel_n = self._pos_n - ned_n
-        rel_e = self._pos_e - ned_e
-        rel_d = self._pos_d - ned_d
-
-        # ============================================================
-        # 4. 世界坐标 NED → 当前无人机机体系
-        #
-        # 这里只考虑 yaw，不考虑 roll / pitch。
-        #
-        # fwd  ：目标在机头前方的距离
-        # right：目标在机体右侧的距离
-        # ============================================================
-
-        cos_y = math.cos(yaw_rad)
-        sin_y = math.sin(yaw_rad)
-
-        fwd = (
-            rel_n * cos_y
-            + rel_e * sin_y
-        )
-
-        right = (
-            -rel_n * sin_y
-            + rel_e * cos_y
-        )
-
-        # ============================================================
-        # 5. 目标在侧面或后方
-        # ============================================================
-
-        if fwd <= 0.5:
-            return [], 0.0, False
-
-        # ============================================================
-        # 6. 针孔相机投影
-        # ============================================================
-
-        u = (
-            self._img_w * 0.5
-            + self._f_px * (right / fwd)
-        )
-
-        v = (
-            self._img_h * 0.5
-            + self._f_px * (rel_d / fwd)
-        )
-
-        # ============================================================
-        # 7. 目标投影超出画面
-        # ============================================================
-
-        if not (
-            0.0 <= u < self._img_w
-            and
-            0.0 <= v < self._img_h
-        ):
-            return [], 0.0, False
-
-        # ============================================================
-        # 8. 构造模拟 YOLO 检测框
-        # ============================================================
-
-        box = Detection(
-            u - MOCK_BOX_W / 2.0,
-            v - MOCK_BOX_H / 2.0,
-            u + MOCK_BOX_W / 2.0,
-            v + MOCK_BOX_H / 2.0,
-            MOCK_CONFIDENCE,
-        )
-
-        # ============================================================
-        # 9. 模拟双目目标距离（前向分量 Z，与检测框几何一致）
-        # ============================================================
-
-        return [box], fwd, True
-
-
-class TelemetryHub:
-    """后台订阅姿态/位置遥测，控制回路每拍读取最新快照（§7.1 同拍取样）。"""
-
-    def __init__(self) -> None:
-        self.yaw_rad = 0.0
-        self.roll_rad = 0.0
-        self.pitch_rad = 0.0
-        self.attitude_time_ms = 0
-        self.north_m = 0.0
-        self.east_m = 0.0
-        self.down_m = 0.0
-        self._tasks: list[asyncio.Task] = []
-
-    def start(self, drone) -> None:
-        self._tasks.append(asyncio.create_task(self._attitude_loop(drone)))
-        self._tasks.append(asyncio.create_task(self._position_loop(drone)))
-
-    async def _attitude_loop(self, drone) -> None:
-        async for euler in drone.telemetry.attitude_euler():
-            self.yaw_rad = math.radians(euler.yaw_deg)
-            self.roll_rad = math.radians(euler.roll_deg)
-            self.pitch_rad = math.radians(euler.pitch_deg)
-            self.attitude_time_ms = now_ms()
-
-    async def _position_loop(self, drone) -> None:
-        async for pos_vel in drone.telemetry.position_velocity_ned():
-            pos = pos_vel.position
-            self.north_m = pos.north_m
-            self.east_m = pos.east_m
-            self.down_m = pos.down_m
-
-    async def wait_attitude(self, timeout_s: float = 10.0) -> bool:
-        deadline = time.monotonic() + timeout_s
-        while self.attitude_time_ms == 0 and time.monotonic() < deadline:
-            await asyncio.sleep(0.1)
-        return self.attitude_time_ms > 0
-
-    async def stop(self) -> None:
-        for task in self._tasks:
-            task.cancel()
-        for task in self._tasks:
-            try:
-                await task
-            except asyncio.CancelledError:
-                pass
-        self._tasks.clear()
-
-
-async def dispatch(sender, out: ControlOutput, tick_ms: int) -> None:
-    """按模式分发指令。
-
-    DEGRADED_HOLD 在胶水层 demo 中不发送，但真实 offboard 会话中断流
-    （约 0.5~1 s）会触发 PX4 offboard failsafe，故此处改发零速设定点
-    （等效刹停悬停）保住设定点流。
-    """
-    if out.mode is ControlMode.VELOCITY_HEADING and out.valid:
-        await sender.send_velocity_heading(out, tick_ms)
-    else:
-        await sender.send_brake_hover(tick_ms)
-
-
-async def run_tracking(drone, anchor_ned: tuple[float, float, float],
-                       anchor_yaw_deg: float) -> bool:
-    """视觉跟踪闭环段：20 Hz 控制回路，返回是否正常走完场景。"""
-    cfg = load_from_json(str(TRACK_CONFIG_PATH))
-    period_s = 1.0 / cfg.control.frequency_hz
-
-    hub = TelemetryHub()
-    hub.start(drone)
-    if not await hub.wait_attitude():
-        logger.error("姿态遥测未就绪，放弃跟踪段")
-        await hub.stop()
-        return False
-
-    tracker = TargetTracker(TrackerConfig(max_lost_frames=TRACK_MAX_LOST_FRAMES))
-    law = TrackingControlLaw(cfg)
-    gateway = MavsdkCommandGateway(drone)
-    target = VirtualTarget(cfg, anchor_ned, math.radians(anchor_yaw_deg))
-    frame_shape = FrameShape(height=cfg.camera.image_height,
-                             width=cfg.camera.image_width)
-
-    if not await gateway.start():
-        logger.error("offboard 启动失败，放弃跟踪段")
-        await hub.stop()
-        return False
-
-    print(f"-- 进入视觉跟踪：锚点航向 {anchor_yaw_deg:.1f}°，虚拟目标按机头朝向放置于 "
-          f"前 {TARGET_AHEAD_M:.0f} m / 右 {TARGET_RIGHT_M:.0f} m / 下 {TARGET_DOWN_M:.0f} m "
-          f"(世界 NED: N={target._pos_n:.1f}, E={target._pos_e:.1f}, D={target._pos_d:.1f})")
-    print(f"   目标运动: 圆弧 R={TARGET_ARC_RADIUS_M:.1f} m, v={MOCK_TARGET_SPEED_MPS:.1f} m/s, "
-          f"{'逆时针' if TARGET_ARC_CCW >= 0 else '顺时针'} "
-          f"(圆心 N={target._center_n:.1f}, E={target._center_e:.1f})")
-    print(f"   场景: 正常跟踪 {TRACK_NORMAL_SECS:.0f}s → 目标消失 → "
-          f"Coast {TRACK_MAX_LOST_FRAMES / cfg.control.frequency_hz:.0f}s → "
-          f"判丢刹车 {TRACK_BRAKE_SECS:.0f}s")
-
-    started_ms = now_ms()
-    target_gone_ms = started_ms + TRACK_NORMAL_SECS * 1000.0
-    lost_at_ms = None
-    next_deadline = time.monotonic() + period_s
-    tick = 0
-
-    # 轨迹记录：(t_s, north_m, east_m, down_m)
-    drone_traj = []
-    target_traj = []
-
-    try:
-        while True:
-            now = now_ms()
-            if now - started_ms >= TRACK_TIMEOUT_SECS * 1000.0:
-                logger.warning("跟踪段超时（%.0f s），提前收尾", TRACK_TIMEOUT_SECS)
-                break
-
-            if target.visible and now >= target_gone_ms:
-                target.set_visible(False)
-                print("-- 模拟目标消失，追踪器进入 Coast 预测")
-
-            detections, distance_m, distance_valid = target.observe(
-                hub.north_m, hub.east_m, hub.down_m, hub.yaw_rad)
-
-            # 记录本拍位置（目标位置在 observe 内已 update）
-            t_s = (now - started_ms) / 1000.0
-            drone_traj.append((t_s, hub.north_m, hub.east_m, hub.down_m))
-            target_traj.append((t_s, target._pos_n, target._pos_e, target._pos_d))
-
-            result = tracker.update(detections, frame_shape)
-            snap = ControlSnapshot(
-                track=track_result_to_state(result),
-                track_time_ms=now,                 # 每拍取样即打戳（视觉始终新鲜）
-                target_distance_m=distance_m,
-                distance_valid=distance_valid,
-                distance_time_ms=now,
-                attitude_present=True,
-                attitude_time_ms=hub.attitude_time_ms,
-                yaw_rad=hub.yaw_rad,
-                roll_rad=hub.roll_rad,
-                pitch_rad=hub.pitch_rad,
-            )
-            out = law.update(snap, now)
-            await dispatch(gateway, out, now)
-
-            if tick % STATUS_EVERY_TICKS == 0:
-                print(f"[跟踪 t={t_s:5.1f}s] "
-                      f"检测={len(detections)} "
-                      f"追踪[trk={result.tracked} pred={result.is_predicted} "
-                      f"lost={result.lost_frames}] 距离={distance_m:5.1f}m | "
-                      f"[{out.mode.name}] vx_body={out.vx_body_mps:+.2f} "
-                      f"vz={out.vz_mps:+.2f} yaw_rate={out.yaw_rate_dps:+.2f}°/s "
-                      f"coast={out.coast_active}")
-            tick += 1
-
-            if not result.tracked:
-                if lost_at_ms is None:
-                    lost_at_ms = now
-                    print("-- 判丢，执行零速刹车")
-                elif now - lost_at_ms >= TRACK_BRAKE_SECS * 1000.0:
-                    print("-- 判丢刹车完成，退出跟踪段")
-                    break
-
-            next_deadline += period_s
-            sleep_s = next_deadline - time.monotonic()
-            if sleep_s > 0:
-                await asyncio.sleep(sleep_s)
-            else:
-                next_deadline = time.monotonic() + period_s  # 节拍落后，重锚
-    finally:
-        await gateway.stop()
-        await hub.stop()
-
-    # 跟踪段结束后绘制轨迹
-    if PLOT_TRAJECTORY:
-        print(f"-- 绘制轨迹：无人机 {len(drone_traj)} 点，目标 {len(target_traj)} 点")
-        plot_trajectories(drone_traj, target_traj, TRAJECTORY_SAVE_PATH)
-
-    return True
-
-
-async def print_flight_mode(drone):
-    """打印当前飞行模式"""
-    async for flight_mode in drone.telemetry.flight_mode():
-        print(f"[模式] 当前飞行模式: {flight_mode}")
-        return flight_mode
-
-async def wait_until_ready(drone):
-    """等待连接 + 位置估计就绪（各阶段限时 30 s）。"""
-    print("等待无人机连接...")
-    try:
-        async with asyncio.timeout(30.0):
-            async for state in drone.core.connection_state():
-                if state.is_connected:
-                    print("-- 已连接")
-                    break
-    except TimeoutError:
-        raise RuntimeError("30 s 内未建立 MAVSDK 连接（检查 SITL 是否在运行、"
-                           "14540 端口是否被占用）") from None
-
-    print("等待全球位置估计就绪...")
-    try:
-        async with asyncio.timeout(30.0):
-            async for health in drone.telemetry.health():
-                if health.is_global_position_ok and health.is_home_position_ok:
-                    print("-- 位置估计 OK")
-                    break
-    except TimeoutError:
-        raise RuntimeError("30 s 内位置估计未就绪") from None
-
-async def run():
-    drone = System()
-    await drone.connect(system_address=CONNECTION)
-
-    # 1. 等待就绪
-    try:
-        await wait_until_ready(drone)
-    except RuntimeError as e:
-        print(f"! {e}")
-        return
-
-    # 1.5 显式设定遥测流速率（不依赖默认配置；失败则沿用默认并告警）
-    for stream, rate in (("position", 50.0),
-                         ("position_velocity_ned", 30.0),
-                         ("attitude_euler", 50.0),
-                         ("in_air", 10.0)):
-        try:
-            await getattr(drone.telemetry, f"set_rate_{stream}")(rate)
-        except Exception as e:
-            print(f"! 设置 {stream} 速率失败（沿用默认值）: {e}")
-    print("-- 遥测速率已设定")
-
-    # 记录初始模式（可选，用于最后恢复）
-    initial_mode = await print_flight_mode(drone)
-
-    # 2. 解锁
-    print("-- 解锁")
-    try:
-        await drone.action.arm()
-    except ActionError as e:
-        print(f"解锁失败: {e}")
-        return
-
-    # 3. 起飞
-    print(f"-- 起飞到 {TAKEOFF_ALTITUDE} m")
-    await drone.action.set_takeoff_altitude(TAKEOFF_ALTITUDE)
-    await drone.action.takeoff()
-
-    # 等待起飞完成。注意：MAVSDK 遥测是带缓冲队列的推送流，循环体内
-    # 不能 sleep —— 否则消费速率（如 2 Hz）远低于推送速率（50 Hz），
-    # 队列积压导致读到的是几十秒前的旧高度（问题一的根因）。全速排水
-    # 使每条消息都是最新值，另加整体超时兜底。
-    print("-- 等待起飞完成...")
-    wait_start = time.monotonic()
-    next_log = wait_start + 2.0
-    reached = False
-    try:
-        async with asyncio.timeout(30.0):
-            async for position in drone.telemetry.position():
-                if position.relative_altitude_m >= TAKEOFF_ALTITUDE * 0.90:
-                    wait_s = time.monotonic() - wait_start
-                    print(f"-- 已到达起飞高度: {position.relative_altitude_m:.2f} m "
-                          f"(等待 {wait_s:.1f} s)")
-                    reached = True
-                    break
-                now_t = time.monotonic()
-                if now_t >= next_log:
-                    print(f"   起飞爬升中: {position.relative_altitude_m:.2f} m")
-                    next_log = now_t + 2.0
-    except TimeoutError:
-        pass
-    if not reached:
-        print("! 30 s 内未达到起飞高度，放弃本次测试（执行降落兜底）")
-        try:
-            await drone.action.land()
-        except ActionError as e:
-            print(f"! 降落指令失败: {e}")
-        return
-
-    await asyncio.sleep(1)
-
-    # 4. 记录悬停锚点与锚点航向（虚拟目标按机头朝向放置）
-    try:
-        async with asyncio.timeout(10.0):
-            async for pos_vel in drone.telemetry.position_velocity_ned():
-                anchor_ned = (pos_vel.position.north_m,
-                              pos_vel.position.east_m,
-                              pos_vel.position.down_m)
-                break
-        anchor_yaw_deg = 0.0
-        async with asyncio.timeout(10.0):
-            async for euler in drone.telemetry.attitude_euler():
-                anchor_yaw_deg = euler.yaw_deg
-                break
-    except TimeoutError:
-        print("! 10 s 内未取得悬停锚点/航向，放弃本次测试（执行降落兜底）")
-        try:
-            await drone.action.land()
-        except ActionError as e:
-            print(f"! 降落指令失败: {e}")
-        return
-    print(f"   悬停锚点 NED: N={anchor_ned[0]:.2f}, "
-          f"E={anchor_ned[1]:.2f}, D={anchor_ned[2]:.2f}，"
-          f"机头 {anchor_yaw_deg:.1f}°")
-
-    # 5. 视觉跟踪闭环（offboard 速度设定点，20 Hz）
-    await run_tracking(drone, anchor_ned, anchor_yaw_deg)
-
-    # 6. 退出 offboard 后回到 Hold，稍候确认模式
-    await asyncio.sleep(1)
-    await print_flight_mode(drone)
-
-    # 7. 返航 (RTL)
-    print("-- 执行 RTL")
-    await drone.action.return_to_launch()
-
-    # 8. 等待落地（同样全速排水，避免 in_air 队列积压读到旧状态）
-    print("-- 等待落地...")
-    try:
-        async with asyncio.timeout(240.0):
-            async for is_in_air in drone.telemetry.in_air():
-                if not is_in_air:
-                    print("-- 已落地")
-                    break
-    except TimeoutError:
-        print("! 240 s 内未检测到落地")
-
-    await asyncio.sleep(2)
-
-    # 9. 上锁
-    print("-- 上锁")
-    try:
-        await drone.action.disarm()
-        print("-- 上锁成功")
-    except ActionError as e:
-        print(f"上锁失败（可能已自动上锁）: {e}")
-
-    print(f"-- 初始模式是: {initial_mode}")
-    print("\n===== 测试流程全部完成 =====")
-
-if __name__ == "__main__":
-    logging.basicConfig(level=logging.INFO,
-                        format="%(asctime)s.%(msecs)03d %(levelname)s %(message)s",
-                        datefmt="%H:%M:%S")
-    asyncio.run(run())

tools/README.md

@@ -0,0 +1,23 @@
+# tools/ —— 临时/台架小工具（不参与运行链路）
+
+这里放的是**一次性或台架用**的脚本，不被 `pod_bridge` / `task_runner` 引用，
+也不在 pytest 收集范围内。移动进来是为了让仓库根目录只保留现行入口。
+
+| 文件 | 用途 | 依赖 | 输出 |
+|---|---|---|---|
+| `camera.py` | 双目相机棋盘格采集 + 标定 | `opencv-python`、`numpy` | `stereo_calib_images/`（图像）、`stereo_calib_params.npz`（内参/外参） |
+| `camera_test.py` | 单相机取流冒烟测试（索引/分辨率/帧率） | `opencv-python` | 无（窗口预览） |
+| `video_test.py` | HDMI 全屏预览（第二屏） | `opencv-python`、`numpy` | 无（窗口预览） |
+
+## 注意
+
+- 这些脚本会在**当前工作目录**下产出文件（`stereo_calib_images/`、`stereo_calib_params.npz`）。
+  建议在 `tools/` 内运行，避免污染仓库根：
+  ```bash
+  cd tools
+  "D:/program file/conda/envs/pytorch/python.exe" camera.py
+  ```
+- 标定结果现归档在 `legacy/stereo_calib_params.npz`（对应已归档的双目链路）。
+  当前运行链路**不需要**这些参数（吊舱不提供距离、双目未接入）。
+- 仓库根原有的 `camera.py` / `camera_test.py` / `video_test.py` 是同一批脚本，
+  2026-10-20 移入本目录，内容未改。

tools/camera.py

@@ -0,0 +1,182 @@
+import cv2
+import numpy as np
+import glob
+import os
+from pathlib import Path
+
+# ==================== 配置参数 ====================
+CHESSBOARD_SIZE = (8, 11)  # 内角点数量 (列, 行)
+SQUARE_SIZE = 20.0  # 每个方格实际边长（毫米），请改成你的真实尺寸
+
+CAMERA_INDEX = 1
+SAVE_DIR = "stereo_calib_images"  # 保存标定图片的文件夹
+os.makedirs(SAVE_DIR, exist_ok=True)
+
+# 标定用分辨率（每个单目）
+CALIB_WIDTH = 1280
+CALIB_HEIGHT = 720
+
+
+# =================================================
+
+def capture_images():
+    """采集标定图片（左右目同步保存）"""
+    cap = cv2.VideoCapture(CAMERA_INDEX, cv2.CAP_DSHOW)
+    if not cap.isOpened():
+        cap = cv2.VideoCapture(CAMERA_INDEX, cv2.CAP_MSMF)
+
+    cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*'MJPG'))
+    cap.set(cv2.CAP_PROP_FRAME_WIDTH, 2560)
+    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)
+
+    print("按空格键保存当前帧，按 q 退出采集")
+    count = 0
+
+    while True:
+        ret, frame = cap.read()
+        if not ret:
+            break
+
+        left = frame[:, :1280].copy()
+        right = frame[:, 1280:].copy()
+
+        gray_l = cv2.cvtColor(left, cv2.COLOR_BGR2GRAY)
+        gray_r = cv2.cvtColor(right, cv2.COLOR_BGR2GRAY)
+
+        ret_l, corners_l = cv2.findChessboardCorners(gray_l, CHESSBOARD_SIZE, None)
+        ret_r, corners_r = cv2.findChessboardCorners(gray_r, CHESSBOARD_SIZE, None)
+
+        # 用于显示的图像（带角点）
+        display_left = left.copy()
+        display_right = right.copy()
+
+        if ret_l:
+            cv2.drawChessboardCorners(display_left, CHESSBOARD_SIZE, corners_l, ret_l)
+        if ret_r:
+            cv2.drawChessboardCorners(display_right, CHESSBOARD_SIZE, corners_r, ret_r)
+
+        display = np.hstack([display_left, display_right])
+        cv2.putText(display, f"Saved: {count}  |  Space: Save  Q: Quit", (20, 40),
+                    cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
+        cv2.imshow("Stereo Capture (Left | Right)", display)
+
+        key = cv2.waitKey(1) & 0xFF
+        if key == ord(' '):  # 空格保存
+            if ret_l and ret_r:
+                # 1. 保存干净的原图（用于正式标定）
+                cv2.imwrite(f"{SAVE_DIR}/left_{count:02d}.png", left)
+                cv2.imwrite(f"{SAVE_DIR}/right_{count:02d}.png", right)
+
+                # 2. 同时保存带检测结果的可视化图（方便检查）
+                cv2.imwrite(f"{SAVE_DIR}/left_{count:02d}_detect.png", display_left)
+                cv2.imwrite(f"{SAVE_DIR}/right_{count:02d}_detect.png", display_right)
+
+                print(f"已保存第 {count} 组（含检测结果图）")
+                count += 1
+            else:
+                print("未同时检测到棋盘格，未保存")
+        elif key == ord('q'):
+            break
+
+    cap.release()
+    cv2.destroyAllWindows()
+    print(f"共采集 {count} 组图片")
+
+
+def calibrate_stereo():
+    """执行双目标定"""
+    # 准备物体点（世界坐标）
+    objp = np.zeros((CHESSBOARD_SIZE[0] * CHESSBOARD_SIZE[1], 3), np.float32)
+    objp[:, :2] = np.mgrid[0:CHESSBOARD_SIZE[0], 0:CHESSBOARD_SIZE[1]].T.reshape(-1, 2)
+    objp *= SQUARE_SIZE
+
+    objpoints = []  # 3D点
+    imgpoints_l = []  # 左目2D点
+    imgpoints_r = []  # 右目2D点
+
+    left_images = sorted(glob.glob(f"{SAVE_DIR}/left_*.png"))
+    right_images = sorted(glob.glob(f"{SAVE_DIR}/right_*.png"))
+
+    if len(left_images) < 10:
+        print("标定图片太少（建议 ≥15 组），请重新采集")
+        return
+
+    print(f"开始处理 {len(left_images)} 组图片...")
+
+    for left_path, right_path in zip(left_images, right_images):
+        img_l = cv2.imread(left_path)
+        img_r = cv2.imread(right_path)
+
+        gray_l = cv2.cvtColor(img_l, cv2.COLOR_BGR2GRAY)
+        gray_r = cv2.cvtColor(img_r, cv2.COLOR_BGR2GRAY)
+
+        ret_l, corners_l = cv2.findChessboardCorners(gray_l, CHESSBOARD_SIZE, None)
+        ret_r, corners_r = cv2.findChessboardCorners(gray_r, CHESSBOARD_SIZE, None)
+
+        if ret_l and ret_r:
+            # 亚像素精化
+            criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
+            corners_l = cv2.cornerSubPix(gray_l, corners_l, (11, 11), (-1, -1), criteria)
+            corners_r = cv2.cornerSubPix(gray_r, corners_r, (11, 11), (-1, -1), criteria)
+
+            objpoints.append(objp)
+            imgpoints_l.append(corners_l)
+            imgpoints_r.append(corners_r)
+            print(f"✓ {Path(left_path).name}")
+        else:
+            print(f"✗ 跳过 {Path(left_path).name}")
+
+    if len(objpoints) < 10:
+        print("有效图片不足，标定失败")
+        return
+
+    # 单目标定（可选，用来给 stereoCalibrate 提供初始值）
+    ret_l, mtx_l, dist_l, rvecs_l, tvecs_l = cv2.calibrateCamera(
+        objpoints, imgpoints_l, (CALIB_WIDTH, CALIB_HEIGHT), None, None)
+    ret_r, mtx_r, dist_r, rvecs_r, tvecs_r = cv2.calibrateCamera(
+        objpoints, imgpoints_r, (CALIB_WIDTH, CALIB_HEIGHT), None, None)
+
+    print("\n左目重投影误差:", ret_l)
+    print("右目重投影误差:", ret_r)
+
+    # 双目标定
+    flags = cv2.CALIB_FIX_INTRINSIC  # 使用上面得到的内参
+    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 1e-5)
+
+    ret, mtx_l, dist_l, mtx_r, dist_r, R, T, E, F = cv2.stereoCalibrate(
+        objpoints, imgpoints_l, imgpoints_r,
+        mtx_l, dist_l, mtx_r, dist_r,
+        (CALIB_WIDTH, CALIB_HEIGHT),
+        criteria=criteria,
+        flags=flags
+    )
+
+    print("\n双目标定完成！")
+    print("重投影误差:", ret)
+    print("旋转矩阵 R:\n", R)
+    print("平移向量 T:\n", T)
+
+    # 计算立体校正参数
+    R1, R2, P1, P2, Q, roi_l, roi_r = cv2.stereoRectify(
+        mtx_l, dist_l, mtx_r, dist_r,
+        (CALIB_WIDTH, CALIB_HEIGHT), R, T,
+        alpha=0  # 0=只保留有效区域，1=保留所有像素
+    )
+
+    # 保存所有结果
+    np.savez("stereo_calib_params.npz",
+             mtx_l=mtx_l, dist_l=dist_l,
+             mtx_r=mtx_r, dist_r=dist_r,
+             R=R, T=T, E=E, F=F,
+             R1=R1, R2=R2, P1=P1, P2=P2, Q=Q)
+
+    print("\n标定参数已保存到 stereo_calib_params.npz")
+    return mtx_l, dist_l, mtx_r, dist_r, R, T, Q
+
+
+if __name__ == "__main__":
+    print("1. 先采集标定图片")
+    capture_images()
+
+    print("\n2. 开始标定")
+    calibrate_stereo()

tools/camera_test.py

@@ -0,0 +1,59 @@
+import cv2
+
+# ==================== 配置区 ====================
+CAMERA_INDEX = 1          # 相机索引，如果不对请用前面的探测代码找
+FRAME_WIDTH  = 2560
+FRAME_HEIGHT = 720
+TARGET_FPS   = 30
+# =================================================
+
+def decode_fourcc(val):
+    """把 get(CAP_PROP_FOURCC) 返回的浮点数转成可读的四字符代码"""
+    val = int(val)
+    return "".join([chr((val >> 8 * i) & 0xFF) for i in range(4)])
+
+# 1. 打开相机，Windows 上指定 DirectShow 后端
+cap = cv2.VideoCapture(CAMERA_INDEX, cv2.CAP_DSHOW)
+
+if not cap.isOpened():
+    print(f"无法打开相机，索引 {CAMERA_INDEX}")
+    exit()
+
+# 2. 设置分辨率（在设置 MJPG 之前或之后都可以，但建议先设格式）
+cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'))
+cap.set(cv2.CAP_PROP_FRAME_WIDTH,  FRAME_WIDTH)
+cap.set(cv2.CAP_PROP_FRAME_HEIGHT, FRAME_HEIGHT)
+cap.set(cv2.CAP_PROP_FPS,          TARGET_FPS)
+
+# 3. 读取一次，确认参数是否真的生效
+ret, frame = cap.read()
+if not ret:
+    print("相机已打开，但无法读取帧（可能分辨率不支持）")
+    cap.release()
+    exit()
+
+# 4. 打印实际生效的参数和帧格式
+print("===== 相机实际参数 =====")
+print(f"分辨率       : {int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))} x {int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))}")
+print(f"帧率         : {cap.get(cv2.CAP_PROP_FPS):.1f}")
+print(f"FOURCC       : {decode_fourcc(cap.get(cv2.CAP_PROP_FOURCC))}")
+print(f"帧 dtype     : {frame.dtype}")
+print(f"帧 shape     : {frame.shape}   # 应为 (H, W, 3)")
+print("========================")
+
+# 5. 实时显示循环
+while True:
+    ret, frame = cap.read()
+    if not ret:
+        print("读取帧失败，退出")
+        break
+
+    cv2.imshow("USB Camera", frame)
+
+    # 按 'q' 退出
+    if cv2.waitKey(1) & 0xFF == ord('q'):
+        break
+
+# 6. 释放资源
+cap.release()
+cv2.destroyAllWindows()

tools/video_test.py

@@ -0,0 +1,42 @@
+import cv2
+import numpy as np
+
+# 摄像头
+cap = cv2.VideoCapture(0)
+if not cap.isOpened():
+    print("无法打开摄像头")
+    exit()
+
+# 目标输出分辨率（匹配你的小屏+转换器）
+OUT_WIDTH = 720
+OUT_HEIGHT = 480
+
+# 创建窗口并设为可调整
+cv2.namedWindow("HDMI_Output", cv2.WINDOW_NORMAL)
+
+# 关键：把窗口移到第二块屏幕（HDMI）的位置
+# 假设主屏幕是 1920 宽，第二块屏幕从 x=1920 开始
+# 请根据你实际的主屏幕宽度修改这个值！
+SECOND_SCREEN_X = 1920   # ← 改成你主屏幕的宽度
+SECOND_SCREEN_Y = 0
+
+cv2.moveWindow("HDMI_Output", SECOND_SCREEN_X, SECOND_SCREEN_Y)
+cv2.setWindowProperty("HDMI_Output", cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN)
+
+while True:
+    ret, frame = cap.read()
+    if not ret:
+        break
+
+    # 缩放到目标分辨率
+    frame = cv2.resize(frame, (OUT_WIDTH, OUT_HEIGHT))
+
+    # 显示
+    cv2.imshow("HDMI_Output", frame)
+
+    # 按 q 退出
+    if cv2.waitKey(1) & 0xFF == ord('q'):
+        break
+
+cap.release()
+cv2.destroyAllWindows()

utils/README.md

@@ -1,51 +1,57 @@
-# utils —— reference-c++ 的 Python 移植 + 双目链路新实现
+# utils —— Python 实现（运行链路）
 
-将 `reference-c++/` 下的 `target_tracker`（多目标追踪过滤器）与
+> **架构现状（2026-10-20）**：运行链路是「光电吊舱 → `pod_bridge` → `task_runner` → PX4」。
-`visual_track_control`（视觉跟踪控制律）移植为 Python，**两包互不依赖**
+> 双目 + YOLO 那套可选感知后端（`stereo_yolo` / `target_tracker` / `stereo_ranger`）
-（唯一的数据转换点在胶水层 demo）。
+> **已整体归档到仓库根的 `legacy/`**，默认不参与 pytest 收集；接回方式见 `legacy/README.md`。
 
-另有双目链路新实现（非 C++ 移植、按规格实现）：
+本目录包含：
-- `stereo_ranger`（双目稀疏目标测距），规格 `docs/双目测距技术路线.md`；
-- `stereo_yolo`（双目 YOLO 检测前端），规格 `docs/双目YOLO目标识别需求说明.md`，
-  输出直接作为 stereo_ranger 的输入（仅复用其 `types.py` 纯值类型）。
 
-Python 侧定位为 **C++ 实现的先验验证**：构件划分与语义按规格冻结，便于逐构件
+- **运行链路**：`pod_bridge`（吊舱桥接）+ `task_runner`（任务编排）+ `visual_track_control`
-移植对照；`stereo_yolo` 的 C++ 移植注记见其包 README。
+  （控制律原语，被前两者复用）
+- **入口共享件**：`sitl_support.py`（时钟/遥测订阅/连接准备/`RATE_*`）
+- **测试**：`tests/`（446 条）、`conftest.py`、`pytest.ini`
+
+`visual_track_control` 是 `reference-c++/visual_track_control` 的逐构件 Python 移植
+（语义对齐、含行为修复）；`target_tracker` 的移植版已随归档移入 `legacy/utils/`。
 
 ## 目录结构
 
 ```
 utils/
-├── target_tracker/            # 包 1：卡尔曼滤波多目标追踪（无第三方硬依赖，仅 numpy）
+├── pod_bridge/                # 吊舱 ↔ PX4 桥接（**运行链路核心**）
+│   ├── bridge.py              #   PodBridge：L1/L2 切换、目标门控、链路维持
+│   ├── los_consistency.py     #   目标有效性第二判据（LOS 运动学一致性）
+│   ├── ground_link.py         #   地面站 UDP 坐标注入 + 命令队列
+│   ├── guidance.py            #   L2 地理引导律
+│   ├── protocol.py            #   表2-1/2-2/3/4 帧编解码
+│   ├── pod_serial.py          #   串口链路（115200 8N1）
+│   ├── px4_gateway.py         #   MAVSDK 遥测 + offboard 网关
+│   ├── 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         #   吊舱识别/锁定指令
+│   └── log.py                 #   事件 JSONL + 遥测 CSV + 简报
+├── visual_track_control/      # 控制律 + MAVSDK 网关（运行链路复用）
 │   └── README.md              # 接口清单 + C++ 差异
-├── visual_track_control/      # 包 2：三通道控制律 + MAVSDK offboard 网关
+├── config/                    # 配置 JSON（pod_bridge.json 为主线）
-│   └── README.md              # 接口清单 + C++ 差异
+├── tests/                     # pytest（446 条）
-├── stereo_ranger/             # 包 3：双目稀疏目标测距（纯标准库，无第三方依赖）
+│   ├── pod_bridge/            #   208 条（协议/桥接/引导/注入/节流/LOS）
-│   └── README.md              # 接口清单 + 与规格的偏差
+│   ├── task_runner/           #   122 条（状态机/编排器/返航/日志/吊舱控制）
-├── stereo_yolo/               # 包 4：双目 YOLO 检测前端（真模型需 ultralytics）
+│   ├── visual_track_control/  #   86 条（对应 C++ gtest + L1 增量）
-│   └── README.md              # 接口清单 + C++ 移植注记
+│   └── ground_station/        #   30 条（页面与日志解读）
-├── demo_track_control.py      # 胶水层集成演示（模拟检测 → 追踪 → 控制律 → 发送）
+├── conftest.py                # 把 utils/ 插进 sys.path + fake_mavsdk fixture
-├── demo_stereo_ranger.py      # 双目测距演示（合成框对，六场景，无硬件）
+├── pytest.ini                 # 收集 *_test.py + importlib 导入模式
-├── demo_stereo_yolo.py        # 检测前端演示（真模型 + 合成素材 → 测距联调）
+└── sitl_support.py            # 入口共用：时钟/遥测订阅/连接准备/速率常量
-├── assets/stereo_yolo/        # demo 占位素材（AI 合成图，可整体替换为真实素材）
-├── config/track_control.json  # 控制律配置（拷贝自 reference-c++）
-├── config/stereo_ranger.json  # 双目测距配置（占位值，待标定回填）
-├── config/stereo_yolo.json    # 检测前端配置（不存内参，几何参数唯一来源见上）
-├── tests/                     # pytest 测试
-│   ├── target_tracker/        # 28 条（对应 C++ 测试文件）
-│   ├── visual_track_control/  # 81 条（对应 C++ 测试文件）
-│   ├── stereo_ranger/         # 111 条（规格 §11.4 构件级 + 全流程）
-│   └── stereo_yolo/           # 36 条（假检测器驱动，无模型/素材/硬件）
-├── conftest.py                # 保证可从任意工作目录导入各包
-└── pytest.ini                 # 收集 *_test.py 命名（与 C++ 测试文件名对齐）；
-                               # importlib 导入模式避免跨包同名测试文件冲突
 ```
 
 ## 环境
 
 conda env `pytorch`（`D:\program file\conda\envs\pytorch`），Python 3.12，
-依赖：numpy、mavsdk、pytest；stereo_ranger 仅标准库；stereo_yolo 只有真模型
+依赖：numpy、mavsdk(<4)、pytest、flask、pyserial
-推理路径需要 ultralytics（在检测器内部延迟导入，测试与假检测器路径不受影响）。
+（`opencv-python` 仅供 `tools/` 下的相机脚本；`ultralytics` 仅供已归档的双目链路）。
 
 ## 运行测试
 
@@ -54,51 +60,46 @@ cd utils
 "D:/program file/conda/envs/pytorch/python.exe" -m pytest -q
 ```
 
-256 条测试全部通过：109 条 C++ 移植用例（含 §9.4 六条硬验收、A1/A2/A4
+**运行链路 446 条全部通过（2026-10-20）**：
-回归、PID anti-windup C1、超龄 C4、首拍限幅 S6）+ 111 条 stereo_ranger
-（合成框对驱动）+ 36 条 stereo_yolo（假检测器驱动）。
-stereo_ranger / stereo_yolo 测试均不需要真实图像、模型权重或硬件。
 
-## 运行演示
+| 组 | 条数 |
+|---|---|
+| `pod_bridge` | 208 |
+| `task_runner` | 122 |
+| `visual_track_control`（含 §9.4 硬验收、A1/A2/A4、C1、C4、S6、L1 角度直入） | 86 |
+| `ground_station` | 30 |
+
+归档的可选后端 175 条（stereo_ranger 111 / stereo_yolo 36 / target_tracker 28）
+在 `legacy/` 下，按需运行：
 
 ```bash
-# mock 传输（本地记录指令，无实机）
+cd legacy/utils && PYTHONPATH="../../utils" "D:/program file/conda/envs/pytorch/python.exe" -m pytest -q
-python utils/demo_track_control.py --speedup 10
-
-# MAVSDK 实发（需飞控/SITL 在 udp://:14540）
-python utils/demo_track_control.py --transport mavsdk
-
-# 双目测距（合成框对，离线，无相机/飞控）
-python utils/demo_stereo_ranger.py
-
-# 双目检测前端（真模型推理；需 ultralytics + models/yolo26n-drone.pt）
-python utils/demo_stereo_yolo.py
 ```
 
-控制链路时间线 13 s（20 Hz）：正常跟踪 6 s（vx 从零渐变 → 逼近限速 3 m/s）→
+运行链路测试不需要硬件（FakePod / VirtualPod / FakePx4 注入）。
-目标消失 4 s（Coast 预测，60 帧后轨道判亡）→ 判零速刹车 3 s。
 
-双目测距演示六场景：逼近（三道有效性门 + 滤波）→ 远场域外（方位仍发布）→
+## 运行入口
-单目退化 → 非同帧拒收 → 一致性降级/判无效 → 多目标贪心配对。
 
-检测前端演示五场景：单图冒烟 → 合成双目对联调（真检测框喂 stereo_ranger，
+入口脚本在仓库根（不在 utils 内）：
-与解析式 D=F·B/Z 对照）→ 空场景 → 左右形状不一致拒收 → 帧周期计时。
+
-合成对用 AI 占位素材 `assets/stereo_yolo/drone_sky.png`（可整体替换为真实
+```bash
-素材），本机实测 |ΔD|≈0.3 px、|Δy|≈0.2 px；帧周期 25~34 ms（640 输入，
+PY="D:/program file/conda/envs/pytorch/python.exe"
-双实例并发，随机器负载波动），与 30 fps 预算 33.3 ms 同量级，结论不可外推
+"$PY" task_runner_sitl.py --conn udpin://0.0.0.0:14540      # 任务全流程 SITL
-到机载平台（§5 的算力预算另见平台工程文档）。
+"$PY" task_runner_real.py --port COM5 --conn udpin://0.0.0.0:14540 --target-type 1
+"$PY" l1_sitl_test.py --conn udpin://0.0.0.0:14540          # 仅 L1 控制律闭环
+"$PY" utils/pod_bridge/run_bridge.py --port COM5 --dry-run  # S2 台架干跑
+```
 
 ## 移植约定
 
-- MAVLink 部分改为 **MAVSDK 语义映射**（offboard API，非字节透传），三分支
+- MAVLink 部分改为 **MAVSDK 语义映射**（offboard API，非字节透传），三分支映射保留
-  映射保留 C++ A2 回归语义，详见包 2 README。
+  C++ A2 回归语义，详见 `visual_track_control/README.md`。
 - 测试逐条移植 C++ gtest 断言（`EXPECT_DOUBLE_EQ` → 精确 `==`，
-  `EXPECT_NEAR` → `pytest.approx`）；MAVSDK 部分因无 SITL/实机，仅 mock 验证。
+  `EXPECT_NEAR` → `pytest.approx`）。
 - 行为修复项（C1/C2/C4、A1/A2/A4、S6）在测试文件头注释中标注对应章节。
-- `stereo_ranger` 不是移植：按规格五步流程新实现，与规格的偏差（越界=有效性门、
+- ⚠ 全部数值为占位值，标定完成前不得作为飞行控制决策依据。
-  视差域权威、补充 consistency_jump_px/isolation_gate_px 等）见其包 README；
+
-  ⚠ 全部数值为占位值，标定完成前不得作为飞行控制决策依据。
+## 已归档内容
-- `stereo_yolo` 不是移植：推理口径为 ultralytics 直跑 `.pt`，letterbox、解码、
+
-  NMS、坐标回映均在库内完成；C++ 侧复现要点与接口契约硬约束（原生分辨率坐标、
+`legacy/utils/` 下保留了原 `stereo_yolo` / `stereo_ranger` / `target_tracker` 三包、
-  左右预处理位级一致、`x1` 即框左边缘、无跨帧状态）见其包 README 的移植注记表。
+175 条测试、三个 demo、`stereo_*.json`、素材与 YOLO 权重；说明见 `legacy/README.md`。
-  ⚠ 占位素材 `assets/stereo_yolo/` 为 AI 合成图，真实双目素材到位后需重跑联调。

utils/config/pod_bridge.json

@@ -39,7 +39,10 @@
     "alpha_from_frame": false,
     "alpha_from_off_target": true,
     "vertical_enabled": true,
-    "require_vision": false
+    "los_consistency_enabled": true,
+    "los_window_ms": 3000,
+    "los_min_yaw_change_deg": 20.0,
+    "los_max_residual_deg": 8.0
   },
 
   "tracking": {

utils/pod_bridge/bridge.py

@@ -33,6 +33,8 @@ from visual_track_control.types import (ControlMode, ControlSnapshot,
 from .config import BridgeConfig
 from .ground_link import GroundTarget
 from .guidance import GuidanceInput, GuidanceLaw
+from .los_consistency import (LosConsistencyConfig, LosConsistencyMonitor,
+                              LosStatus)
 from .pod_serial import PodLink, PodSerial
 from .protocol import (CarrierAttitude, ControlCommand, TargetGeo, Telemetry,
                        encode_attitude, encode_control)
@@ -49,23 +51,39 @@ class PodBridge:
     """px4：Px4Gateway（或等价 mock）；pod：PodLink（PodSerial 或内存 FakePod）。"""
 
     def __init__(self, cfg: BridgeConfig, px4: Px4Gateway, pod: PodLink,
-                 now_ms_fn=_monotonic_ms, vision_target_fn=None,
+                 now_ms_fn=_monotonic_ms,
                  ground_target_fn=None) -> None:
         self._cfg = cfg
         self._px4 = px4
         self._pod = pod
         self._now_ms = now_ms_fn
-        self._vision_target_fn = vision_target_fn  # §2.3 第二判据（可选，S3+ 接入我方视觉）
         self._ground_target_fn = ground_target_fn  # 地面注入坐标访问器（A2，无激光替代表4）
         self._use_heading_servo = cfg.heading_servo.enabled  # 运行时控制律选择（A3 状态机切换）
         self._guidance = GuidanceLaw(cfg.guidance)
         self._tracking = TrackingControlLaw(cfg.tracking)  # L1 航向伺服（方案 §0）
+        # 目标有效性第二判据：LOS 运动学一致性（取代已废弃的 require_vision）
+        hs = cfg.heading_servo
+        self._los_monitor = LosConsistencyMonitor(LosConsistencyConfig(
+            enabled=hs.los_consistency_enabled,
+            window_ms=hs.los_window_ms,
+            min_yaw_change_deg=hs.los_min_yaw_change_deg,
+            max_residual_deg=hs.los_max_residual_deg))
+        self._los_consistent = True       # 最近一次一致性判定（公共门控用）
+        self._los_logged_ok = True        # 日志：只在判定翻转时打印
         self._attitude_period_ms = max(
             1, int(round(1000.0 / cfg.link.attitude_downlink_hz)))
         self._last_attitude_ms: Optional[int] = None
+        # 生效律周期：step() 被外部以更高频率调用时（任务编排器统一 20Hz，而
+        # L2 引导律配置为 10Hz），按此节流，避免 dt 相关项/限幅被以双倍速率推进。
+        self._law_period_ms = max(
+            1, int(round(1000.0 / max(1e-6, self._active_frequency_hz()))))
+        self._last_law_ms: Optional[int] = None
+        self._last_law_output = None    # 节流期内重发的上一拍输出
         self._latest_target = None      # (TargetGeo, time_ms)
         self._latest_telemetry = None   # (Telemetry, time_ms)
         self._step_failures = 0         # run() 连续异常计数（告警节流用）
+        self._stopped = False           # stop() 幂等标志（真机实测：RTL 与收尾会各调一次）
+        self._landing_stopped = False   # 是否已为降落退出 offboard（遥测仍保留）
 
     # --- 生命周期 -----------------------------------------------------------
     async def start(self) -> bool:
@@ -74,13 +92,22 @@ class PodBridge:
         if not await self._px4.start_offboard():
             logger.error("[Bridge] offboard 启动失败，桥接未运行")
             return False
+        self._stopped = False
         logger.info("[Bridge] 启动：引导 %.1fHz，姿态下发 %.1fHz",
                     self._cfg.guidance.frequency_hz,
                     self._cfg.link.attitude_downlink_hz)
         return True
 
     async def stop(self) -> None:
-        """安全停机：先零速刹车，再停 offboard / 遥测 / 串口。"""
+        """安全停机：先零速刹车，再停 offboard / 遥测 / 串口。
+
+        幂等：任务编排器在进入 RTL 时停一次，收尾 `_teardown` 还会再停一次
+        （真机在环实测确认）。重复执行会二次关闭串口、二次停 offboard，
+        在 MAVSDK/串口驱动上产生无意义告警，故第二次直接返回。
+        """
+        if self._stopped:
+            return
+        self._stopped = True
         try:
             await self._px4.send_brake_hover(self._now_ms())
         finally:
@@ -91,6 +118,20 @@ class PodBridge:
             self._tracking.reset()
             logger.info("[Bridge] 已停止")
 
+    async def landing_stop(self) -> None:
+        """降落前停机：零速刹车 + 退出 offboard，但**保留遥测与串口**。
+
+        与 `stop()` 的差别：交给飞控 `land()` 之后仍需遥测来判定落地、记录高度
+        曲线。若此时就 `stop_telemetry()`，落地段的遥测会冻结在最后高度（SITL
+        实测：日志停在 5m，看不到下降）。完整清理留给收尾 `stop()`。
+        """
+        try:
+            await self._px4.send_brake_hover(self._now_ms())
+        finally:
+            await self._px4.stop_offboard()
+        self._landing_stopped = True
+        logger.info("[Bridge] 已退出 offboard（保留遥测用于落地判定）")
+
     # --- 主循环 -------------------------------------------------------------
     async def run(self, stop_event: asyncio.Event) -> None:
         """按引导频率循环 step()，直到 stop_event 置位。
@@ -133,8 +174,16 @@ class PodBridge:
         self._use_heading_servo = enabled
         if enabled:
             self._tracking.reset()
+            # 进入 L1：LOS 一致性历史从零开始（避免上一段跟踪的样本污染本次判定）
+            self._los_monitor.reset()
+            self._los_consistent = True
+            self._los_logged_ok = True
         else:
             self._guidance.reset()
+        # 切律后按新律频率重算节流周期，并允许立即出第一拍
+        self._law_period_ms = max(
+            1, int(round(1000.0 / max(1e-6, self._active_frequency_hz()))))
+        self._last_law_ms = None
 
     async def step(self, now_ms: int):
         """单控制周期（显式传入时钟，便于确定性单测）。返回 ControlOutput。
@@ -142,19 +191,43 @@ class PodBridge:
         按运行时选择的控制律在 L1（TrackingControlLaw，吊舱角度直入）与
         L2（GuidanceLaw，表4/注入坐标地理引导）间切换（方案 §7）。
         """
+        # 链路维持先做：poll 吊舱 + 按 link.attitude_downlink_hz 下发表3 姿态。
+        # 这两件事与生效律频率无关（吊舱解算依赖 表3，目标门控依赖遥测新鲜度）。
         self._ingest_pod_frames(now_ms)
-
         own = self._px4.own_state()
         self._downlink_attitude(own, now_ms)
 
+        # 生效律节流：外部（任务编排器）统一 20Hz 调用，而 L2 引导律配置为 10Hz。
+        # 未到下一拍时**重发上一拍输出**（而非刹车），否则会与本拍的 GOTO 指令打架。
+        if (self._last_law_ms is not None
+                and now_ms - self._last_law_ms < self._law_period_ms
+                and self._last_law_output is not None):
+            await self._dispatch(self._last_law_output, now_ms)
+            return self._last_law_output
+
+        self._last_law_ms = now_ms
         if self._use_heading_servo:
             out = self._tracking.update(self._build_snapshot(own, now_ms), now_ms)
         else:
             out = self._guidance.update(
                 self._build_guidance_input(own, now_ms), now_ms)
         await self._dispatch(out, now_ms)
+        self._last_law_output = out
         return out
 
+    async def search_step(self, now_ms: int, yaw_rate_dps: float) -> None:
+        """搜索周期（SEARCH 态）：只维持链路与心跳 + 原地偏航扫视，不出控制律。
+
+        之所以不让 SEARCH 走 step()：该态既不用 L2 地理引导（已在参考点，再发
+        速度会漂离），也不用 L1（还没锁定目标，控制律会判丢输出刹车）。此处
+        **仍要 poll 吊舱并下发表3 姿态**——否则搜索期间吊舱遥测超龄、锁定位永远
+        不新鲜，搜到目标也切不进 TRACK。
+        """
+        self._ingest_pod_frames(now_ms)
+        own = self._px4.own_state()
+        self._downlink_attitude(own, now_ms)
+        await self._px4.send_yaw_rate(yaw_rate_dps)
+
     # --- 吊舱侧 -------------------------------------------------------------
     def _ingest_pod_frames(self, now_ms: int) -> None:
         for frame in self._pod.poll():
@@ -239,16 +312,24 @@ class PodBridge:
         return target
 
     def pod_locked(self, now_ms: int) -> bool:
-        """吊舱目标门控（公共接口，供编排层/状态机复用，A3）。"""
+        """目标门控（公共接口，供编排层/状态机复用，A3）。
-        return self._pod_locked(now_ms)
+
+        = 吊舱 bit2 门控 **AND** LOS 运动学一致性判定。
+        这样"bit2 粘滞但角度冻结"的幽灵目标会让状态机退出 TRACK 重新搜索，
+        而不只是让控制律单方面刹车（否则状态机会一直以为还锁着）。
+        """
+        return self._pod_locked(now_ms) and self._los_consistent
 
     def _pod_locked(self, now_ms: int) -> bool:
-        """吊舱目标门控：表2-2 遥测在 link.telemetry_timeout_ms 内、状态字节1
+        """吊舱 bit2 门控（**不含** LOS 一致性，供内部合成本拍快照用）。
-        bit2=锁定、且（若 link.require_laser_valid）激光测距有效，才算目标有效。
+
+        表2-2 遥测在 link.telemetry_timeout_ms 内、状态字节1 bit2=锁定、
+        且（若 link.require_laser_valid）激光测距有效，才算"吊舱自报锁定"。
 
         实测依据：部分吊舱目标丢失后**锁定位置位不复位**（脱靶量也没有无效标志位，
         恒为 0），仅凭 bit2 会朝过期坐标继续飞；激光无效时吊舱不解算目标经纬度，
         故此兜底。无激光或激光恒无效的吊舱把 require_laser_valid 置 False。
+        更进一步的"角度是否活体"由 LOS 一致性判据在 _build_snapshot 里叠加。
 
         从未收到遥测 / 遥测超龄 / 搜索状态 / 激光无效 一律视为目标无效（宁可刹车）。"""
         tel = self._latest_telemetry
@@ -293,17 +374,38 @@ class PodBridge:
             alpha_deg = 0.0
 
         tracked = self._pod_locked(now_ms)
-        # §2.3：require_vision 时需我方视觉作第二判据（bit2 粘滞兜底）。未接入视觉源
+        # 第二判据：LOS 运动学一致性（独立于 bit2）。
-        # 时 fail-safe 为 False（无法确认 → 宁可刹车）。
+        # 本吊舱无电机、相机刚性固连机体 → 机头转 Δψ 时真实目标的脱靶量必变 ≈−Δψ；
-        if tracked and hs.require_vision:
+        # 机头已明显转动而 β 不动 → 角度冻结/幽灵目标 → 当丢目标处理（fail-safe）。
-            tracked = self._vision_target_fn() if self._vision_target_fn else False
+        # 仅在"吊舱自报锁定"期间喂样本；解锁即清历史，避免跨段污染。
+        if tracked:
+            status = self._los_monitor.update(
+                now_ms, math.degrees(own.yaw_rad), beta_deg)
+            self._los_consistent = status.consistent
+            if status.frozen:
+                tracked = False
+                if self._los_logged_ok:
+                    self._los_logged_ok = False
+                    logger.warning(
+                        "[Bridge] 吊舱角度与机头转动不一致（Δψ=%.1f° Δβ=%.1f° "
+                        "残差=%.1f°，超 %.1f°）→ 判为丢目标",
+                        status.delta_yaw_deg, status.delta_beta_deg,
+                        status.residual_deg, hs.los_max_residual_deg)
+            elif self._los_consistent and not self._los_logged_ok:
+                self._los_logged_ok = True
+                logger.info("[Bridge] 吊舱角度一致性恢复（残差 %.1f°）",
+                            status.residual_deg)
+        else:
+            self._los_monitor.reset()
+            self._los_consistent = True
+            self._los_logged_ok = True
 
         return ControlSnapshot(
             track=TrackState(tracked=tracked),
             track_time_ms=t_ms,          # 无遥测(t_ms=0) → 控制律判视觉超龄 → 降级
             los_beta_deg=beta_deg,
             los_alpha_deg=alpha_deg,
-            distance_valid=False,        # L1 无距离（L2 换 stereo_ranger.forward_distance_m）
+            distance_valid=False,        # L1 无距离（距离源接入后改为可用，见 prompt 文档）
             attitude_present=own.has_attitude,
             attitude_time_ms=own.time_ms,
             yaw_rad=own.yaw_rad,

utils/pod_bridge/config.py

@@ -67,7 +67,14 @@ class HeadingServoConfig:
     alpha_from_frame: bool = False      # α 是否叠加框架角 frame_pitch_deg
     alpha_from_off_target: bool = True  # α 是否叠加脱靶量 off_target_y_deg
     vertical_enabled: bool = True  # 无人机 L1 开垂直（决策3）；false → α 强制 0
-    require_vision: bool = False   # true = 需我方视觉作第二判据（S3 起，§2.3）
+    # --- 目标有效性第二判据：LOS 运动学一致性（2026-10-19 取代 require_vision）---
+    # 背景：吊舱已取代双目+YOLO 成为唯一目标源，"我方视觉"不再存在；而 bit2
+    # 丢失后不复位、脱靶量丢失表现为 0。故改用"机头转动 vs 脱靶量变化"的一致性
+    # 作为独立于 bit2 的活体判据（详见 los_consistency.py）。
+    los_consistency_enabled: bool = True
+    los_window_ms: int = 3000              # 比较窗口（毫秒）
+    los_min_yaw_change_deg: float = 20.0   # 窗口内机头至少转这么多才判定（激励门槛）
+    los_max_residual_deg: float = 8.0      # |Δβ+Δψ| 上限，超过判不一致
 
 
 @dataclass
@@ -283,8 +290,20 @@ def _load_heading_servo(root: dict) -> HeadingServoConfig:
         s, "heading_servo", "alpha_from_off_target", c.alpha_from_off_target)
     c.vertical_enabled = _get_bool(
         s, "heading_servo", "vertical_enabled", c.vertical_enabled)
-    c.require_vision = _get_bool(
+    c.los_consistency_enabled = _get_bool(
-        s, "heading_servo", "require_vision", c.require_vision)
+        s, "heading_servo", "los_consistency_enabled",
+        c.los_consistency_enabled)
+    c.los_window_ms = int(_positive(
+        _get_integer(s, "heading_servo", "los_window_ms", c.los_window_ms),
+        "heading_servo.los_window_ms"))
+    c.los_min_yaw_change_deg = _in_range(
+        _get_number(s, "heading_servo", "los_min_yaw_change_deg",
+                    c.los_min_yaw_change_deg),
+        0.0, 180.0, "heading_servo.los_min_yaw_change_deg")
+    c.los_max_residual_deg = _in_range(
+        _get_number(s, "heading_servo", "los_max_residual_deg",
+                    c.los_max_residual_deg),
+        0.0, 180.0, "heading_servo.los_max_residual_deg")
     return c
 
 

utils/pod_bridge/ground_link.py

@@ -1,20 +1,25 @@
-"""ground_link.py —— 地面坐标注入通道（预定流程.md「地面给坐标」）。
+"""ground_link.py —— 地面站链路（坐标注入 + 控制命令，预定流程.md「地面给坐标」）。
 
 本吊舱无激光 → 表4 目标经纬度永不出（部署文档 §10.0），目标坐标改由地面站注入。
-本模块负责接收地面站下发的目标坐标（UDP JSON），维护一份带本地打戳的注入目标
+本模块负责接收地面站下发的两类 UDP JSON 消息：
-缓存，供 PodBridge 的 L2 地理引导**优先消费**（回退表4）。
+  * 目标坐标 {"lat":..,"lon":..,"alt":..} → 维护一份带本地打戳的注入目标缓存，
+    供 PodBridge 的 L2 地理引导**优先消费**（回退表4）；
+  * 控制命令 {"cmd":"<name>"}（rtl / emergency / pod_*）→ 入命令队列，
+    由 TaskRunner 主循环 drain_commands() 取走并分派。
 
 设计要点：
-  * UDP 无连接监听，每包 JSON {"lat":..,"lon":..,"alt":..}（可选 "ts" 仅诊断，不用）。
+  * UDP 无连接监听；坐标包可选 "ts" 仅诊断，不用。
   * 新鲜度用「本地接收时刻」打戳（now_ms_fn 单调时钟），**不信任地面站时钟**
     （两端时钟不同步）；超龄由下游按 GroundInjectConfig.timeout_ms 判无效。
-  * parse_payload 为纯函数（可单测）；socket / 事件循环拆到 run()，且 socket
+  * 命令是事件（非状态）：队列缓存、满则丢最旧；未知命令名由上层忽略。
-    延迟导入，模块 import 无副作用（bridge 仅复用 GroundTarget 类型）。
+  * parse_payload / parse_message 为纯函数（可单测）；socket / 事件循环拆到
+    run()，且 socket 延迟导入，模块 import 无副作用（bridge 仅复用 GroundTarget 类型）。
 """
 
 import asyncio
 import json
 import logging
+from collections import deque
 from dataclasses import dataclass
 from typing import Callable, Optional
 
@@ -42,6 +47,18 @@ class GroundTarget:
     time_ms: int = 0  # 本地接收时刻（单调毫秒），0 = 从未注入
 
 
+@dataclass
+class GroundCommand:
+    """地面站控制命令（事件语义）。
+
+    name 为小写命令名（rtl / emergency / pod_detect_on / …）；time_ms 为本地
+    接收时刻（单调毫秒，仅日志/诊断用）。合法性（白名单）由上层分派时判。
+    """
+
+    name: str = ""
+    time_ms: int = 0
+
+
 @dataclass
 class GroundInjectConfig:
     """地面坐标注入配置（全外置；数值占位，待落地时回填）。"""
@@ -53,12 +70,13 @@ class GroundInjectConfig:
 
 
 class GroundLink:
-    """地面坐标注入接收器。
+    """地面站链路接收器（坐标注入 + 控制命令队列）。
 
     用法（在 runner 外部以 asyncio task 运行）：
         link = GroundLink(cfg, now_ms_fn)
         task = asyncio.create_task(link.run(stop_event))
-    PodBridge 通过 `ground_target_fn=lambda: link.latest()` 读取注入坐标。
+    PodBridge 通过 `ground_target_fn=lambda: link.latest()` 读取注入坐标；
+    TaskRunner 主循环通过 `drain_commands()` 取控制命令。
     """
 
     def __init__(self, cfg: GroundInjectConfig,
@@ -66,6 +84,7 @@ class GroundLink:
         self._cfg = cfg
         self._now_ms = now_ms_fn
         self._target = GroundTarget()
+        self._commands: deque[str] = deque(maxlen=16)
 
     def latest(self) -> GroundTarget:
         """返回当前注入目标（可能 time_ms=0 表示从未注入）。"""
@@ -76,6 +95,12 @@ class GroundLink:
         target.time_ms = self._now_ms()
         self._target = target
 
+    def drain_commands(self) -> list[str]:
+        """取走全部待处理命令（FIFO，取后清空）。命令为事件，无新鲜度概念。"""
+        cmds = list(self._commands)
+        self._commands.clear()
+        return cmds
+
     def is_valid(self, now_ms: int) -> bool:
         """注入坐标是否新鲜有效（未注入 / 超龄 → False）。"""
         t = self._target
@@ -84,15 +109,19 @@ class GroundLink:
         return not is_stale(t.time_ms, now_ms, self._cfg.timeout_ms)
 
     @staticmethod
-    def parse_payload(payload: bytes) -> Optional[GroundTarget]:
+    def _load_object(payload: bytes) -> Optional[dict]:
-        """解析地面站 JSON 包（纯函数，可单测）。返回不含 time_ms 的目标；
+        """解 JSON 且要求顶层为对象（纯函数）。非法 JSON / 非对象 → None。"""
-        非法 JSON / 非对象 / 缺 lat 或 lon → None（静默丢弃）。"""
         try:
             obj = json.loads(payload.decode("utf-8"))
         except (UnicodeDecodeError, json.JSONDecodeError):
             return None
         if not isinstance(obj, dict):
             return None
+        return obj
+
+    @staticmethod
+    def _target_from_object(obj: dict) -> Optional[GroundTarget]:
+        """从 dict 提取坐标（缺 lat/lon 或类型错 → None）。"""
         lat = obj.get("lat")
         lon = obj.get("lon")
         if not _is_number(lat) or not _is_number(lon):
@@ -103,6 +132,27 @@ class GroundLink:
         return GroundTarget(latitude_deg=float(lat), longitude_deg=float(lon),
                             abs_alt_m=float(alt))
 
+    @staticmethod
+    def parse_payload(payload: bytes) -> Optional[GroundTarget]:
+        """解析坐标包（纯函数，可单测）。返回不含 time_ms 的目标；
+        非法 JSON / 非对象 / 缺 lat 或 lon → None（静默丢弃）。"""
+        obj = GroundLink._load_object(payload)
+        if obj is None:
+            return None
+        return GroundLink._target_from_object(obj)
+
+    @staticmethod
+    def parse_message(payload: bytes) -> Optional[GroundTarget | GroundCommand]:
+        """解析任意地面站包（纯函数）：{"cmd":".."} → GroundCommand（优先），
+        否则按坐标包解析；非法包 → None（静默丢弃）。"""
+        obj = GroundLink._load_object(payload)
+        if obj is None:
+            return None
+        cmd = obj.get("cmd")
+        if isinstance(cmd, str) and cmd.strip():
+            return GroundCommand(name=cmd.strip().lower())
+        return GroundLink._target_from_object(obj)
+
     async def run(self, stop_event: asyncio.Event) -> None:
         """UDP 接收循环：每收一包合法 JSON 更新注入目标并本地打戳。"""
         import socket  # 延迟导入：仅真正跑 socket 时才需要
@@ -123,13 +173,17 @@ class GroundLink:
                     logger.warning("[GroundLink] 接收异常: %s", exc)
                     await asyncio.sleep(0.05)
                     continue
-                target = self.parse_payload(data)
+                msg = self.parse_message(data)
-                if target is None:
+                if msg is None:
                     continue  # 非法包静默丢弃（热路径不打日志）
-                target.time_ms = self._now_ms()
+                msg.time_ms = self._now_ms()
-                self._target = target
+                if isinstance(msg, GroundCommand):
+                    self._commands.append(msg.name)
+                    logger.debug("[GroundLink] 收到命令: %s", msg.name)
+                    continue
+                self._target = msg
                 logger.debug("[GroundLink] 更新注入目标 (%f, %f, %fm)",
-                             target.latitude_deg, target.longitude_deg,
+                             msg.latitude_deg, msg.longitude_deg,
-                             target.abs_alt_m)
+                             msg.abs_alt_m)
         finally:
             sock.close()

utils/pod_bridge/los_consistency.py

@@ -0,0 +1,115 @@
+"""los_consistency.py —— 吊舱角度「运动学一致性」校验（替代旧的 require_vision）。
+
+为什么需要它
+------------
+吊舱协议有两处已知缺陷（部署文档 §10）：
+  * bit2 锁定位在目标丢失后**不复位**；
+  * 脱靶量没有无效标志位，**丢失时表现为 0**。
+只信 bit2 会让飞机朝"过期坐标/幽灵目标"继续飞。
+
+原设计用「我方双目视觉」作第二判据（`require_vision`）。但后续决定**吊舱取代
+双目 + YOLO 成为唯一目标源**，于是"我方视觉"不再存在：该判据要么恒 False
+（链路永远进不了 TRACK），要么指向吊舱自身（自证，零信息量）。
+
+本模块给出**不依赖任何额外传感器**的替代判据：
+    本吊舱无电机、框架刚性固连机体 → 相机光轴随刚体转动。
+    因此目标在世界系静止时，机头转 Δψ，脱靶量必须变化约 −Δψ。
+    若机头已经明显转动（|Δψ| ≥ 阈值）而脱靶量几乎不动 → 角度冻结/幽灵，
+    判为不一致（fail-safe：当丢目标处理）。
+
+判别量：`residual = Δβ + Δψ`（β 为右正视线角，ψ 为 NED 航向，均取度）。
+  * 一致：|residual| ≤ max_residual_deg（目标在世界系缓慢移动也会落在余量内）
+  * 不一致：|residual| 超限且 |Δψ| 足够大（否则是"没有激励"，不做判断）
+
+局限（必须在文档里写清）：它只能证明「角度是活的」，**不能证明「是那个目标」**。
+若吊舱锁错了物体但角度正常变化，本判据无法识别 —— 那属于识别问题，需另接
+独立传感器（双目/其他），不是本模块能覆盖的。
+"""
+
+from collections import deque
+from dataclasses import dataclass
+import math
+
+
+def wrap_deg_180(deg: float) -> float:
+    """角度回绕到 [-180, 180)。"""
+    d = math.fmod(deg + 180.0, 360.0)
+    if d < 0.0:
+        d += 360.0
+    return d - 180.0
+
+
+@dataclass
+class LosConsistencyConfig:
+    """一致性判据参数（全外置）。"""
+
+    enabled: bool = True            # False → 关掉判据（退回"只信 bit2"）
+    window_ms: int = 3000           # 比较窗口：用"当前"与"约 window 前"的样本比对
+    min_yaw_change_deg: float = 20.0   # 窗口内机头至少转这么多才做判断（激励门槛）
+    max_residual_deg: float = 8.0      # |Δβ+Δψ| 超过即判不一致
+    history_factor: float = 2.0     # 历史缓冲 = window × factor
+
+
+@dataclass
+class LosStatus:
+    """单次判定结果（供门控与日志）。"""
+
+    consistent: bool = True     # 是否一致（默认一致 = 不干扰正常跟踪）
+    excited: bool = False       # 本窗口是否达到"激励门槛"（转过足够角度）
+    residual_deg: float = 0.0   # Δβ + Δψ
+    delta_yaw_deg: float = 0.0
+    delta_beta_deg: float = 0.0
+
+    @property
+    def frozen(self) -> bool:
+        """判为"角度冻结/幽灵"：有激励但不一致。"""
+        return self.excited and not self.consistent
+
+
+class LosConsistencyMonitor:
+    """按窗口比较 Δβ 与 Δψ，判断吊舱角度是否与机体转动一致。
+
+    用法：每个控制周期喂一次（仅在吊舱报告锁定期间喂），
+    `update(now_ms, yaw_deg, beta_deg)` 返回 `LosStatus`。
+    吊舱解锁时应调用 `reset()`，避免上一次跟踪的历史污染下一次。
+    """
+
+    def __init__(self, cfg: LosConsistencyConfig | None = None) -> None:
+        self._cfg = cfg or LosConsistencyConfig()
+        self._samples: deque = deque()
+
+    def reset(self) -> None:
+        self._samples.clear()
+
+    def update(self, now_ms: int, yaw_deg: float, beta_deg: float) -> LosStatus:
+        cfg = self._cfg
+        if not cfg.enabled:
+            return LosStatus(consistent=True, excited=False)
+
+        self._samples.append((now_ms, float(yaw_deg), float(beta_deg)))
+        keep_ms = cfg.window_ms * max(1.0, cfg.history_factor)
+        while self._samples and now_ms - self._samples[0][0] > keep_ms:
+            self._samples.popleft()
+
+        # 参考样本：窗口内**最老**的那个
+        ref = None
+        for sample in self._samples:
+            if now_ms - sample[0] <= cfg.window_ms:
+                ref = sample
+                break
+        if ref is None:
+            return LosStatus(consistent=True, excited=False)
+
+        span_ms = now_ms - ref[0]
+        # 历史不足一个窗口的 3/4 时，Δψ 天然偏小，不做判断（避免误判丢目标）
+        if span_ms < cfg.window_ms * 0.75:
+            return LosStatus(consistent=True, excited=False)
+
+        delta_yaw = wrap_deg_180(yaw_deg - ref[1])
+        delta_beta = beta_deg - ref[2]
+        residual = delta_beta + delta_yaw
+        excited = abs(delta_yaw) >= cfg.min_yaw_change_deg
+        consistent = (not excited) or (abs(residual) <= cfg.max_residual_deg)
+        return LosStatus(consistent=consistent, excited=excited,
+                         residual_deg=residual, delta_yaw_deg=delta_yaw,
+                         delta_beta_deg=delta_beta)

utils/pod_bridge/px4_gateway.py

@@ -3,6 +3,8 @@
 职责（桥接的飞控侧）：
   * 订阅 PX4 遥测（position / attitude_euler / gps_info / velocity_ned），缓存为
     OwnState 快照，供桥接层构造 表3(载机姿态下发) 与 GuidanceInput(引导输入)；
+  * 订阅飞控状态文本 status_text()（STATUSTEXT），缓存供任务日志/地面站显示
+    （飞控警告/异常信息）；
   * 复用 visual_track_control.MavsdkCommandGateway 发送 offboard 速度设定点
     （不重造三分支映射，直接喂 GuidanceLaw 的 ControlOutput）。
 
@@ -15,6 +17,7 @@ import asyncio
 import logging
 import math
 import time
+from collections import deque
 from dataclasses import dataclass, replace
 from typing import Any, Callable, Optional
 
@@ -23,6 +26,7 @@ from visual_track_control.mavsdk_gateway import MavsdkCommandGateway
 logger = logging.getLogger(__name__)
 
 _GPS_FIX_3D = 3  # mavsdk GpsFixType.THREE_D_FIX 起视为定位有效
+_FC_MSG_BUFFER = 64  # 飞控状态文本缓存上限（低频流，仅供日志消费）
 
 
 def _enum_int(value: Any) -> int:
@@ -69,6 +73,7 @@ class Px4Gateway:
         self._now_ms = now_ms_fn or (lambda: int(time.monotonic() * 1000))
         self._state = OwnState()
         self._tasks: list = []
+        self._fc_msgs: deque = deque(maxlen=_FC_MSG_BUFFER)
 
     # --- 连接（实机）-------------------------------------------------------
     @classmethod
@@ -123,13 +128,25 @@ class Px4Gateway:
         """返回当前缓存的快照副本（调用方修改不影响内部状态）。"""
         return replace(self._state)
 
+    def update_fc_status(self, st: Any) -> None:
+        """缓存一条飞控状态文本（STATUSTEXT）；severity 取枚举名（如 WARNING）。"""
+        sev = getattr(st, "type", None)
+        sev_name = getattr(sev, "name", None) or str(sev or "UNKNOWN")
+        self._fc_msgs.append((sev_name, str(getattr(st, "text", ""))))
+
+    def take_fc_messages(self) -> list:
+        """取走并清空飞控状态文本（供任务日志落盘；无则返回 []）。"""
+        msgs = list(self._fc_msgs)
+        self._fc_msgs.clear()
+        return msgs
+
     def _recompute_valid(self) -> None:
         self._state.position_valid = (self._state.has_position
                                       and self._state.fix_type >= _GPS_FIX_3D)
 
     # --- 遥测订阅（异步胶水）----------------------------------------------
     async def start_telemetry(self) -> None:
-        """订阅四路遥测流，后台任务持续刷新缓存。幂等：已订阅则直接返回。"""
+        """订阅遥测流 + 飞控状态文本，后台任务持续刷新缓存。幂等：已订阅则直接返回。"""
         if self._tasks:
             return  # 已订阅，避免重复起任务（runner 会先订阅，bridge.start 再触发时跳过）
         t = self._drone.telemetry
@@ -140,6 +157,8 @@ class Px4Gateway:
             asyncio.ensure_future(self._consume(t.gps_info(), self.update_gps)),
             asyncio.ensure_future(
                 self._consume(t.velocity_ned(), self.update_velocity_ned)),
+            asyncio.ensure_future(
+                self._consume(t.status_text(), self.update_fc_status)),
         ]
         logger.info("[Px4Gateway] 遥测订阅已启动")
 
@@ -171,5 +190,9 @@ class Px4Gateway:
     async def send_brake_hover(self, time_boot_ms: Optional[int] = None) -> bool:
         return await self._cmd.send_brake_hover(time_boot_ms)
 
+    async def send_yaw_rate(self, yaw_rate_dps: float) -> bool:
+        """原地偏航扫视（SEARCH 态）：零速 + 指定角速度，保持 offboard 心跳。"""
+        return await self._cmd.send_yaw_rate(yaw_rate_dps)
+
     async def stop_offboard(self) -> None:
         await self._cmd.stop()

utils/sitl_support.py

@@ -0,0 +1,131 @@
+"""sitl_support.py —— SITL / 真机入口共用的 MAVSDK 小工具。
+
+从根目录 `pod_sitl_test.py` 拆出：原先两个现行入口
+（`task_runner_sitl.py` / `task_runner_real.py`）都从这个"测试脚本"里 import
+`now_ms` / `wait_until_ready` 等，语义上很别扭。现归位为独立支持模块，
+入口只依赖它，`l1_sitl_test.py` 则作为纯粹的 L1 集成测试脚本。
+
+内容（刻意保持无业务逻辑）：
+  * `RATE_*`：遥测流速率**唯一来源**（各订阅方共用；PX4 速率由最后一次设置决定）
+  * `now_ms`：单调毫秒时钟（与 PodBridge / TaskRunner 同源）
+  * `TelemetryHub`：后台订阅 NED 位置 + 欧拉角，供 VirtualPod 几何合成
+  * `wait_until_ready` / `set_telemetry_rates` / `print_flight_mode`：连接准备与诊断
+"""
+
+import asyncio
+import math
+import time
+
+from pod_bridge.virtual_pod import CarrierView
+
+# 遥测流速率（Hz）——**唯一来源**，供各订阅方（Px4Gateway / TelemetryHub）共用。
+# PX4 的消息速率由最后一次 SET_MESSAGE_INTERVAL 决定，多个订阅方用不同值会互相
+# 覆盖；速率越高链路负荷越大（实测压出 MAVSDK 回调队列积压告警）。
+# 取值依据见 set_telemetry_rates()。
+RATE_ATTITUDE_HZ = 20.0     # 控制律 attitude_stale_ms=300 要用姿态
+RATE_POSITION_HZ = 10.0     # 起飞到位 / 到参考点距离
+RATE_POSITION_VELOCITY_NED_HZ = 10.0  # 虚拟吊舱几何（TelemetryHub）
+RATE_GPS_HZ = 2.0
+RATE_VELOCITY_HZ = 2.0
+RATE_IN_AIR_HZ = 2.0
+
+
+def now_ms() -> int:
+    """单调毫秒（与 Px4Gateway / PodBridge / TaskRunner 共用同一时钟源）。"""
+    return int(time.monotonic() * 1000.0)
+
+
+class TelemetryHub:
+    """后台订阅 NED 位置/姿态，供 VirtualPod 几何合成与锚点记录。"""
+
+    def __init__(self) -> None:
+        self.yaw_rad = 0.0
+        self.north_m = 0.0
+        self.east_m = 0.0
+        self.down_m = 0.0
+        self.attitude_time_ms = 0
+        self._tasks: list = []
+
+    def start(self, drone) -> None:
+        self._tasks.append(asyncio.create_task(self._attitude_loop(drone)))
+        self._tasks.append(asyncio.create_task(self._position_loop(drone)))
+
+    async def _attitude_loop(self, drone) -> None:
+        async for euler in drone.telemetry.attitude_euler():
+            self.yaw_rad = math.radians(euler.yaw_deg)
+            self.attitude_time_ms = now_ms()
+
+    async def _position_loop(self, drone) -> None:
+        async for pos_vel in drone.telemetry.position_velocity_ned():
+            pos = pos_vel.position
+            self.north_m = pos.north_m
+            self.east_m = pos.east_m
+            self.down_m = pos.down_m
+
+    async def stop(self) -> None:
+        for task in self._tasks:
+            task.cancel()
+        for task in self._tasks:
+            try:
+                await task
+            except asyncio.CancelledError:
+                pass
+        self._tasks.clear()
+
+    def view(self) -> CarrierView:
+        return CarrierView(north_m=self.north_m, east_m=self.east_m,
+                           down_m=self.down_m, yaw_rad=self.yaw_rad)
+
+
+async def print_flight_mode(drone):
+    async for flight_mode in drone.telemetry.flight_mode():
+        print(f"[模式] 当前飞行模式: {flight_mode}")
+        return flight_mode
+
+
+async def wait_until_ready(drone):
+    """等待连接 + 位置估计就绪（各阶段限时 30 s）。"""
+    print("等待无人机连接...")
+    try:
+        async with asyncio.timeout(30.0):
+            async for state in drone.core.connection_state():
+                if state.is_connected:
+                    print("-- 已连接")
+                    break
+    except TimeoutError:
+        raise RuntimeError("30 s 内未建立 MAVSDK 连接（检查 SITL 是否在运行）") from None
+
+    print("等待全球位置估计就绪...")
+    try:
+        async with asyncio.timeout(30.0):
+            async for health in drone.telemetry.health():
+                if health.is_global_position_ok and health.is_home_position_ok:
+                    print("-- 位置估计 OK")
+                    break
+    except TimeoutError:
+        raise RuntimeError("30 s 内位置估计未就绪") from None
+
+
+async def set_telemetry_rates(drone):
+    """按**实际消费需求**设定遥测流速率（不是越高越好）。
+
+    取值依据（2026-10-15 复核）：
+      * `attitude_euler` 20Hz —— 控制律 `attitude_stale_ms=300` 要用姿态；20Hz 足够
+      * `position` 10Hz —— 任务层只需「起飞到位」与「到参考点距离」，10Hz 足够
+      * `position_velocity_ned` 10Hz —— 仅虚拟吊舱几何用（真机链路不订阅）
+      * `gps_info` / `velocity_ned` / `in_air` 2Hz —— 状态量，变化极慢
+
+    原值（position/attitude 各 50Hz + pvn 30Hz ≈ 130 msg/s）在 CPU 紧张的宿主机上
+    会压出 MAVSDK「User callback queue slow」告警；下调后链路负荷降一个量级。
+    失败沿用默认值并告警。
+    """
+    for stream, rate in (("position", RATE_POSITION_HZ),
+                         ("attitude_euler", RATE_ATTITUDE_HZ),
+                         ("position_velocity_ned", RATE_POSITION_VELOCITY_NED_HZ),
+                         ("gps_info", RATE_GPS_HZ),
+                         ("velocity_ned", RATE_VELOCITY_HZ),
+                         ("in_air", RATE_IN_AIR_HZ)):
+        try:
+            await getattr(drone.telemetry, f"set_rate_{stream}")(rate)
+        except Exception as e:
+            print(f"! 设置 {stream} 速率失败（沿用默认值）: {e}")