diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..d0b8f2d --- /dev/null +++ b/.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/ diff --git a/README.md b/README.md index ca3b414..514e597 100644 --- a/README.md +++ b/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/ -├── reference-c++/ # C++ 参考实现 -│ ├── target_tracker/ # 目标追踪库（独立子工程） -│ │ ├── include/ # 头文件目录 -│ │ ├── src/ # 源文件目录 -│ │ └── tests/ # 单元测试 -│ └── visual_track_control/# 视觉追踪控制库 -│ ├── include/ # 头文件目录 -│ ├── src/ # 源文件目录 -│ └── tests/ # 单元测试 -└── utils/ # Python 移植 - ├── target_tracker/ # 目标追踪（Python 版） - ├── visual_track_control/# 追踪控制（Python 版） - └── tests/ # 单元测试 +code_drone-test/ +├── utils/ # Python 实现（主体） +│ ├── pod_bridge/ # 吊舱 ↔ PX4 桥接（现行运行链路核心） +│ │ ├── bridge.py # PodBridge：L1/L2 运行时切换、目标门控、链路维持 +│ │ ├── protocol.py # 表2-1/2-2/3/4 帧编解码 +│ │ ├── pod_serial.py # 串口链路（115200 8N1） +│ │ ├── px4_gateway.py # MAVSDK 遥测订阅 + offboard 指令 +│ │ ├── guidance.py # L2 地理引导律（飞到坐标） +│ │ ├── ground_link.py # 地面站 UDP 坐标注入 + 命令队列 +│ │ ├── los_consistency.py # 目标有效性第二判据（LOS 运动学一致性） +│ │ ├── virtual_pod.py # 虚拟吊舱（SITL/台架） +│ │ └── {run_bridge,listen_pod,pod_console,sim_fc,verify_px4}.py # 台架工具 +│ ├── task_runner/ # 任务级编排（现行） +│ │ ├── state_machine.py # WAIT→ARM_TAKEOFF→GOTO→SEARCH→TRACK→RTL +│ │ ├── runner.py # 编排器（采集快照/动作/异常兜底） +│ │ ├── return_home.py # 自定义返航（回起飞点→恢复航向→降落，不爬高） +│ │ ├── pod_control.py # 吊舱识别/锁定指令（0x07/0x0F/0x0E/0x08） +│ │ └── log.py # 事件 JSONL + 遥测 CSV + 简报 +│ ├── visual_track_control/ # 控制律 + MAVSDK 网关（被运行链路复用） +│ ├── sitl_support.py # 入口共用：时钟/遥测订阅/连接准备/速率常量 +│ ├── config/ # 配置 JSON（pod_bridge.json 为主线配置） +│ ├── tests/ # pytest（446 条，仅运行链路） +│ └── README.md # utils 索引 +├── legacy/ # 已归档：双目 + YOLO 可选感知后端（175 条测试） +│ ├── utils/ # stereo_yolo / stereo_ranger / target_tracker + demo + tests +│ ├── docs/ # 三份双目规格 +│ ├── models/ # YOLO 权重 +│ └── README.md # 归档说明与运行方式 +├── tools/ # 临时/台架脚本（相机标定、视频预览；不参与运行链路） +├── ground_station/ # 地面站页面（Flask：坐标注入/命令/状态/视频） +├── task_runner_sitl.py # 任务全流程 SITL 入口（虚拟吊舱） +├── task_runner_real.py # 任务全流程真机入口（真吊舱串口 + UDP 注入） +├── l1_sitl_test.py # L1 航向伺服 SITL 集成测试（只覆盖控制律段） +├── reference-c++/ # C++ 参考实现（target_tracker / visual_track_control） +├── docs/ # 规格与方案（权威来源） +└── .workbuddy/memory/ # 项目记忆：ARCHITECTURE.md / MEMORY.md ``` -## 主要模块 +--- -### target_tracker - 目标追踪库 +## 2. 运行链路（现行） -提供基于卡尔曼滤波的目标追踪功能： +``` +地面站 ──UDP 坐标/命令──▶ ground_link ──▶ TaskRunner（状态机） + │ + ┌─────────────────────────────────────┼──────────────────────────────┐ + ▼ ▼ ▼ + GOTO(L2 飞参考点) ──到点──▶ SEARCH(扫视) ──锁定──▶ TRACK(L1 缓慢靠近) ──超时──▶ RTL + │ │ + 吊舱表2-2 角度/bit2 自定义返航→垂直降落 + ▲ │ + PodBridge ──表3 姿态──▶ 吊舱 PX4 offboard + │ + PX4 遥测 ◀── MAVSDK +``` -- **KalmanBoxFilter**: 8维卡尔曼滤波器（位置、速度） -- **Tracklet**: 目标轨迹管理与状态更新 -- **TargetTracker**: 主追踪器，包含关联、选举逻辑 +- **状态链**：`WAIT → ARM_TAKEOFF → GOTO → SEARCH → TRACK → RTL` + （`APPROACH` 保留但已非主流程；`TRACK` 不再因"目标居中"退出） +- **参考点语义**：地面坐标是**搜索起点**，不是终点；到点后转原地扫视。 +- **缓慢靠近**：复用 `TrackingControlLaw` 的无距离降级（`no_distance_action=slow_approach`，0.2 m/s）。 +- **返航**：不调 PX4 原生 `return_to_launch()`（会爬到 `RTL_RETURN_ALT`，SITL 默认 30 m）， + 改为保持转场高度回起飞点 → 原地恢复初始航向 → 垂直降落。 -核心特性： -- 贪婪关联算法进行检测匹配 -- 主目标选举三级回退机制 -- 支持 Coast（纯预测）与 Lost（丢失）状态管理 +--- -### visual_track_control - 视觉追踪控制库 +## 3. 环境与快速开始 -基于追踪结果生成无人机控制指令： +**Python 环境（唯一口径）**：conda env `pytorch` +（`D:\program file\conda\envs\pytorch\python.exe`，Python 3.12；含 numpy/mavsdk/pytest/flask/pyserial）。 -- **PIDController**: 距离 PID 控制 -- **TrackingControlLaw**: 追踪控制律 -- **MavlinkCommandGateway**: MAVLink 指令网关 -- **PixelToAngle**: 像素到角度转换 -- **StalenessChecker**: 数据时效性检查 -- **SlewRateLimiter**: 速率限制器 - -支持多种控制模式： -- **Rate 模式**: 角速率控制 -- **Position 模式**: 绝对航向控制 -- **距离控制**: 接近/后退策略 - -## 环境要求 - -### C++ 版本 -- CMake 3.10+ -- C++17 编译器 -- MAVLink 库（MAVSDK） - -### Python 版本 -- Python 3.12+ -- 依赖：`numpy`, `pymavlink`, `mavsdk` - -## 安装 - -### Python 环境 ```bash -pip install -r requirements.txt +# 单元测试（621 条） +cd "D:/ProgramData/code_drone-test/utils" +"D:/program file/conda/envs/pytorch/python.exe" -m pytest -q + +# 吊舱串口自检（只读；--detect 会下发 0x07） +"D:/program file/conda/envs/pytorch/python.exe" utils/pod_bridge/listen_pod.py COM5 --seconds 5 + +# S2 台架干跑：只接吊舱、不下发 offboard +"D:/program file/conda/envs/pytorch/python.exe" utils/pod_bridge/run_bridge.py --port COM5 --dry-run --duration 10 + +# 任务全流程 SITL（需 WSL 里 PX4 SITL） +"D:/program file/conda/envs/pytorch/python.exe" task_runner_sitl.py --conn udpin://0.0.0.0:14540 + +# 地面站页面 +"D:/program file/conda/envs/pytorch/python.exe" ground_station/app.py --udp-port 14555 --log-dir logs + +# 真机入口（真吊舱串口 + 地面站 UDP 注入 + PX4） +"D:/program file/conda/envs/pytorch/python.exe" task_runner_real.py --port COM5 \ + --conn udpin://0.0.0.0:14540 --target-type 1 # --land 改为就地降落 ``` -### C++ 构建 +> MAVSDK 连接串：本机 SITL 用 `udpin://0.0.0.0:14540`（WSL 镜像网络下可直接连通）。 + +--- + +## 4. 配置 + +主线配置：`utils/config/pod_bridge.json`（分组：`pod_serial` / `px4` / `link` / `guidance` / +`heading_servo` / `tracking` / `ground_inject`）。 + +- `heading_servo`：L1 开关、β/α 符号（S1 已标定 +1/−1）、 + **LOS 一致性判据**（`los_consistency_enabled` / `los_window_ms` / + `los_min_yaw_change_deg` / `los_max_residual_deg`）。 +- `tracking`：内嵌整份控制律配置（各分组须齐全）；无距离时看 + `distance.no_distance_action` 与 `no_distance_approach_limit_mps`。 +- `ground_inject`：地面站 UDP 端口与坐标超龄门限；**真机入口会用 `timeout_ms` + 统一 `guidance.target_stale_ms`**（避免"走走停停"）。 + +任务级参数在 `task_runner_{sitl,real}.py` 的 `build_sm_config()` / `build_return_config()`： +到达半径、搜索/追踪超时、扫视速率、返航速度与航向容差。**均为占位值，须台架标定。** + +--- + +## 5. 测试 + ```bash -cd reference-c++/target_tracker -mkdir build && cd build -cmake .. -make +# 运行链路（446 条） +cd utils && "D:/program file/conda/envs/pytorch/python.exe" -m pytest -q -cd ../../visual_track_control -mkdir build && cd build -cmake .. -make +# 归档的可选后端（175 条，默认不跑） +cd legacy/utils && PYTHONPATH="../../utils" "D:/program file/conda/envs/pytorch/python.exe" -m pytest -q ``` -## 测试 +| 组 | 条数 | 说明 | +|---|---|---| +| `visual_track_control` | 86 | 控制律（运行链路复用） | +| `pod_bridge` | 208 | 协议/桥接/引导/注入/**LOS 一致性** | +| `task_runner` | 122 | 状态机/编排器/**自定义返航**/日志/吊舱控制 | +| `ground_station` | 30 | 页面与日志解读 | +| **运行链路合计** | **446** | | +| （归档）`stereo_ranger` / `stereo_yolo` / `target_tracker` | 175 | 在 `legacy/`，见 `legacy/README.md` | + +--- + +## 6. C++ 参考实现 + +`reference-c++/` 是 `target_tracker` 与 `visual_track_control` 的参考实现（各自独立 CMake +子工程，含 gtest）。Python 侧为逐构件移植 + 行为修复，二者语义对齐。当前**无 build 目录**， +未复跑 `cmake && ctest`。构建方式： -### Python 测试 ```bash -cd utils -pytest +cd reference-c++/ && mkdir -p build && cd build && cmake .. && make && ctest ``` -### C++ 测试 -```bash -cd reference-c++/target_tracker/build -ctest +--- -cd ../../visual_track_control/build -ctest -``` - -## 配置 - -C++ 与 Python 版本均使用 JSON 格式配置文件： - -```json -{ - "control": { - "frequency_hz": 20.0, - "visual_stale_ms": 200, - "radar_stale_ms": 500, - "attitude_stale_ms": 300 - }, - "camera": { - "image_width": 1920, - "image_height": 1080, - "fov_h_deg": 60.0, - "fov_v_deg": 35.0 - }, - "heading": { - "mode": "rate", - "gain": 1.0, - "yaw_rate_limit_dps": 30.0, - "yaw_slew_limit_dps2": 60.0 - }, - "distance": { - "d_exp_m": 10.0, - "kp": 0.5, - "ki": 0.05, - "kd": 0.1, - "integral_limit": 2.0 - } -} -``` - -## 使用示例 - -### Python 追踪演示 -```bash -cd utils -python demo_track_control.py -``` - -### C++ 追踪演示 -```bash -cd reference-c++/visual_track_control/build -./track_control_demo -``` - -## 移植说明 - -Python 版本与 C++ 版的接口差异请参考各子目录的 README： -- [target_tracker Python 版](./utils/target_tracker/README.md) -- [visual_track_control Python 版](./utils/visual_track_control/README.md) - -## 许可证 +## 7. 许可 本项目遵循开源许可证协议。 - -## 贡献指南 - -欢迎提交 Issue 和 Pull Request 改进本项目。 \ No newline at end of file diff --git a/__pycache__/test.cpython-312.pyc b/__pycache__/test.cpython-312.pyc deleted file mode 100644 index d96a712..0000000 Binary files a/__pycache__/test.cpython-312.pyc and /dev/null differ diff --git a/docs/L1航向伺服实现方案.md b/docs/L1航向伺服实现方案.md index bd7b05a..eafdad8 100644 --- a/docs/L1航向伺服实现方案.md +++ b/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 干跑** -`run_bridge.py --dry-run` 复核方向与量级（目标偏左→yaw_rate 负、居中→趋 0），并标定 `heading.gain`； -之后 **SITL 全流程** `python pod_sitl_test.py`（需 PX4 SITL，本机尚未实跑）→ S3 不装桨接飞控。 +**电机无用** → `beta_from_frame/alpha_from_frame` 恒 false。 + +**已完成的后续验证（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 不装桨接飞控。 diff --git a/docs/光电吊舱控制协议G版-PB.docx b/docs/光电吊舱控制协议G版-PB.docx new file mode 100644 index 0000000..2062dbf Binary files /dev/null and b/docs/光电吊舱控制协议G版-PB.docx differ diff --git a/docs/树莓派4B部署注意事项.md b/docs/树莓派4B部署注意事项.md index ed2a38f..ae1c833 100644 --- a/docs/树莓派4B部署注意事项.md +++ b/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 这条引导路径在本设备上等于不存在** | -| 目标距离/坐标来源 | 必须改由**我方自有链路**提供:`stereo_yolo` 检测 → `stereo_ranger` 双目视距。吊舱只当作"稳定指向 + 表2-2 脱靶量"的伺服/传感器用 | +| `guidance.py` 地理位置引导律 | **表4 路径在本设备上恒不可用**(`target_valid` 恒假)。**现行做法:坐标走地面站 UDP 注入**(`ground_link` → `ground_target_fn`),`guidance` 吃注入坐标;表4 仅作回退 | +| 目标距离/坐标来源 | **坐标 = 地面站注入**(已落地)。**距离尚未接入**:规划由激光雷达提供(低算力);~~`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 双目视距(斜距/前向距离) - → 载机 GPS + 姿态(表3 我方已有) + 吊舱框架角(表2-2) - → **我方自己算出目标经纬度**,再喂给 guidance.py +目标**坐标** ← 地面站 UDP 注入(ground_link) ← 已落地 +目标**方位** ← 吊舱表2-2 脱靶量(角度直入 L1) ← 已落地 +目标**距离** ← 待接激光雷达(串口,低算力) ← 未接 ``` 吊舱在本设备上退化为:**伺服稳定指向 + 表2-2 脱靶量/框架角传感器**。 它仍然很有用(稳像、跟得住、给得出视场角),只是不再提供坐标。 -> 这意味着 `guidance.py` 的输入要从"吊舱表4"换成"我方自算的目标经纬度", -> `bridge.py:169` 的 `target_valid` 判据也要跟着改。**动手前先对齐方案再实现。** +> ~~下文"stereo_yolo → stereo_ranger 自算经纬度再喂 guidance"的方案~~ 已被上述 +> 分工取代:`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` 已覆盖),与"解锁以便重新 搜索"是两件事,不要混为一谈。 diff --git a/ground_station/app.py b/ground_station/app.py index c6572e8..421612c 100644 --- a/ground_station/app.py +++ b/ground_station/app.py @@ -1,30 +1,193 @@ """ground_station/app.py —— 地面站页面（Flask，临时测试用）。 -预定流程 C 阶段：地面坐标输入 + 状态可视化配套页面。 +预定流程 C 阶段：地面坐标输入 + 任务/吊舱控制 + 状态可视化配套页面。 功能： - * GET / 地面站页面（坐标输入 + 状态面板 + 模拟视野） - * POST /api/inject 接收 {lat, lon, alt}，UDP 发到吊舱桥接的 ground_inject 端口 - （默认 14555，GroundLink.run() 接收） - * GET /api/state 返回最新任务状态（读任务日志：事件最后状态 + 遥测最后一行） + * GET / 地面站页面（坐标注入 + 任务控制 + 吊舱控制 + 状态面板 + 模拟视野） + * GET /px4 PX4 信息子页面（实时遥测 + 任务日志解读分页） + * POST /api/inject 接收 {lat, lon, alt} 或局部 {north_m, east_m, alt} + （局部按注入时刻无人机位置换算为经纬度），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") - lon = data.get("lon") - if lat is None or lon is None: - return jsonify({"ok": False, "error": "需要 lat 和 lon"}), 400 try: - lat = float(lat) - lon = float(lon) + alt = float(data.get("alt", 0.0) or 0.0) except (TypeError, ValueError): - return jsonify({"ok": False, "error": "lat/lon 必须是数值"}), 400 - alt = float(data.get("alt", 0.0) or 0.0) + 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 或 north_m/east_m"}), 400 + try: + lat = float(lat) + lon = float(lon) + except (TypeError, ValueError): + return jsonify({"ok": False, + "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: - s.sendto(payload.encode("utf-8"), ("127.0.0.1", udp_port)) + _send_udp(target, {"lat": lat, "lon": lon, "alt": alt}) 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) - print(f"[地面站] http://{args.host}:{args.port} | UDP 注入 → 127.0.0.1:{args.udp_port}" - f" | 日志 → {args.log_dir}") + app = create_app(args.udp_port, args.log_dir, udp_host=args.udp_host, + rtsp_url=args.rtsp_url, + 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 diff --git a/ground_station/templates/index.html b/ground_station/templates/index.html index b5071a9..9c0f8be 100644 --- a/ground_station/templates/index.html +++ b/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); }

地面站 · 预定流程

-
地面坐标注入 + 状态可视化（临时测试用）
+
坐标注入 + 任务/吊舱控制 + 状态可视化（临时测试用）
+
+
+

链路目标 -

+ + + + + +
+

坐标注入

- + + + + - + - + +
局部坐标按选定原点换算（起飞前注入即为起飞点；「当前坐标」模式随注入时刻无人机位置变化）。
+ +
坐标须持续注入：断流超过 5s 判失效（任务将返航）。持续注入由服务端执行， + 切窗口/最小化不受影响；取消勾选或关闭地面站程序才停止。
+
+

任务控制

+
+ + +
+
RTL = 人工返航请求；紧急 = 最高优先级立即返航（均经 UDP 下发到 runner）。
+
+
+ +
+

吊舱控制

+
+ + + + + +
+
对齐 pod_console 按键：开识别 d(0x07+0x0F) / 自动锁定 a(0x0F+0x10) / + 停跟踪 u(0x0E，仅解当前锁，0x0F 自动锁定仍常驻) / 关识别 s(0x0E+0x08，彻底退出) / 归中 c(0x2B)
+
真机经 runner → 吊舱串口下发；SITL 无吊舱控制口，命令将被忽略。
+
+
+

任务状态 -

位置-
@@ -65,28 +140,108 @@
+
+

任务日志 0

+
加载中…
+
阶段切换 / 地面站命令 / 飞控状态（STATUSTEXT 警告）实时滚动
+
+

模拟视野（脱靶量）

十字线 = 视野中心，红点 = 目标（按吊舱脱靶量映射）
+ +
+

吊舱视频 -

+ +
检测中…
+
diff --git a/ground_station/templates/px4.html b/ground_station/templates/px4.html new file mode 100644 index 0000000..7a160a6 --- /dev/null +++ b/ground_station/templates/px4.html @@ -0,0 +1,564 @@ + + + + + +地面站 · PX4 信息 + + + +
+

地面站 · PX4 信息

+
PX4 输出实时视图 + 任务日志解读
+ +
+ +
+
+ + +
+ + +
+
+

PX4 遥测 - + 注入 - + 视频 - +

+ + +
加载中…
+
+
+
+ + + +
+ + + + diff --git a/ground_station/video_source.py b/ground_station/video_source.py new file mode 100644 index 0000000..ec9241c --- /dev/null +++ b/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 diff --git a/pod_sitl_test.py b/l1_sitl_test.py similarity index 76% rename from pod_sitl_test.py rename to l1_sitl_test.py index fd919e3..c8089ba 100644 --- a/pod_sitl_test.py +++ b/l1_sitl_test.py @@ -1,5 +1,5 @@ -#!/usr/bin/env python3 -"""pod_sitl_test.py —— L1 航向伺服 SITL 全流程集成测试（test.py 的吊舱版）。 +#!/usr/bin/env python3 +"""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 pod_sitl_test.py --conn udp://:14540 --config utils/config/pod_bridge.json + python l1_sitl_test.py + 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) - -class TelemetryHub: - """后台订阅 NED 位置/姿态，供 VirtualPod 几何合成与锚点记录（同 test.py）。""" - - 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}") +# 共享件（时钟/遥测订阅/连接准备）：utils/sitl_support.py +from sitl_support import (TelemetryHub, now_ms, print_flight_mode, + set_telemetry_rates, wait_until_ready) async def takeoff(drone) -> bool: diff --git a/legacy/README.md b/legacy/README.md new file mode 100644 index 0000000..6b1c53f --- /dev/null +++ b/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 上跑不动实时）。 diff --git a/docs/双目YOLO目标识别需求说明.md b/legacy/docs/双目YOLO目标识别需求说明.md similarity index 100% rename from docs/双目YOLO目标识别需求说明.md rename to legacy/docs/双目YOLO目标识别需求说明.md diff --git a/docs/双目测距技术路线.md b/legacy/docs/双目测距技术路线.md similarity index 100% rename from docs/双目测距技术路线.md rename to legacy/docs/双目测距技术路线.md diff --git a/legacy/docs/双目相机模组到位作业指导.md b/legacy/docs/双目相机模组到位作业指导.md new file mode 100644 index 0000000..252896b --- /dev/null +++ b/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//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（性能实测）不在本文范围**：它依赖机载/边缘平台，属平台工程文档。 diff --git a/models/yolo26n-drone.pt b/legacy/models/yolo26n-drone.pt similarity index 100% rename from models/yolo26n-drone.pt rename to legacy/models/yolo26n-drone.pt diff --git a/utils/assets/stereo_yolo/drone_sky.png b/legacy/utils/assets/stereo_yolo/drone_sky.png similarity index 100% rename from utils/assets/stereo_yolo/drone_sky.png rename to legacy/utils/assets/stereo_yolo/drone_sky.png diff --git a/utils/config/stereo_ranger.json b/legacy/utils/config/stereo_ranger.json similarity index 100% rename from utils/config/stereo_ranger.json rename to legacy/utils/config/stereo_ranger.json diff --git a/utils/config/stereo_yolo.json b/legacy/utils/config/stereo_yolo.json similarity index 100% rename from utils/config/stereo_yolo.json rename to legacy/utils/config/stereo_yolo.json diff --git a/legacy/utils/conftest.py b/legacy/utils/conftest.py new file mode 100644 index 0000000..96490d9 --- /dev/null +++ b/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)) diff --git a/utils/demo_stereo_ranger.py b/legacy/utils/demo_stereo_ranger.py similarity index 100% rename from utils/demo_stereo_ranger.py rename to legacy/utils/demo_stereo_ranger.py diff --git a/utils/demo_stereo_yolo.py b/legacy/utils/demo_stereo_yolo.py similarity index 100% rename from utils/demo_stereo_yolo.py rename to legacy/utils/demo_stereo_yolo.py diff --git a/utils/demo_track_control.py b/legacy/utils/demo_track_control.py similarity index 100% rename from utils/demo_track_control.py rename to legacy/utils/demo_track_control.py diff --git a/legacy/utils/pytest.ini b/legacy/utils/pytest.ini new file mode 100644 index 0000000..b7f7a7b --- /dev/null +++ b/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 diff --git a/utils/stereo_ranger/README.md b/legacy/utils/stereo_ranger/README.md similarity index 100% rename from utils/stereo_ranger/README.md rename to legacy/utils/stereo_ranger/README.md diff --git a/utils/stereo_ranger/__init__.py b/legacy/utils/stereo_ranger/__init__.py similarity index 100% rename from utils/stereo_ranger/__init__.py rename to legacy/utils/stereo_ranger/__init__.py diff --git a/utils/stereo_ranger/config.py b/legacy/utils/stereo_ranger/config.py similarity index 100% rename from utils/stereo_ranger/config.py rename to legacy/utils/stereo_ranger/config.py diff --git a/utils/stereo_ranger/consistency.py b/legacy/utils/stereo_ranger/consistency.py similarity index 100% rename from utils/stereo_ranger/consistency.py rename to legacy/utils/stereo_ranger/consistency.py diff --git a/utils/stereo_ranger/distance_filter.py b/legacy/utils/stereo_ranger/distance_filter.py similarity index 100% rename from utils/stereo_ranger/distance_filter.py rename to legacy/utils/stereo_ranger/distance_filter.py diff --git a/utils/stereo_ranger/geometry.py b/legacy/utils/stereo_ranger/geometry.py similarity index 100% rename from utils/stereo_ranger/geometry.py rename to legacy/utils/stereo_ranger/geometry.py diff --git a/utils/stereo_ranger/matching.py b/legacy/utils/stereo_ranger/matching.py similarity index 100% rename from utils/stereo_ranger/matching.py rename to legacy/utils/stereo_ranger/matching.py diff --git a/utils/stereo_ranger/sampling.py b/legacy/utils/stereo_ranger/sampling.py similarity index 100% rename from utils/stereo_ranger/sampling.py rename to legacy/utils/stereo_ranger/sampling.py diff --git a/utils/stereo_ranger/stereo_ranger.py b/legacy/utils/stereo_ranger/stereo_ranger.py similarity index 100% rename from utils/stereo_ranger/stereo_ranger.py rename to legacy/utils/stereo_ranger/stereo_ranger.py diff --git a/utils/stereo_ranger/target_store.py b/legacy/utils/stereo_ranger/target_store.py similarity index 100% rename from utils/stereo_ranger/target_store.py rename to legacy/utils/stereo_ranger/target_store.py diff --git a/utils/stereo_ranger/types.py b/legacy/utils/stereo_ranger/types.py similarity index 100% rename from utils/stereo_ranger/types.py rename to legacy/utils/stereo_ranger/types.py diff --git a/utils/stereo_ranger/warn_throttle.py b/legacy/utils/stereo_ranger/warn_throttle.py similarity index 100% rename from utils/stereo_ranger/warn_throttle.py rename to legacy/utils/stereo_ranger/warn_throttle.py diff --git a/utils/stereo_yolo/README.md b/legacy/utils/stereo_yolo/README.md similarity index 100% rename from utils/stereo_yolo/README.md rename to legacy/utils/stereo_yolo/README.md diff --git a/utils/stereo_yolo/__init__.py b/legacy/utils/stereo_yolo/__init__.py similarity index 100% rename from utils/stereo_yolo/__init__.py rename to legacy/utils/stereo_yolo/__init__.py diff --git a/utils/stereo_yolo/config.py b/legacy/utils/stereo_yolo/config.py similarity index 100% rename from utils/stereo_yolo/config.py rename to legacy/utils/stereo_yolo/config.py diff --git a/utils/stereo_yolo/detector.py b/legacy/utils/stereo_yolo/detector.py similarity index 100% rename from utils/stereo_yolo/detector.py rename to legacy/utils/stereo_yolo/detector.py diff --git a/utils/stereo_yolo/frontend.py b/legacy/utils/stereo_yolo/frontend.py similarity index 100% rename from utils/stereo_yolo/frontend.py rename to legacy/utils/stereo_yolo/frontend.py diff --git a/utils/stereo_yolo/sbs.py b/legacy/utils/stereo_yolo/sbs.py similarity index 100% rename from utils/stereo_yolo/sbs.py rename to legacy/utils/stereo_yolo/sbs.py diff --git a/utils/stereo_yolo/warn_throttle.py b/legacy/utils/stereo_yolo/warn_throttle.py similarity index 100% rename from utils/stereo_yolo/warn_throttle.py rename to legacy/utils/stereo_yolo/warn_throttle.py diff --git a/utils/target_tracker/README.md b/legacy/utils/target_tracker/README.md similarity index 100% rename from utils/target_tracker/README.md rename to legacy/utils/target_tracker/README.md diff --git a/utils/target_tracker/__init__.py b/legacy/utils/target_tracker/__init__.py similarity index 100% rename from utils/target_tracker/__init__.py rename to legacy/utils/target_tracker/__init__.py diff --git a/utils/target_tracker/kalman_box_filter.py b/legacy/utils/target_tracker/kalman_box_filter.py similarity index 100% rename from utils/target_tracker/kalman_box_filter.py rename to legacy/utils/target_tracker/kalman_box_filter.py diff --git a/utils/target_tracker/target_tracker.py b/legacy/utils/target_tracker/target_tracker.py similarity index 100% rename from utils/target_tracker/target_tracker.py rename to legacy/utils/target_tracker/target_tracker.py diff --git a/utils/target_tracker/tracklet.py b/legacy/utils/target_tracker/tracklet.py similarity index 100% rename from utils/target_tracker/tracklet.py rename to legacy/utils/target_tracker/tracklet.py diff --git a/utils/tests/stereo_ranger/config_test.py b/legacy/utils/tests/stereo_ranger/config_test.py similarity index 100% rename from utils/tests/stereo_ranger/config_test.py rename to legacy/utils/tests/stereo_ranger/config_test.py diff --git a/utils/tests/stereo_ranger/consistency_test.py b/legacy/utils/tests/stereo_ranger/consistency_test.py similarity index 100% rename from utils/tests/stereo_ranger/consistency_test.py rename to legacy/utils/tests/stereo_ranger/consistency_test.py diff --git a/utils/tests/stereo_ranger/distance_filter_test.py b/legacy/utils/tests/stereo_ranger/distance_filter_test.py similarity index 100% rename from utils/tests/stereo_ranger/distance_filter_test.py rename to legacy/utils/tests/stereo_ranger/distance_filter_test.py diff --git a/utils/tests/stereo_ranger/geometry_test.py b/legacy/utils/tests/stereo_ranger/geometry_test.py similarity index 100% rename from utils/tests/stereo_ranger/geometry_test.py rename to legacy/utils/tests/stereo_ranger/geometry_test.py diff --git a/utils/tests/stereo_ranger/matching_test.py b/legacy/utils/tests/stereo_ranger/matching_test.py similarity index 100% rename from utils/tests/stereo_ranger/matching_test.py rename to legacy/utils/tests/stereo_ranger/matching_test.py diff --git a/utils/tests/stereo_ranger/sampling_test.py b/legacy/utils/tests/stereo_ranger/sampling_test.py similarity index 100% rename from utils/tests/stereo_ranger/sampling_test.py rename to legacy/utils/tests/stereo_ranger/sampling_test.py diff --git a/utils/tests/stereo_ranger/stereo_ranger_test.py b/legacy/utils/tests/stereo_ranger/stereo_ranger_test.py similarity index 100% rename from utils/tests/stereo_ranger/stereo_ranger_test.py rename to legacy/utils/tests/stereo_ranger/stereo_ranger_test.py diff --git a/utils/tests/stereo_ranger/target_store_test.py b/legacy/utils/tests/stereo_ranger/target_store_test.py similarity index 100% rename from utils/tests/stereo_ranger/target_store_test.py rename to legacy/utils/tests/stereo_ranger/target_store_test.py diff --git a/utils/tests/stereo_ranger/warn_throttle_test.py b/legacy/utils/tests/stereo_ranger/warn_throttle_test.py similarity index 100% rename from utils/tests/stereo_ranger/warn_throttle_test.py rename to legacy/utils/tests/stereo_ranger/warn_throttle_test.py diff --git a/utils/tests/stereo_yolo/config_test.py b/legacy/utils/tests/stereo_yolo/config_test.py similarity index 100% rename from utils/tests/stereo_yolo/config_test.py rename to legacy/utils/tests/stereo_yolo/config_test.py diff --git a/utils/tests/stereo_yolo/frontend_test.py b/legacy/utils/tests/stereo_yolo/frontend_test.py similarity index 100% rename from utils/tests/stereo_yolo/frontend_test.py rename to legacy/utils/tests/stereo_yolo/frontend_test.py diff --git a/utils/tests/stereo_yolo/sbs_test.py b/legacy/utils/tests/stereo_yolo/sbs_test.py similarity index 100% rename from utils/tests/stereo_yolo/sbs_test.py rename to legacy/utils/tests/stereo_yolo/sbs_test.py diff --git a/utils/tests/target_tracker/kalman_box_filter_test.py b/legacy/utils/tests/target_tracker/kalman_box_filter_test.py similarity index 100% rename from utils/tests/target_tracker/kalman_box_filter_test.py rename to legacy/utils/tests/target_tracker/kalman_box_filter_test.py diff --git a/utils/tests/target_tracker/target_tracker_test.py b/legacy/utils/tests/target_tracker/target_tracker_test.py similarity index 100% rename from utils/tests/target_tracker/target_tracker_test.py rename to legacy/utils/tests/target_tracker/target_tracker_test.py diff --git a/utils/tests/target_tracker/tracklet_test.py b/legacy/utils/tests/target_tracker/tracklet_test.py similarity index 100% rename from utils/tests/target_tracker/tracklet_test.py rename to legacy/utils/tests/target_tracker/tracklet_test.py diff --git a/requirements-pod.txt b/requirements-pod.txt index 392710d..40cc834 100644 --- a/requirements-pod.txt +++ b/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 条)需要，部署运行可省 diff --git a/requirements.txt b/requirements.txt index 712b412..8283354 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,21 +1,18 @@ -# code_drone-test Python 依赖 -# 扫描自全仓库 .py 文件的实际 import(2026-09-22) +# code_drone-test Python 依赖（运行链路） +# 按 utils/ + ground_station/ + tools/ 的实际 import 核对（2026-10-20 精简后） -# ---- 运行时核心 ---- -numpy # target_tracker 卡尔曼滤波 / stereo_yolo sbs 拆分 -opencv-python # camera.py / camera_test.py / video_test.py / demo_stereo_yolo.py (cv2) +# ---- 运行链路核心 ---- +mavsdk<4 # ⚠ 锁 <4：v4 为全新原生 API，经典 gRPC API（System/offboard/telemetry）已移除 +pyserial # 吊舱串口（pod_bridge.pod_serial，惰性导入） -# ---- 检测推理(stereo_yolo,真实推理才加载,torch 为其依赖) ---- -ultralytics - -# ---- 飞控通信(MAVSDK offboard -> PX4) ---- -mavsdk<4 # ⚠ 锁 <4:v4 为全新原生 API,经典 gRPC API(System/offboard/telemetry)已移除 - -# ---- 吊舱串口(pod_bridge.pod_serial,惰性导入) ---- -pyserial - -# ---- 可视化(test.py 轨迹绘图) ---- -matplotlib +# ---- 地面站页面 / 视频（ground_station.app + video_source）---- +flask # 页面与 API +opencv-python # RTSP 拉流 / MJPEG 推流；tools/ 下相机脚本也用 +numpy # video_source / tools 图像处理 # ---- 测试 ---- -pytest # utils/tests 全量单测(256 条) +pytest # utils/tests 运行链路单测（446 条） + +# ---- 可选：已归档的感知后端（仅跑 legacy/ 时需要）---- +# ultralytics # legacy/utils/stereo_yolo 真模型推理路径 +# torch # ultralytics 依赖 diff --git a/task_runner_real.py b/task_runner_real.py index ea1a84d..41587c8 100644 --- a/task_runner_real.py +++ b/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 自动锁定， - RTL 前发 0x0E 停止 + 0x08 识别关）； - * PX4 连接 = cfg.px4.connection_string（真机串口/遥测），非 SITL 14540。 + * 吊舱目标识别/锁定 = PodController（首次进入引导阶段发 0x07 识别 + 0x0F 自动锁定， + RTL 前发 0x0E 停止 + 0x08 识别关；**只发一次**，重入不会重发打断跟踪）； + * 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 坐标 - {"lat":..,"lon":..,"alt":..}（每 0.5~1s 一次）。 + 3. 地面站往 ground_inject.port（默认 14555）**持续**发 JSON 坐标 + {"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, - center_hold_s=3.0, - center_threshold_deg=3.0, - approach_hover_s=5.0, + search_timeout_s=20.0, # 搜索 20s 未发现目标 → 返航 + track_timeout_s=20.0, # 追踪/靠近段上限 → 返航（无距离，判不出"已到目标"） + center_threshold_deg=3.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}）") - print(f" 等待地面站注入坐标（每 0.5~1s 一次 JSON）… 日志 → {log_dir}") + f"地面站 UDP {cfg.ground_inject.port}，目标类型 {args.target_type}，" + 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)) diff --git a/task_runner_sitl.py b/task_runner_sitl.py index 0d1310d..c52c6f6 100644 --- a/task_runner_sitl.py +++ b/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（固定场景、人工编排）， -本脚本把「飞行编排 + 控制律切换」交给 TaskRunner 状态机统一驱动。 +用状态机驱动完整流程： + WAIT → ARM_TAKEOFF → GOTO(L2 飞向参考点) → SEARCH(原地扫视) → TRACK(L1 缓慢靠近) + → 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, - center_hold_s=3.0, - center_threshold_deg=3.0, # 脱靶量 < 3° 判居中 - approach_hover_s=5.0, # 悬停 5s 后 RTL + search_timeout_s=20.0, # 搜索 20s 未发现目标 → 返航 + track_timeout_s=20.0, # 追踪/靠近段上限 → 返航 + center_threshold_deg=3.0, # 脱靶量 < 3° 判居中（仅日志/诊断） + 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"center={sm_cfg.center_threshold_deg}°/{sm_cfg.center_hold_s}s " - f"hover={sm_cfg.approach_hover_s}s") + 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") # 状态打印 + 总超时保护 started = time.monotonic() diff --git a/test.py b/test.py deleted file mode 100644 index a59e18b..0000000 --- a/test.py +++ /dev/null @@ -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()) diff --git a/tools/README.md b/tools/README.md new file mode 100644 index 0000000..bff1ffc --- /dev/null +++ b/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 移入本目录，内容未改。 diff --git a/tools/camera.py b/tools/camera.py new file mode 100644 index 0000000..95e16c8 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/tools/camera_test.py b/tools/camera_test.py new file mode 100644 index 0000000..ffe1dda --- /dev/null +++ b/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() \ No newline at end of file diff --git a/tools/video_test.py b/tools/video_test.py new file mode 100644 index 0000000..7efd9bc --- /dev/null +++ b/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() \ No newline at end of file diff --git a/trajectory_plot.png b/trajectory_plot.png deleted file mode 100644 index d26d650..0000000 Binary files a/trajectory_plot.png and /dev/null differ diff --git a/utils/README.md b/utils/README.md index c2c6764..6d1a185 100644 --- a/utils/README.md +++ b/utils/README.md @@ -1,51 +1,57 @@ -# utils —— reference-c++ 的 Python 移植 + 双目链路新实现 +# utils —— Python 实现（运行链路） -将 `reference-c++/` 下的 `target_tracker`（多目标追踪过滤器）与 -`visual_track_control`（视觉跟踪控制律）移植为 Python，**两包互不依赖** -（唯一的数据转换点在胶水层 demo）。 +> **架构现状（2026-10-20）**：运行链路是「光电吊舱 → `pod_bridge` → `task_runner` → PX4」。 +> 双目 + YOLO 那套可选感知后端（`stereo_yolo` / `target_tracker` / `stereo_ranger`） +> **已整体归档到仓库根的 `legacy/`**，默认不参与 pytest 收集；接回方式见 `legacy/README.md`。 -另有双目链路新实现（非 C++ 移植、按规格实现）： -- `stereo_ranger`（双目稀疏目标测距），规格 `docs/双目测距技术路线.md`； -- `stereo_yolo`（双目 YOLO 检测前端），规格 `docs/双目YOLO目标识别需求说明.md`， - 输出直接作为 stereo_ranger 的输入（仅复用其 `types.py` 纯值类型）。 +本目录包含： -Python 侧定位为 **C++ 实现的先验验证**：构件划分与语义按规格冻结，便于逐构件 -移植对照；`stereo_yolo` 的 C++ 移植注记见其包 README。 +- **运行链路**：`pod_bridge`（吊舱桥接）+ `task_runner`（任务编排）+ `visual_track_control` + （控制律原语，被前两者复用） +- **入口共享件**：`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 网关 -│ └── README.md # 接口清单 + C++ 差异 -├── stereo_ranger/ # 包 3：双目稀疏目标测距（纯标准库，无第三方依赖） -│ └── README.md # 接口清单 + 与规格的偏差 -├── stereo_yolo/ # 包 4：双目 YOLO 检测前端（真模型需 ultralytics） -│ └── README.md # 接口清单 + C++ 移植注记 -├── demo_track_control.py # 胶水层集成演示（模拟检测 → 追踪 → 控制律 → 发送） -├── demo_stereo_ranger.py # 双目测距演示（合成框对，六场景，无硬件） -├── demo_stereo_yolo.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 导入模式避免跨包同名测试文件冲突 +├── config/ # 配置 JSON（pod_bridge.json 为主线） +├── tests/ # pytest（446 条） +│ ├── pod_bridge/ # 208 条（协议/桥接/引导/注入/节流/LOS） +│ ├── task_runner/ # 122 条（状态机/编排器/返航/日志/吊舱控制） +│ ├── visual_track_control/ # 86 条（对应 C++ gtest + L1 增量） +│ └── ground_station/ # 30 条（页面与日志解读） +├── conftest.py # 把 utils/ 插进 sys.path + fake_mavsdk fixture +├── pytest.ini # 收集 *_test.py + importlib 导入模式 +└── sitl_support.py # 入口共用：时钟/遥测订阅/连接准备/速率常量 ``` ## 环境 conda env `pytorch`（`D:\program file\conda\envs\pytorch`），Python 3.12， -依赖：numpy、mavsdk、pytest；stereo_ranger 仅标准库；stereo_yolo 只有真模型 -推理路径需要 ultralytics（在检测器内部延迟导入，测试与假检测器路径不受影响）。 +依赖：numpy、mavsdk(<4)、pytest、flask、pyserial +（`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 -回归、PID anti-windup C1、超龄 C4、首拍限幅 S6）+ 111 条 stereo_ranger -（合成框对驱动）+ 36 条 stereo_yolo（假检测器驱动）。 -stereo_ranger / stereo_yolo 测试均不需要真实图像、模型权重或硬件。 +**运行链路 446 条全部通过（2026-10-20）**： -## 运行演示 +| 组 | 条数 | +|---|---| +| `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 传输（本地记录指令，无实机） -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 +cd legacy/utils && PYTHONPATH="../../utils" "D:/program file/conda/envs/pytorch/python.exe" -m pytest -q ``` -控制链路时间线 13 s（20 Hz）：正常跟踪 6 s（vx 从零渐变 → 逼近限速 3 m/s）→ -目标消失 4 s（Coast 预测，60 帧后轨道判亡）→ 判零速刹车 3 s。 +运行链路测试不需要硬件（FakePod / VirtualPod / FakePx4 注入）。 -双目测距演示六场景：逼近（三道有效性门 + 滤波）→ 远场域外（方位仍发布）→ -单目退化 → 非同帧拒收 → 一致性降级/判无效 → 多目标贪心配对。 +## 运行入口 -检测前端演示五场景：单图冒烟 → 合成双目对联调（真检测框喂 stereo_ranger， -与解析式 D=F·B/Z 对照）→ 空场景 → 左右形状不一致拒收 → 帧周期计时。 -合成对用 AI 占位素材 `assets/stereo_yolo/drone_sky.png`（可整体替换为真实 -素材），本机实测 |ΔD|≈0.3 px、|Δy|≈0.2 px；帧周期 25~34 ms（640 输入， -双实例并发，随机器负载波动），与 30 fps 预算 33.3 ms 同量级，结论不可外推 -到机载平台（§5 的算力预算另见平台工程文档）。 +入口脚本在仓库根（不在 utils 内）： + +```bash +PY="D:/program file/conda/envs/pytorch/python.exe" +"$PY" task_runner_sitl.py --conn udpin://0.0.0.0:14540 # 任务全流程 SITL +"$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，非字节透传），三分支 - 映射保留 C++ A2 回归语义，详见包 2 README。 +- MAVLink 部分改为 **MAVSDK 语义映射**（offboard API，非字节透传），三分支映射保留 + 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++ 侧复现要点与接口契约硬约束（原生分辨率坐标、 - 左右预处理位级一致、`x1` 即框左边缘、无跨帧状态）见其包 README 的移植注记表。 - ⚠ 占位素材 `assets/stereo_yolo/` 为 AI 合成图，真实双目素材到位后需重跑联调。 +- ⚠ 全部数值为占位值，标定完成前不得作为飞行控制决策依据。 + +## 已归档内容 + +`legacy/utils/` 下保留了原 `stereo_yolo` / `stereo_ranger` / `target_tracker` 三包、 +175 条测试、三个 demo、`stereo_*.json`、素材与 YOLO 权重；说明见 `legacy/README.md`。 diff --git a/utils/__pycache__/conftest.cpython-312-pytest-9.1.1.pyc b/utils/__pycache__/conftest.cpython-312-pytest-9.1.1.pyc deleted file mode 100644 index a8a06ab..0000000 Binary files a/utils/__pycache__/conftest.cpython-312-pytest-9.1.1.pyc and /dev/null differ diff --git a/utils/__pycache__/demo_stereo_yolo.cpython-312.pyc b/utils/__pycache__/demo_stereo_yolo.cpython-312.pyc deleted file mode 100644 index 7794ef4..0000000 Binary files a/utils/__pycache__/demo_stereo_yolo.cpython-312.pyc and /dev/null differ diff --git a/utils/__pycache__/demo_track_control.cpython-312.pyc b/utils/__pycache__/demo_track_control.cpython-312.pyc deleted file mode 100644 index 7103de5..0000000 Binary files a/utils/__pycache__/demo_track_control.cpython-312.pyc and /dev/null differ diff --git a/utils/config/pod_bridge.json b/utils/config/pod_bridge.json index 98956c1..8e01b44 100644 --- a/utils/config/pod_bridge.json +++ b/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": { diff --git a/utils/pod_bridge/bridge.py b/utils/pod_bridge/bridge.py index e604514..38aaaec 100644 --- a/utils/pod_bridge/bridge.py +++ b/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）。""" - return self._pod_locked(now_ms) + """目标门控（公共接口，供编排层/状态机复用，A3）。 + + = 吊舱 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=锁定、且（若 link.require_laser_valid）激光测距有效，才算目标有效。 + """吊舱 bit2 门控（**不含** LOS 一致性，供内部合成本拍快照用）。 + + 表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 粘滞兜底）。未接入视觉源 - # 时 fail-safe 为 False（无法确认 → 宁可刹车）。 - if tracked and hs.require_vision: - tracked = self._vision_target_fn() if self._vision_target_fn else False + # 第二判据：LOS 运动学一致性（独立于 bit2）。 + # 本吊舱无电机、相机刚性固连机体 → 机头转 Δψ 时真实目标的脱靶量必变 ≈−Δψ； + # 机头已明显转动而 β 不动 → 角度冻结/幽灵目标 → 当丢目标处理（fail-safe）。 + # 仅在"吊舱自报锁定"期间喂样本；解锁即清历史，避免跨段污染。 + 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, diff --git a/utils/pod_bridge/config.py b/utils/pod_bridge/config.py index 9c8b84a..b4921c2 100644 --- a/utils/pod_bridge/config.py +++ b/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( - s, "heading_servo", "require_vision", c.require_vision) + c.los_consistency_enabled = _get_bool( + 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 diff --git a/utils/pod_bridge/ground_link.py b/utils/pod_bridge/ground_link.py index 3681590..081c079 100644 --- a/utils/pod_bridge/ground_link.py +++ b/utils/pod_bridge/ground_link.py @@ -1,20 +1,25 @@ -"""ground_link.py —— 地面坐标注入通道（预定流程.md「地面给坐标」）。 +"""ground_link.py —— 地面站链路（坐标注入 + 控制命令，预定流程.md「地面给坐标」）。 本吊舱无激光 → 表4 目标经纬度永不出（部署文档 §10.0），目标坐标改由地面站注入。 -本模块负责接收地面站下发的目标坐标（UDP JSON），维护一份带本地打戳的注入目标 -缓存，供 PodBridge 的 L2 地理引导**优先消费**（回退表4）。 +本模块负责接收地面站下发的两类 UDP JSON 消息： + * 目标坐标 {"lat":..,"lon":..,"alt":..} → 维护一份带本地打戳的注入目标缓存， + 供 PodBridge 的 L2 地理引导**优先消费**（回退表4）； + * 控制命令 {"cmd":""}（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]: - """解析地面站 JSON 包（纯函数，可单测）。返回不含 time_ms 的目标； - 非法 JSON / 非对象 / 缺 lat 或 lon → None（静默丢弃）。""" + def _load_object(payload: bytes) -> Optional[dict]: + """解 JSON 且要求顶层为对象（纯函数）。非法 JSON / 非对象 → 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) - if target is None: + msg = self.parse_message(data) + if msg is None: continue # 非法包静默丢弃（热路径不打日志） - target.time_ms = self._now_ms() - self._target = target + msg.time_ms = self._now_ms() + 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, - target.abs_alt_m) + msg.latitude_deg, msg.longitude_deg, + msg.abs_alt_m) finally: sock.close() diff --git a/utils/pod_bridge/los_consistency.py b/utils/pod_bridge/los_consistency.py new file mode 100644 index 0000000..68365f5 --- /dev/null +++ b/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) diff --git a/utils/pod_bridge/px4_gateway.py b/utils/pod_bridge/px4_gateway.py index 96f3c56..a876dd6 100644 --- a/utils/pod_bridge/px4_gateway.py +++ b/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() diff --git a/utils/sitl_support.py b/utils/sitl_support.py new file mode 100644 index 0000000..16698b7 --- /dev/null +++ b/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}") diff --git a/utils/stereo_ranger/__pycache__/__init__.cpython-312.pyc b/utils/stereo_ranger/__pycache__/__init__.cpython-312.pyc deleted file mode 100644 index d35fad2..0000000 Binary files a/utils/stereo_ranger/__pycache__/__init__.cpython-312.pyc and /dev/null differ diff --git a/utils/stereo_ranger/__pycache__/config.cpython-312.pyc b/utils/stereo_ranger/__pycache__/config.cpython-312.pyc deleted file mode 100644 index 9a20814..0000000 Binary files a/utils/stereo_ranger/__pycache__/config.cpython-312.pyc and /dev/null differ diff --git a/utils/stereo_ranger/__pycache__/consistency.cpython-312.pyc b/utils/stereo_ranger/__pycache__/consistency.cpython-312.pyc deleted file mode 100644 index 74faed1..0000000 Binary files a/utils/stereo_ranger/__pycache__/consistency.cpython-312.pyc and /dev/null differ diff --git a/utils/stereo_ranger/__pycache__/distance_filter.cpython-312.pyc b/utils/stereo_ranger/__pycache__/distance_filter.cpython-312.pyc deleted file mode 100644 index 0f2c03b..0000000 Binary files a/utils/stereo_ranger/__pycache__/distance_filter.cpython-312.pyc and /dev/null differ diff --git a/utils/stereo_ranger/__pycache__/geometry.cpython-312.pyc b/utils/stereo_ranger/__pycache__/geometry.cpython-312.pyc deleted file mode 100644 index 867f375..0000000 Binary files a/utils/stereo_ranger/__pycache__/geometry.cpython-312.pyc and /dev/null differ diff --git a/utils/stereo_ranger/__pycache__/matching.cpython-312.pyc b/utils/stereo_ranger/__pycache__/matching.cpython-312.pyc deleted file mode 100644 index e185f58..0000000 Binary files a/utils/stereo_ranger/__pycache__/matching.cpython-312.pyc and /dev/null differ diff --git a/utils/stereo_ranger/__pycache__/sampling.cpython-312.pyc b/utils/stereo_ranger/__pycache__/sampling.cpython-312.pyc deleted file mode 100644 index 1e465a3..0000000 Binary files a/utils/stereo_ranger/__pycache__/sampling.cpython-312.pyc and /dev/null differ diff --git a/utils/stereo_ranger/__pycache__/stereo_ranger.cpython-312.pyc b/utils/stereo_ranger/__pycache__/stereo_ranger.cpython-312.pyc deleted file mode 100644 index 70df4cc..0000000 Binary files a/utils/stereo_ranger/__pycache__/stereo_ranger.cpython-312.pyc and /dev/null differ diff --git a/utils/stereo_ranger/__pycache__/target_store.cpython-312.pyc b/utils/stereo_ranger/__pycache__/target_store.cpython-312.pyc deleted file mode 100644 index 37b17a2..0000000 Binary files a/utils/stereo_ranger/__pycache__/target_store.cpython-312.pyc and /dev/null differ diff --git a/utils/stereo_ranger/__pycache__/types.cpython-312.pyc b/utils/stereo_ranger/__pycache__/types.cpython-312.pyc deleted file mode 100644 index 60e0174..0000000 Binary files a/utils/stereo_ranger/__pycache__/types.cpython-312.pyc and /dev/null differ diff --git a/utils/stereo_ranger/__pycache__/warn_throttle.cpython-312.pyc b/utils/stereo_ranger/__pycache__/warn_throttle.cpython-312.pyc deleted file mode 100644 index 52747a4..0000000 Binary files a/utils/stereo_ranger/__pycache__/warn_throttle.cpython-312.pyc and /dev/null differ diff --git a/utils/stereo_yolo/__pycache__/__init__.cpython-312.pyc b/utils/stereo_yolo/__pycache__/__init__.cpython-312.pyc deleted file mode 100644 index 5e4c54e..0000000 Binary files a/utils/stereo_yolo/__pycache__/__init__.cpython-312.pyc and /dev/null differ diff --git a/utils/stereo_yolo/__pycache__/config.cpython-312.pyc b/utils/stereo_yolo/__pycache__/config.cpython-312.pyc deleted file mode 100644 index 147678c..0000000 Binary files a/utils/stereo_yolo/__pycache__/config.cpython-312.pyc and /dev/null differ diff --git a/utils/stereo_yolo/__pycache__/detector.cpython-312.pyc b/utils/stereo_yolo/__pycache__/detector.cpython-312.pyc deleted file mode 100644 index b85d3cc..0000000 Binary files a/utils/stereo_yolo/__pycache__/detector.cpython-312.pyc and /dev/null differ diff --git a/utils/stereo_yolo/__pycache__/frontend.cpython-312.pyc b/utils/stereo_yolo/__pycache__/frontend.cpython-312.pyc deleted file mode 100644 index 4078101..0000000 Binary files a/utils/stereo_yolo/__pycache__/frontend.cpython-312.pyc and /dev/null differ diff --git a/utils/stereo_yolo/__pycache__/sbs.cpython-312.pyc b/utils/stereo_yolo/__pycache__/sbs.cpython-312.pyc deleted file mode 100644 index 93fab62..0000000 Binary files a/utils/stereo_yolo/__pycache__/sbs.cpython-312.pyc and /dev/null differ diff --git a/utils/stereo_yolo/__pycache__/warn_throttle.cpython-312.pyc b/utils/stereo_yolo/__pycache__/warn_throttle.cpython-312.pyc deleted file mode 100644 index 681ae27..0000000 Binary files a/utils/stereo_yolo/__pycache__/warn_throttle.cpython-312.pyc and /dev/null differ diff --git a/utils/target_tracker/__pycache__/__init__.cpython-312.pyc b/utils/target_tracker/__pycache__/__init__.cpython-312.pyc deleted file mode 100644 index 92aaace..0000000 Binary files a/utils/target_tracker/__pycache__/__init__.cpython-312.pyc and /dev/null differ diff --git a/utils/target_tracker/__pycache__/kalman_box_filter.cpython-312.pyc b/utils/target_tracker/__pycache__/kalman_box_filter.cpython-312.pyc deleted file mode 100644 index f01aa75..0000000 Binary files a/utils/target_tracker/__pycache__/kalman_box_filter.cpython-312.pyc and /dev/null differ diff --git a/utils/target_tracker/__pycache__/target_tracker.cpython-312.pyc b/utils/target_tracker/__pycache__/target_tracker.cpython-312.pyc deleted file mode 100644 index 72411a1..0000000 Binary files a/utils/target_tracker/__pycache__/target_tracker.cpython-312.pyc and /dev/null differ diff --git a/utils/target_tracker/__pycache__/tracklet.cpython-312.pyc b/utils/target_tracker/__pycache__/tracklet.cpython-312.pyc deleted file mode 100644 index f15a0b9..0000000 Binary files a/utils/target_tracker/__pycache__/tracklet.cpython-312.pyc and /dev/null differ diff --git a/utils/task_runner/__init__.py b/utils/task_runner/__init__.py index 9b9897e..067ab07 100644 --- a/utils/task_runner/__init__.py +++ b/utils/task_runner/__init__.py @@ -1,9 +1,20 @@ """task_runner —— 任务编排器（预定流程.md 的天空侧任务级状态机）。 把已通的 L1（航向伺服 TrackingControlLaw）与 L2（地理引导 GuidanceLaw）按任务阶段 -编排成一条可运行的全流程：WAIT → ARM_TAKEOFF → GOTO → TRACK → APPROACH → RTL。 +编排成一条可运行的全流程： + + WAIT → ARM_TAKEOFF → GOTO → SEARCH → TRACK → RTL + + * GOTO ：L2 地理引导飞向**参考点**（地面给的坐标是搜索起点，不是终点）； + * SEARCH ：到达参考点后原地偏航扫视，靠吊舱/视觉找目标；超时未发现 → RTL； + * TRACK ：L1 角度直入 + 无距离降级（slow_approach 缓慢靠近）；超时 → RTL； + * RTL ：自定义返航（回起飞点 → 恢复初始航向 → 垂直降落），不爬高； + 异常/未起飞等情形兜底退回 PX4 原生 RTL。 模块划分： * state_machine.py 纯逻辑任务状态机（无 IO，可单测） - * runner.py 编排器（MAVSDK 飞行编排 + 状态机驱动 + PodBridge 闭环，A3） + * runner.py 编排器（MAVSDK 飞行编排 + 状态机驱动 + PodBridge 闭环） + * return_home.py 自定义返航纯逻辑（HomingController：转场/航向/高度） + * pod_control.py 真机吊舱识别/锁定指令下发（0x07/0x0F/0x0E/0x08） + * log.py 结构化任务日志（事件 JSONL + 遥测 CSV + 简报） """ diff --git a/utils/task_runner/log.py b/utils/task_runner/log.py index ef1341c..3e35552 100644 --- a/utils/task_runner/log.py +++ b/utils/task_runner/log.py @@ -6,15 +6,21 @@ * 事件日志（JSONL）：状态机转移、飞行编排事件（解锁/起飞/返航）、异常/紧急。 低频、带语义、结构化，便于 grep / jq 定位「什么时候发生了什么」。 * 周期遥测日志（CSV）：每控制周期的位姿 / GPS / 目标 / 控制输出，便于回放与画图。 + * 简洁任务简报（task_brief.log，纯文本）：一行一事件（阶段切换/地面站命令/ + 飞控告警）+ 运行结束汇总（时长/最高/最近距目标/状态链），供人工阅读与归档； + 机器日志（JSONL/CSV）格式不变。 时间戳：单调毫秒（now_ms，与运行时代码同源）+ 相对时间 t_s（自首个 now_ms 起）。 -文件 IO 集中在 open / close，热路径只追加一行，不做 flush 决策。 +文件 IO 集中在 open / close；热路径只追加（并立即 flush 落盘——缓冲期间文件不可见， +地面站页面运行中需要读到最新状态与遥测，2026-10-14 实测）。 """ import csv import json +import math import time from dataclasses import dataclass +from datetime import datetime from pathlib import Path from typing import Optional @@ -60,13 +66,26 @@ class TaskLogConfig: dir: str = "logs" # 输出目录（相对工作目录） event_file: str = "task_events.jsonl" # 事件日志 telemetry_file: str = "task_telemetry.csv" # 周期遥测日志 + brief_file: str = "task_brief.log" # 人类可读任务简报（追加） telemetry_every_n: int = 1 # 每 N 周期记一次遥测（1 = 每周期） + telemetry_flush_ms: int = 1000 # 遥测落盘间隔（毫秒）；0 = 每行立即 flush def _monotonic_ms() -> int: return int(time.monotonic() * 1000) +_STATE_CN = {"wait": "待命", "arm_takeoff": "起飞", "goto": "坐标引导", + "search": "搜索", "track": "追踪靠近", "approach": "接近悬停", + "rtl": "返航"} + + +def _fmt_t_s(t_s: float) -> str: + """相对时间 → mm:ss.s（简报用）。""" + m, s = divmod(max(0.0, float(t_s or 0.0)), 60.0) + return f"{int(m):02d}:{s:04.1f}" + + class TaskLogger: """结构化任务日志。用法： @@ -86,12 +105,20 @@ class TaskLogger: self._event_fp = None self._tel_fp = None self._tel_writer = None + self._brief_fp = None self._t0_ms: Optional[int] = None self._tick = 0 + self._last_tel_flush_ms: Optional[int] = None # 遥测上次落盘时刻 + # 简报汇总统计（随遥测行/事件更新，close 时写一行汇总） + self._n_events = 0 + self._last_t_s = 0.0 + self._max_rel_alt_m: Optional[float] = None + self._min_dist_m: Optional[float] = None + self._state_chain: list = [] # --- 生命周期 ----------------------------------------------------------- def open(self) -> None: - """创建输出目录并打开两个日志文件；disabled 时跳过。""" + """创建输出目录并打开日志文件；disabled 时跳过。""" if not self._cfg.enabled: return out_dir = Path(self._cfg.dir) @@ -103,11 +130,22 @@ class TaskLogger: self._tel_writer = csv.writer(self._tel_fp) if (out_dir / self._cfg.telemetry_file).stat().st_size == 0: self._tel_writer.writerow(_TELEMETRY_FIELDS) + self._brief_fp = (out_dir / self._cfg.brief_file).open( + "a", encoding="utf-8") + self._brief_fp.write( + f"===== 任务开始 {datetime.now().strftime('%Y-%m-%d %H:%M:%S')} =====\n") + self._last_tel_flush_ms = None self._event_fp.flush() self._tel_fp.flush() + self._brief_fp.flush() def close(self) -> None: - """flush 并关闭两个日志文件。""" + """写简报汇总并关闭全部日志文件。""" + if self._brief_fp is not None: + self._brief_fp.write(self._brief_summary() + "\n\n") + self._brief_fp.flush() + self._brief_fp.close() + self._brief_fp = None if self._event_fp is not None: self._event_fp.flush() self._event_fp.close() @@ -120,16 +158,32 @@ class TaskLogger: # --- 记录 --------------------------------------------------------------- def log_event(self, now_ms: int, event_type: str, **fields) -> None: - """追加一条 JSONL 事件。""" + """追加一条 JSONL 事件并**立即落盘**（事件低频且语义关键：地面站要实时看阶段）。 + + 注意：遥测行不在这里 flush——见 log_telemetry 的节流落盘说明。 + """ if self._event_fp is None: return record = {"type": event_type, "ts_ms": now_ms, "t_s": self._rel_s(now_ms)} record.update(fields) self._event_fp.write(json.dumps(record, ensure_ascii=False) + "\n") + self._event_fp.flush() + self._n_events += 1 + if self._brief_fp is not None: + self._brief_fp.write(self._brief_text(record) + "\n") + self._brief_fp.flush() def log_telemetry(self, now_ms: int, rec: TelemetryRecord) -> None: - """追加一行遥测（按 telemetry_every_n 节流）。""" + """追加一行遥测（按 telemetry_every_n 节流）。 + + 落盘策略：**写入靠 Python 缓冲、按 telemetry_flush_ms 定期 flush**， + 而不是每行 fsync 式 flush。原因：本方法在主控制循环里以 20Hz 调用， + 每行都 flush 等于每秒 20 次同步磁盘 IO，在树莓派 SD 卡上会阻塞事件 + 循环（实测 MAVSDK 回调队列因此积压告警）；事件行仍即时落盘，故页面 + 的阶段显示不受影响，只有图表数据最多滞后 telemetry_flush_ms。 + close() 会做最后一次 flush，进程正常退出不丢数据。 + """ if self._tel_writer is None: return self._tick += 1 @@ -137,8 +191,75 @@ class TaskLogger: return rec.t_s = self._rel_s(now_ms) self._tel_writer.writerow([getattr(rec, f) for f in _TELEMETRY_FIELDS]) + self._flush_telemetry(now_ms) + # 简报汇总统计（只随真正落盘的行更新） + self._last_t_s = rec.t_s + if math.isfinite(rec.relative_altitude_m): + if (self._max_rel_alt_m is None + or rec.relative_altitude_m > self._max_rel_alt_m): + self._max_rel_alt_m = rec.relative_altitude_m + dist = rec.distance_to_target_m + if math.isfinite(dist) and dist > 0: + if self._min_dist_m is None or dist < self._min_dist_m: + self._min_dist_m = dist + if rec.state and (not self._state_chain + or self._state_chain[-1] != rec.state): + if len(self._state_chain) < 32: + self._state_chain.append(rec.state) + + # --- 简报（人类可读，一行一事件）---------------------------------------- + @staticmethod + def _brief_text(record: dict) -> str: + t = _fmt_t_s(record.get("t_s", 0.0)) + etype = record.get("type", "?") + if etype == "state": + to = _STATE_CN.get(record.get("to_state"), + record.get("to_state", "?")) + if record.get("from_state") == record.get("to_state"): + return f"[{t}] 任务启动（当前: {to}）" + frm = _STATE_CN.get(record.get("from_state"), + record.get("from_state", "?")) + return f"[{t}] 阶段: {frm} → {to}（{record.get('reason', '')}）" + if etype == "command": + return f"[{t}] 地面站命令: {record.get('name', '')}" + if etype == "fc_status": + return (f"[{t}] 飞控 {record.get('severity', '')}: " + f"{str(record.get('text', '')).strip()}") + if etype == "takeoff": + return f"[{t}] 起飞至 {record.get('altitude_m', '?')} m" + if etype == "takeoff_failed": + return f"[{t}] 起飞失败: {record.get('error', '')}" + if etype == "rtl": + return f"[{t}] 开始返航" + fields = [f"{k}={v}" for k, v in record.items() + if k not in ("type", "ts_ms", "t_s")] + return f"[{t}] {etype}: {', '.join(fields)}" + + def _brief_summary(self) -> str: + parts = [f"时长 {self._last_t_s:.1f}s"] + if self._max_rel_alt_m is not None: + parts.append(f"最高 {self._max_rel_alt_m:.1f}m") + if self._min_dist_m is not None: + parts.append(f"最近距目标 {self._min_dist_m:.1f}m") + parts.append(f"事件 {self._n_events} 条") + if self._state_chain: + parts.append("状态链 " + "→".join( + _STATE_CN.get(s, s) for s in self._state_chain)) + return "----- 本次运行汇总: " + " · ".join(parts) + " -----" # --- 内部 --------------------------------------------------------------- + def _flush_telemetry(self, now_ms: int) -> None: + """按 telemetry_flush_ms 节流落盘遥测（0 = 每行立即落盘）。""" + interval = self._cfg.telemetry_flush_ms + if interval <= 0: + self._tel_fp.flush() + self._last_tel_flush_ms = now_ms + return + if (self._last_tel_flush_ms is None + or now_ms - self._last_tel_flush_ms >= interval): + self._tel_fp.flush() + self._last_tel_flush_ms = now_ms + def _rel_s(self, now_ms: int) -> float: if self._t0_ms is None: self._t0_ms = now_ms diff --git a/utils/task_runner/pod_control.py b/utils/task_runner/pod_control.py index d161d88..ecff9d2 100644 --- a/utils/task_runner/pod_control.py +++ b/utils/task_runner/pod_control.py @@ -8,7 +8,8 @@ * 0x08 目标识别关（真正退出识别） 本模块把这些指令序列封装成 PodController，供 TaskRunner 在 GOTO / RTL 阶段调用。 -SITL 用 VirtualPod（直接合成锁定态），不注入 PodController。 +另含地面站手动命令（stop_track / auto_lock / center），与 pod_console.py 按键 +u / a / c 语义对齐。SITL 用 VirtualPod（直接合成锁定态），不注入 PodController。 """ from pod_bridge.protocol import ControlCode, ControlCommand @@ -37,3 +38,18 @@ class PodController: """停止跟踪 + 关闭目标识别（RTL 前调用）。""" self._bridge.send_control(ControlCommand(code=ControlCode.STOP_TRACK)) self._bridge.send_control(ControlCommand(code=ControlCode.TARGET_DETECT_OFF)) + + def stop_track(self) -> None: + """停止跟踪（只停当前这一次，0x0E；0x0F 常驻自动锁定仍生效）。""" + self._bridge.send_control(ControlCommand(code=ControlCode.STOP_TRACK)) + + def auto_lock(self) -> None: + """发现目标自动锁定 + 识别转跟踪（0x0F + 0x10）。""" + self._bridge.send_control(ControlCommand( + code=ControlCode.AUTO_LOCK_ON_FOUND)) + self._bridge.send_control(ControlCommand( + code=ControlCode.DETECT_TO_TRACK)) + + def center(self) -> None: + """手动归中（0x2B）。""" + self._bridge.send_control(ControlCommand(code=ControlCode.CENTER)) diff --git a/utils/task_runner/return_home.py b/utils/task_runner/return_home.py new file mode 100644 index 0000000..3e1deca --- /dev/null +++ b/utils/task_runner/return_home.py @@ -0,0 +1,130 @@ +"""return_home.py —— 自定义返航（回起飞点 + 恢复初始航向 + 就地降落）的纯逻辑控制。 + +背景：PX4 自带的 RTL 会先爬到 `RTL_RETURN_ALT`（默认数十米）再飞回、再下降到 +`RTL_DESCEND_ALT` 后落地——即"飞很高再降落"。本模块给出替代方案： + + RETURN 阶段：保持在**转场高度**（取进入返航时的高度与起飞高度的较大者， + 绝不比任务高度更高）飞向起飞点，机头朝目标方向； + YAW 阶段 ：到位后原地把机头转回**起飞时刻的航向**； + LAND 阶段 ：交给飞控 `land()` 垂直降落（不爬升）。 + +设计约束（与项目铁律一致）： + * 纯逻辑：输入为载机位置/航向/相对高度，输出速度与角速度意图，不碰 IO， + 可确定性单测；实际下发由 runner 负责。 + * NED 语义：vz 下正。目标高度高于当前 → vz 为负（爬升）。 + * 只做"航位推算"级别的控制：无距离传感器时不需要它（返航点是已知识别坐标）， + 因此这里用比例控制 + 限幅即可，不引入积分项（避免返航中积分饱和）。 +""" + +import math +from dataclasses import dataclass + +from pod_bridge.guidance import haversine_distance_m, initial_bearing_deg +from visual_track_control.rate_limiter import clamp + + +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 ReturnConfig: + """自定义返航参数（全外置；数值为占位，须按机体/场地标定）。""" + + enabled: bool = True # False → 退回 PX4 原生 RTL + speed_mps: float = 2.0 # 转场水平速度上限 + arrival_radius_m: float = 3.0 # 距起飞点该半径内视为"已回到起飞点" + timeout_s: float = 120.0 # 转场阶段超时（超时 → 退回 PX4 RTL 兜底） + yaw_kp: float = 1.0 # 航向误差 → 偏航角速度（dps/deg） + yaw_rate_limit_dps: float = 20.0 # 偏航角速度上限 + align_kp: float = 1.0 # 恢复初始航向阶段的比例增益 + align_rate_limit_dps: float = 15.0 + yaw_align_tolerance_deg: float = 5.0 # 航向对齐容差 + yaw_align_timeout_s: float = 15.0 # 航向对齐超时（超时也进入降落） + alt_kp: float = 0.5 # 高度误差 → vz（m/s per m） + alt_limit_mps: float = 0.5 # 垂直速度上限（与 L1 vertical 限速同量级） + + +@dataclass +class HomingOutput: + """单周期归航意图（机体系前向 + 垂直 NED + 偏航角速度）。""" + + vx_mps: float = 0.0 + vz_mps: float = 0.0 + yaw_rate_dps: float = 0.0 + distance_m: float = math.inf + bearing_err_deg: float = 0.0 + arrived: bool = False + + +class HomingController: + """把载机当前位置/航向/高度 → 飞回起飞点的速度与航向意图。 + + 用法：进入返航时用起飞点经纬度、起飞时刻航向、转场高度构造一次； + 之后每周期 `update(lat, lon, yaw_rad, rel_alt_m)`。 + """ + + def __init__(self, cfg: ReturnConfig, home_lat_deg: float, + home_lon_deg: float, initial_yaw_rad: float, + transit_alt_m: float) -> None: + self._cfg = cfg + self._home_lat = float(home_lat_deg) + self._home_lon = float(home_lon_deg) + self._initial_yaw_rad = float(initial_yaw_rad) + self._transit_alt_m = float(transit_alt_m) + + @property + def home(self) -> tuple[float, float]: + return self._home_lat, self._home_lon + + @property + def initial_yaw_rad(self) -> float: + return self._initial_yaw_rad + + def update(self, lat_deg: float, lon_deg: float, yaw_rad: float, + rel_alt_m: float) -> HomingOutput: + """转场阶段单周期意图：朝起飞点飞，同时保持转场高度。""" + cfg = self._cfg + dist = haversine_distance_m(lat_deg, lon_deg, + self._home_lat, self._home_lon) + bearing = initial_bearing_deg(lat_deg, lon_deg, + self._home_lat, self._home_lon) + err = wrap_deg_180(bearing - math.degrees(yaw_rad)) + + yaw_rate = clamp(cfg.yaw_kp * err, + -cfg.yaw_rate_limit_dps, cfg.yaw_rate_limit_dps) + arrived = dist <= cfg.arrival_radius_m + # 对准门控：没对准先转，避免斜着"蹭"过去（与 L2 引导同思路） + alignment = max(0.0, math.cos(math.radians(err))) + vx = 0.0 if arrived else cfg.speed_mps * alignment + return HomingOutput( + vx_mps=vx, + vz_mps=self.alt_hold_vz(rel_alt_m), + yaw_rate_dps=yaw_rate, + distance_m=dist, + bearing_err_deg=err, + arrived=arrived, + ) + + def alt_hold_vz(self, rel_alt_m: float) -> float: + """保持转场高度（NED 下正）：低于目标 → 负值（爬升），高于 → 正值（下降）。""" + cfg = self._cfg + return clamp(cfg.alt_kp * (rel_alt_m - self._transit_alt_m), + -cfg.alt_limit_mps, cfg.alt_limit_mps) + + def yaw_align(self, yaw_rad: float) -> tuple[float, float]: + """恢复初始航向：返回 (航向误差 deg, 偏航角速度 dps)。""" + cfg = self._cfg + err = wrap_deg_180(math.degrees(self._initial_yaw_rad) + - math.degrees(yaw_rad)) + rate = clamp(cfg.align_kp * err, + -cfg.align_rate_limit_dps, cfg.align_rate_limit_dps) + return err, rate + + def yaw_aligned(self, yaw_rad: float) -> bool: + err, _ = self.yaw_align(yaw_rad) + return abs(err) <= self._cfg.yaw_align_tolerance_deg diff --git a/utils/task_runner/runner.py b/utils/task_runner/runner.py index a5b997c..6d093aa 100644 --- a/utils/task_runner/runner.py +++ b/utils/task_runner/runner.py @@ -7,6 +7,8 @@ L2（地理引导 GuidanceLaw）按任务阶段编排，配合地面坐标注入 职责边界： * 状态机（TaskStateMachine）只做「状态 + 转移判据」决策； * 本编排器负责「采集快照 → 喂状态机 → 按状态执行动作」； + * 地面站命令（RTL/紧急/吊舱手动指令，经 GroundLink 队列）主循环逐拍分派； + * 飞控状态文本（STATUSTEXT，警告/异常）逐拍落任务日志（fc_status 事件）； * 闭环控制复用 PodBridge（L1/L2 运行时切换走 set_heading_servo_enabled）； * 飞行编排（解锁/起飞/返航）通过 duck-typed `drone`（MAVSDK System）完成， 便于 mock 单测；真实 SITL 装配见仓库根 task_runner_sitl.py。 @@ -23,13 +25,38 @@ from typing import Callable, Optional from pod_bridge.guidance import haversine_distance_m from visual_track_control.staleness_checker import is_stale +from visual_track_control.types import ControlMode, ControlOutput from .log import TaskLogger, TelemetryRecord +from .return_home import HomingController, ReturnConfig from .state_machine import (TaskInputs, TaskState, TaskStateConfig, TaskStateMachine) logger = logging.getLogger(__name__) +# 自定义返航的阶段 +_RTL_RETURN = "return" # 转场：飞回起飞点，保持转场高度 +_RTL_YAW = "yaw" # 到位：把机头转回初始航向 +_RTL_LAND = "land" # 降落：交给飞控垂直降落 +_RTL_FALLBACK = "fallback" # 兜底：退回 PX4 原生 RTL（会爬升，仅异常时使用） +_RTL_IDLE = "idle" + +# 地面站吊舱命令 → PodController 方法（与 ground_station/app.py 白名单对齐） +_POD_COMMAND_METHODS = { + "pod_detect_on": "arm_detection", + "pod_auto_lock": "auto_lock", + "pod_stop_track": "stop_track", + "pod_detect_off": "stop_tracking", + "pod_center": "center", +} + +# 搜索扫视换向周期计数中「向右」的半周期序号（0 → 右，1 → 左，交替） +_SWEEP_RIGHT, _SWEEP_LEFT = 0, 1 + +# RTL 落地判定：单次等待 in_air 遥测的上限（秒）。MAVSDK 遥测流挂起时 +# `async for` 会永久阻塞并卡死主循环，故必须有兜底（超时 → 下一拍重试）。 +_IN_AIR_TIMEOUT_S = 5.0 + def _monotonic_ms() -> int: return int(time.monotonic() * 1000) @@ -45,14 +72,14 @@ class TaskRunner: sm TaskStateMachine —— update() ground GroundLink —— latest()（地面注入坐标） drone MAVSDK System —— action.arm/takeoff/return_to_launch/land/disarm - vision_fn callable->bool —— 我方视觉第二判据（可选，require_vision 时用） """ def __init__(self, cfg, sm_cfg: TaskStateConfig, px4, bridge, sm: TaskStateMachine, - ground, drone, vision_fn: Optional[Callable[[], bool]] = None, + ground, drone, now_ms_fn: Callable[[], int] = _monotonic_ms, logger: Optional[TaskLogger] = None, - pod_control=None) -> None: + pod_control=None, land_instead_of_rtl: bool = False, + return_cfg: Optional[ReturnConfig] = None) -> None: self._cfg = cfg self._sm_cfg = sm_cfg self._px4 = px4 @@ -60,14 +87,32 @@ class TaskRunner: self._sm = sm self._ground = ground self._drone = drone - self._vision_fn = vision_fn self._now = now_ms_fn self._logger = logger # 结构化任务日志（可选，B 阶段） self._pod_control = pod_control # 吊舱识别/锁定控制（可选，真机） + self._land_instead_of_rtl = land_instead_of_rtl # 收尾改用就地降落 + self._return_cfg = return_cfg or ReturnConfig() self._centered_since_ms: Optional[int] = None self._rtl_requested = False self._emergency = False self._rtl_done = False + # SEARCH 扫视状态：当前扫向（右/左）与本次扫向的起始时刻 + self._sweep_dir = _SWEEP_RIGHT + self._sweep_since_ms: Optional[int] = None + self._pod_detection_on = False # 吊舱识别/自动锁定只开一次（防重发 0x07） + self._takeoff_done = False # 解锁/起飞只执行一次（防空中重复起飞） + self._in_air_stream = None # in_air 遥测流复用（避免每拍重新订阅） + self._rtl_entered_ms: Optional[int] = None # 进入 RTL 时刻（落地等待上限用） + # --- 自定义返航（回起飞点 → 恢复初始航向 → 降落）--- + self._home_lat: Optional[float] = None # 起飞点纬度（解锁时刻记录） + self._home_lon: Optional[float] = None + self._initial_yaw_rad: Optional[float] = None # 起飞时刻航向 + self._bridge_started = False # 是否已进入 offboard（未起飞则不能自主返航） + self._rtl_phase = _RTL_IDLE + self._return_ctrl: Optional[HomingController] = None + self._return_started_ms: Optional[int] = None + self._yaw_started_ms: Optional[int] = None + self._land_cmd_sent = False # --- 外部控制 ----------------------------------------------------------- def request_rtl(self) -> None: @@ -78,6 +123,43 @@ class TaskRunner: """外部触发紧急终止（丢控 / RC 接管 / 失控）。""" self._emergency = True + # --- 地面站命令分派 ----------------------------------------------------- + def _apply_ground_commands(self) -> None: + """排空地面站命令队列并逐条分派（主循环每拍调用）。""" + for name in self._ground.drain_commands(): + self._apply_command(name) + + def _apply_command(self, name: str) -> None: + """分派一条地面站命令；未知命令名忽略并警告。""" + if name == "rtl": + self.request_rtl() + elif name == "emergency": + self.trigger_emergency() + elif name in _POD_COMMAND_METHODS: + if self._pod_control is None: + logger.warning("[Task] 地面站命令 %s 被忽略：未配置吊舱控制（SITL）", + name) + return + try: + getattr(self._pod_control, _POD_COMMAND_METHODS[name])() + except Exception as exc: + logger.warning("[Task] 吊舱命令 %s 执行失败: %s", name, exc) + return + else: + logger.warning("[Task] 未知地面站命令: %s", name) + return + logger.info("[Task] 地面站命令: %s", name) + if self._logger is not None: + self._logger.log_event(self._now(), "command", name=name) + + def _log_fc_messages(self) -> None: + """把飞控状态文本（STATUSTEXT）落任务日志并输出控制台（每拍调用）。""" + for sev, text in self._px4.take_fc_messages(): + logger.info("[FC] %s: %s", sev, text) + if self._logger is not None: + self._logger.log_event(self._now(), "fc_status", + severity=sev, text=text) + @property def state(self) -> TaskState: return self._sm.state @@ -92,24 +174,29 @@ class TaskRunner: await self._px4.start_telemetry() # 幂等：先拿遥测，供起飞到位/距离计算 if self._logger is not None: self._logger.open() + # 记初始状态：页面「任务状态」取日志最后一个 state 事件，缺此行会显示上次旧状态 + self._logger.log_event(self._now(), "state", + from_state=self._sm.state.value, + to_state=self._sm.state.value, + reason="任务启动") period_s = 1.0 / self._sm_cfg.loop_frequency_hz try: while not stop_event.is_set(): now_ms = self._now() - inputs = self._collect_inputs(now_ms) - tr = self._sm.update(inputs, now_ms) - if tr.occurred: - logger.info("[Task] %s -> %s (%s)", - tr.from_state.value, tr.to_state.value, tr.reason) + try: + await self._iteration(now_ms) + except asyncio.CancelledError: + raise + except Exception as exc: + # 单周期异常不得让任务带着 offboard 指令静默停摆：转紧急 → 下一拍 + # 状态机转 RTL（与 bridge.run() 的按周期护栏同思路，但这里必须 + # 升级为返航，因为再没有别的循环会接管飞机）。 + logger.error("[Task] 任务周期异常，转紧急返航: %s", exc) + self._emergency = True if self._logger is not None: - self._logger.log_event( - now_ms, "state", from_state=tr.from_state.value, - to_state=tr.to_state.value, reason=tr.reason) - await self._on_enter(tr.to_state, now_ms) - out = await self._tick(now_ms) - if self._logger is not None: - self._logger.log_telemetry( - now_ms, self._build_telemetry_record(now_ms, inputs, out)) + self._logger.log_event(self._now(), "iteration_error", + error=f"{type(exc).__name__}: {exc}") + await self._safe_abort() if self._finished(): break await asyncio.sleep(period_s) @@ -118,10 +205,52 @@ class TaskRunner: if self._logger is not None: self._logger.close() + async def _iteration(self, now_ms: int) -> None: + """单周期：命令 → 快照 → 状态机 → 进入动作 → 持续动作 → 日志。""" + self._apply_ground_commands() # 地面站命令先于快照采集生效 + self._log_fc_messages() + inputs = self._collect_inputs(now_ms) + tr = self._sm.update(inputs, now_ms) + if tr.occurred: + logger.info("[Task] %s -> %s (%s)", + tr.from_state.value, tr.to_state.value, tr.reason) + if self._logger is not None: + self._logger.log_event( + now_ms, "state", from_state=tr.from_state.value, + to_state=tr.to_state.value, reason=tr.reason) + await self._on_enter(tr.to_state, now_ms) + out = await self._tick(now_ms) + if self._logger is not None: + self._logger.log_telemetry( + now_ms, self._build_telemetry_record(now_ms, inputs, out)) + + async def _safe_abort(self) -> None: + """异常兜底：直接进入 RTL 流程（不依赖状态机下一拍转移）。 + + 异常可能来自采集/控制律/日志任意环节，下一拍未必能正常跑完转移；因此在 + 兜底里显式走一次 RTL 进入动作（停机 + 返航），保证飞机不会停在 offboard + 速度指令上。 + """ + if self._sm.state is TaskState.RTL: + return + self._sm.force_state(TaskState.RTL, self._now()) + self._rtl_entered_ms = self._now() + if self._pod_control is not None: + try: + self._pod_control.stop_tracking() + except Exception: + pass + try: + # 兜底不走自定义返航：异常后不再信任自身状态/定位，直接交给 PX4 原生 RTL + await self._begin_rtl(self._now(), force_fallback=True) + except Exception as exc: + logger.error("[Task] 异常兜底返航失败: %s", exc) + # --- 输入采集 ----------------------------------------------------------- def _collect_inputs(self, now_ms: int) -> TaskInputs: """聚合 PX4 / 注入坐标 / 吊舱 / 视觉为一份 TaskInputs 快照。""" own = self._px4.own_state() + self._maybe_record_home(own) # 起飞阶段定位可能晚到：每拍补记一次 injected = self._ground.latest() target_injected = bool( @@ -135,10 +264,12 @@ class TaskRunner: own.latitude_deg, own.longitude_deg, injected.latitude_deg, injected.longitude_deg) + # 起飞到达判据带容差：悬停/高度估计通常略低于设定目标（实测 SITL 4.99m vs 5.0m）， + # 无容差会差厘米误判"起飞超时"→ 真机上更明显 takeoff_reached = (own.relative_altitude_m - >= self._sm_cfg.takeoff_altitude_m) + >= self._sm_cfg.takeoff_altitude_m + - self._sm_cfg.takeoff_reached_tolerance_m) target_locked = self._bridge.pod_locked(now_ms) - vision = bool(self._vision_fn and self._vision_fn()) centered, centered_s = self._update_centered(now_ms) return TaskInputs( @@ -146,7 +277,6 @@ class TaskRunner: distance_to_target_m=distance, takeoff_reached=takeoff_reached, target_locked=target_locked, - vision_has_target=vision, target_centered=centered, centered_duration_s=centered_s, rtl_requested=self._rtl_requested, @@ -174,6 +304,12 @@ class TaskRunner: # --- 状态进入动作 ------------------------------------------------------- async def _on_enter(self, state: TaskState, now_ms: int) -> None: + """进入状态的**一次性**动作。 + + 注意：这些动作只在「首次」进入时执行——任务中途可能需要多次经过 GOTO/ + SEARCH（丢目标回搜），重复 `offboard.start()` / `arm_detection()` + （0x07 目标识别开会打断正在进行的跟踪）会造成真机自激，故以标志位守卫。 + """ if state is TaskState.ARM_TAKEOFF: await self._arm_takeoff() elif state is TaskState.GOTO: @@ -181,17 +317,50 @@ class TaskRunner: logger.error("[Task] offboard 启动失败，紧急终止") self._emergency = True return + self._bridge_started = True self._bridge.set_heading_servo_enabled(False) # L2 地理引导 - if self._pod_control is not None: - self._pod_control.arm_detection() # 真机：开启目标识别 + 自动锁定 + self._start_pod_detection() # 首次：开识别 + 自动锁定 + elif state is TaskState.SEARCH: + self._bridge.set_heading_servo_enabled(False) # 搜索不用控制律 + self._sweep_dir = _SWEEP_RIGHT # 每次进入从右向起扫 + self._sweep_since_ms = now_ms elif state is TaskState.TRACK: - self._bridge.set_heading_servo_enabled(True) # L1 航向伺服 + self._bridge.set_heading_servo_enabled(True) # L1 视觉追踪 + 缓慢靠近 elif state is TaskState.APPROACH: pass # 悬停由 _tick 持续发零速 elif state is TaskState.RTL: + self._rtl_entered_ms = now_ms if self._pod_control is not None: - self._pod_control.stop_tracking() # 真机：停止跟踪 + 识别关（bridge 停机前） - await self._start_rtl() + self._pod_control.stop_tracking() # 真机：停止跟踪 + 识别关 + await self._begin_rtl(now_ms) + + def _start_pod_detection(self) -> None: + """首次到达引导/搜索阶段时开启吊舱目标识别 + 自动锁定（只发一次）。""" + if self._pod_control is None or self._pod_detection_on: + return + self._pod_detection_on = True + self._pod_control.arm_detection() + + # --- SEARCH 扫视 -------------------------------------------------------- + def _search_yaw_rate(self, now_ms: int) -> float: + """SEARCH 扫视角速度：按 sweep_reverse_s 交替左右，0 = 单向扫视。 + + 吊舱相机刚性固连机体（无电机、不独立转动），故只能靠机头旋转改变观察 + 方向；扫视期间零线速度，飞机保持悬停在参考点附近。 + """ + rate = self._sm_cfg.sweep_rate_dps + if rate == 0.0: + return 0.0 + period_s = self._sm_cfg.sweep_reverse_s + if period_s > 0.0: + if self._sweep_since_ms is None: + self._sweep_since_ms = now_ms + elif (now_ms - self._sweep_since_ms) >= period_s * 1000.0: + self._sweep_since_ms = now_ms + self._sweep_dir = (_SWEEP_LEFT if self._sweep_dir == _SWEEP_RIGHT + else _SWEEP_RIGHT) + sign = 1.0 if self._sweep_dir == _SWEEP_RIGHT else -1.0 + return sign * rate # --- 每周期持续动作 ----------------------------------------------------- async def _tick(self, now_ms: int): @@ -199,10 +368,13 @@ class TaskRunner: state = self._sm.state if state in (TaskState.GOTO, TaskState.TRACK): return await self._bridge.step(now_ms) + elif state is TaskState.SEARCH: + # 原地偏航扫视（同时 poll 吊舱，保遥测新鲜，否则搜到也切不进 TRACK） + await self._bridge.search_step(now_ms, self._search_yaw_rate(now_ms)) elif state is TaskState.APPROACH: await self._px4.send_brake_hover(now_ms) # 悬停目标上空（保 offboard 心跳） elif state is TaskState.RTL: - await self._check_landed() + await self._tick_rtl(now_ms) return None # WAIT / ARM_TAKEOFF：无持续动作（待命 / 等起飞高度） @@ -238,6 +410,17 @@ class TaskRunner: # --- 飞行编排 ----------------------------------------------------------- async def _arm_takeoff(self) -> None: + """解锁 + 起飞到悬停高度（每次任务只执行一次）。 + + 重复执行会导致：机已在空中时再次 arm/takeoff → PX4 拒绝甚至触发保护， + 且日志看起来像"重新起飞"。故用标志位守卫，重复进入只告警不改动飞机。 + """ + if self._takeoff_done: + logger.warning("[Task] 已在飞行中，忽略重复的解锁/起飞请求") + return + self._takeoff_done = True + # 解锁前记录起飞点与初始航向（自定义返航要用；PX4 的 home 此时已固定） + self._record_home() try: await self._drone.action.arm() await self._drone.action.set_takeoff_altitude( @@ -254,9 +437,175 @@ class TaskRunner: self._logger.log_event(self._now(), "takeoff_failed", error=str(exc)) - async def _start_rtl(self) -> None: + def _record_home(self) -> bool: + """尝试记录起飞点与初始航向；成功返回 True（可重复调用，只记一次）。""" + if self._home_lat is not None: + return True try: - await self._bridge.stop() # 先零速刹车 + 停 offboard/遥测/串口 + own = self._px4.own_state() + except Exception as exc: # 遥测不可用时不阻塞起飞 + logger.warning("[Task] 读取起飞点失败: %s", exc) + return False + return self._maybe_record_home(own) + + def _maybe_record_home(self, own) -> bool: + """在**起飞前/起飞中**用首个有效定位记下起飞点与初始航向。 + + ⚠ SITL 实测踩到的坑：解锁发生在地面站坐标到达后约 0.3s，此时 `gps_info` + （2Hz）可能还没到，`position_valid` 仍为 False——若只在解锁那一刻记一次， + 起飞点就会丢失、自定义返航只能退回 PX4 RTL。故起飞阶段每拍补记。 + 只接受 WAIT / ARM_TAKEOFF 阶段的定位：进入 GOTO 后飞机已在空中， + 那时的位置不该再当作"起飞点"。 + """ + if self._home_lat is not None: + return True + if not own.position_valid: + return False + if self._sm.state not in (TaskState.WAIT, TaskState.ARM_TAKEOFF): + return False + self._home_lat = own.latitude_deg + self._home_lon = own.longitude_deg + self._initial_yaw_rad = own.yaw_rad + logger.info("[Task] 起飞点已记录: (%.7f, %.7f) 初始航向 %.1f°", + self._home_lat, self._home_lon, + math.degrees(self._initial_yaw_rad)) + return True + + # --- 返航（自定义：回起飞点 → 恢复初始航向 → 降落）--------------------- + def _custom_return_possible(self) -> bool: + """能否走自定义返航（已起飞 + 有起飞点 + 位置有效）。""" + if not self._return_cfg.enabled: + return False + if not self._bridge_started: + return False # 没进过 offboard：还在待命/起飞段 + if self._home_lat is None or self._home_lon is None: + return False + if self._initial_yaw_rad is None: + return False + try: + return bool(self._px4.own_state().position_valid) + except Exception: + return False + + async def _begin_rtl(self, now_ms: int, force_fallback: bool = False) -> None: + """进入返航：选择「自定义返航」或「就地降落」或「PX4 RTL 兜底」。""" + if self._land_instead_of_rtl: + self._rtl_phase = _RTL_LAND + await self._start_land() + return + if force_fallback or not self._custom_return_possible(): + reason = "异常兜底" if force_fallback else "未起飞/无起飞点" + logger.info("[Task] 使用 PX4 原生 RTL（%s）", reason) + await self._fallback_rtl() + return + own = self._px4.own_state() + # 转场高度：取"当前高度"与"起飞高度"的较大者 —— 不低于任务高度， + # 也不会为了返航额外爬高（这正是原生 RTL 会做的事）。 + transit_alt = max(own.relative_altitude_m, + self._sm_cfg.takeoff_altitude_m) + self._return_ctrl = HomingController( + self._return_cfg, self._home_lat, self._home_lon, + self._initial_yaw_rad, transit_alt) + self._return_started_ms = now_ms + self._rtl_phase = _RTL_RETURN + logger.info("[Task] 自定义返航：回起飞点 (%.7f, %.7f)，转场高度 %.1fm，" + "到位后恢复航向 %.1f° 并降落", + self._home_lat, self._home_lon, transit_alt, + math.degrees(self._initial_yaw_rad)) + if self._logger is not None: + self._logger.log_event( + now_ms, "return_home", home_lat=self._home_lat, + home_lon=self._home_lon, transit_alt_m=transit_alt, + initial_yaw_deg=math.degrees(self._initial_yaw_rad)) + + async def _return_step(self, now_ms: int) -> None: + """转场阶段：朝起飞点飞，保持转场高度。""" + cfg = self._return_cfg + if (self._return_started_ms is not None + and now_ms - self._return_started_ms >= cfg.timeout_s * 1000.0): + logger.warning("[Task] 返航转场超时 %.0fs，退回 PX4 RTL", cfg.timeout_s) + if self._logger is not None: + self._logger.log_event(now_ms, "return_timeout", + timeout_s=cfg.timeout_s) + await self._fallback_rtl() + return + own = self._px4.own_state() + if not own.position_valid: + # 定位丢失：不能自主返航，先刹停等恢复（不盲目飞） + logger.warning("[Task] 返航中定位失效，刹停等待") + await self._px4.send_brake_hover(now_ms) + return + cmd = self._return_ctrl.update(own.latitude_deg, own.longitude_deg, + own.yaw_rad, own.relative_altitude_m) + out = ControlOutput(mode=ControlMode.VELOCITY_HEADING, valid=True, + vx_body_mps=cmd.vx_mps, vz_mps=cmd.vz_mps, + yaw_rate_dps=cmd.yaw_rate_dps) + await self._px4.send_velocity_heading(out, now_ms) + if cmd.arrived: + logger.info("[Task] 已回到起飞点（距 %.1fm），开始恢复初始航向", + cmd.distance_m) + self._rtl_phase = _RTL_YAW + self._yaw_started_ms = now_ms + + async def _yaw_align_step(self, now_ms: int) -> None: + """恢复初始航向阶段：原地转回起飞时刻的航向。""" + cfg = self._return_cfg + own = self._px4.own_state() + err, rate = self._return_ctrl.yaw_align(own.yaw_rad) + # 保持转场高度（不额外爬升） + out = ControlOutput(mode=ControlMode.VELOCITY_HEADING, valid=True, + vx_body_mps=0.0, + vz_mps=self._return_ctrl.alt_hold_vz( + own.relative_altitude_m), + yaw_rate_dps=rate) + await self._px4.send_velocity_heading(out, now_ms) + + timed_out = (self._yaw_started_ms is not None + and now_ms - self._yaw_started_ms + >= cfg.yaw_align_timeout_s * 1000.0) + if abs(err) <= cfg.yaw_align_tolerance_deg: + logger.info("[Task] 航向已恢复（误差 %.1f°），开始降落", err) + elif timed_out: + logger.warning("[Task] 航向对齐超时（误差 %.1f°），仍进入降落", err) + else: + return + if self._logger is not None: + self._logger.log_event(now_ms, "yaw_restored", + err_deg=err, timed_out=timed_out) + self._rtl_phase = _RTL_LAND + await self._start_land() + + async def _start_land(self) -> None: + """退出 offboard 并垂直降落（不爬升）。""" + if self._land_cmd_sent: + return + self._land_cmd_sent = True + # 只退 offboard、保留遥测：落地判定与高度曲线都依赖遥测（SITL 实测过： + # 若在这里就全停，日志会冻结在降落前的高度，看不出下降）。 + stop_for_landing = getattr(self._bridge, "landing_stop", None) + try: + if stop_for_landing is not None: + await stop_for_landing() + else: + await self._bridge.stop() + except Exception as exc: + logger.warning("[Task] 停机异常: %s", exc) + try: + await self._drone.action.land() + except Exception as exc: + logger.warning("[Task] 降落指令失败，尝试 PX4 RTL 兜底: %s", exc) + try: + await self._drone.action.return_to_launch() + except Exception: + pass + if self._logger is not None: + self._logger.log_event(self._now(), "rtl", action="land") + + async def _fallback_rtl(self) -> None: + """兜底：停机后交给 PX4 原生 RTL（会先爬升到 RTL_RETURN_ALT）。""" + self._rtl_phase = _RTL_FALLBACK + try: + await self._bridge.stop() except Exception as exc: logger.warning("[Task] 停机异常: %s", exc) try: @@ -268,21 +617,57 @@ class TaskRunner: except Exception: pass if self._logger is not None: - self._logger.log_event(self._now(), "rtl") + self._logger.log_event(self._now(), "rtl", action="rtl") + + async def _tick_rtl(self, now_ms: int) -> None: + """RTL 状态机：按阶段推进（转场 → 恢复航向 → 降落 / 兜底）。""" + phase = self._rtl_phase + if phase == _RTL_RETURN: + await self._return_step(now_ms) + elif phase == _RTL_YAW: + await self._yaw_align_step(now_ms) + elif phase == _RTL_LAND: + await self._check_landed() + elif phase == _RTL_FALLBACK: + await self._check_landed() + # 整体落地等待上限：任何阶段都不得无限等 + if (not self._rtl_done and self._rtl_entered_ms is not None + and now_ms - self._rtl_entered_ms + >= self._sm_cfg.rtl_timeout_s * 1000.0): + logger.warning("[Task] RTL 等待落地超过 %.0fs，结束任务（无法确认落地）", + self._sm_cfg.rtl_timeout_s) + self._rtl_done = True + if self._logger is not None: + self._logger.log_event(now_ms, "rtl_timeout", + timeout_s=self._sm_cfg.rtl_timeout_s) async def _check_landed(self) -> None: - """RTL 状态每周期取一帧 in_air 遥测；落地后 disarm 并标记完成。""" + """RTL 状态每周期取一帧 in_air 遥测；落地后 disarm 并标记完成。 + + 两个要点： + * `in_air()` 是 MAVSDK 遥测流：流挂起时 `async for` 会**永久阻塞**， + 主循环随之卡死（rtl_done 永不置位）。故加超时兜底，超时下拍重试。 + * 生成器**复用**（`_in_air_stream`）：每拍新建一个 `in_air()` 会在 + mavsdk_server 侧反复订阅/退订同一条流（20Hz × 整个 RTL 段），是 + 回调队列的无谓负担；复用后只订阅一次。 + """ try: - async for is_in_air in self._drone.telemetry.in_air(): - if not is_in_air and not self._rtl_done: - self._rtl_done = True - try: - await self._drone.action.disarm() - except Exception: - pass - break # 只取最新一帧 + if self._in_air_stream is None: + self._in_air_stream = self._drone.telemetry.in_air() + async with asyncio.timeout(_IN_AIR_TIMEOUT_S): + async for is_in_air in self._in_air_stream: + if not is_in_air and not self._rtl_done: + self._rtl_done = True + try: + await self._drone.action.disarm() + except Exception: + pass + break # 只取最新一帧 + except (TimeoutError, asyncio.TimeoutError): + logger.debug("[Task] in_air 遥测超时 %.1fs，下拍重试", _IN_AIR_TIMEOUT_S) except Exception: - pass + # 建流或消费失败都算流不可用：丢弃缓存，下拍重建（不永久卡在坏流上） + self._in_air_stream = None # --- 收尾 --------------------------------------------------------------- def _finished(self) -> bool: diff --git a/utils/task_runner/state_machine.py b/utils/task_runner/state_machine.py index e174807..3f946cd 100644 --- a/utils/task_runner/state_machine.py +++ b/utils/task_runner/state_machine.py @@ -5,23 +5,29 @@ L2（地理引导 GuidanceLaw）按任务阶段串成一条全流程。本模块 + 守卫」，不驱动 MAVSDK/offboard —— 那些由 runner.py（编排器）负责。每周期吃一份 TaskInputs 快照，返回是否转移 + 原因。 -状态链： - WAIT ──收到有效坐标──▶ ARM_TAKEOFF ──到起飞高度──▶ GOTO（L2 坐标引导） - │ 距目标 < switch_radius - │ 且锁定(+可选视觉) +状态链（2026-10-15 按实机作业预期重构）： + WAIT ──收到有效坐标──▶ ARM_TAKEOFF ──到起飞高度──▶ GOTO（L2 飞到参考点） + │ 距参考点 < arrival_radius ▼ - APPROACH ◀──居中稳定── TRACK（L1 航向伺服） - │ - 任意状态 ──超时 / 返航 / 紧急──▶ RTL + SEARCH（原地偏航扫视找目标）──锁定(+可选视觉)──▶ TRACK + │ 搜索超时 （L1 视觉追踪 + + │ slow_approach 缓慢靠近） + ▼ │ 追踪超时 + RTL ◀────────────────────────────────┘ 设计要点（铁律对齐）： * 纯逻辑：输入/输出都是 dataclass，时间用外部传入的 now_ms（单调毫秒）， 不碰时钟/IO/网络，可确定性单测。 * 全局最高优先级：emergency（丢控/RC 接管/失控）与 rtl_requested（人工返航） → 任何非 RTL 状态立即转 RTL（安全第一）。 - * GOTO→TRACK 守卫复用 L1 方案 §2.3：目标锁定 bit2 +（可选）我方视觉第二判据， - 避免 bit2 粘滞导致误入追踪。 - * TRACK 丢目标：若注入坐标仍有效 → 退回 GOTO 重新引导；否则 → RTL。 + * 「参考点」语义：地面给的坐标是**搜索起点**，不是终点——单点坐标精度不足以 + 直接覆盖目标，故进入到达半径即转 SEARCH 扫视，由吊舱/视觉去发现目标。 + * GOTO/SEARCH→TRACK 守卫复用 L1 方案 §2.3：目标锁定 bit2 +（可选）我方视觉 + 第二判据，避免 bit2 粘滞导致误入追踪。 + * TRACK 丢目标 → 回 SEARCH 重新扫视（**不回 GOTO**：飞机本就在参考点附近， + 回 GOTO 只会反复重跑进入动作，而参考点坐标本身不精确）；坐标同时失效 → RTL。 + * TRACK 不因「目标居中」退出：居中正是吊舱对准目标、应当继续缓慢靠近的时刻。 + 接近的收尾只由超时 / 人工返航 / 紧急终止（无距离输入，判不出"已到目标"）。 * 阈值/超时全外置（TaskStateConfig），代码不写死数值。 """ @@ -35,9 +41,10 @@ class TaskState(Enum): WAIT = "wait" # 待命：等地面坐标 ARM_TAKEOFF = "arm_takeoff" # 解锁 + 起飞到悬停高度 - GOTO = "goto" # L2 坐标引导（飞向注入坐标） - TRACK = "track" # L1 航向伺服（吊舱角度追踪） - APPROACH = "approach" # 接近/悬停目标上空 + GOTO = "goto" # L2 地理引导：飞向参考点（地面给的搜索起点） + SEARCH = "search" # 到点后原地偏航扫视搜索目标（吊舱/视觉） + TRACK = "track" # L1 视觉追踪 + 缓慢靠近（slow_approach 保守速度） + APPROACH = "approach" # 到点悬停（真机可选收尾；SITL/常规流程不用） RTL = "rtl" # 返航 + 落地 + 上锁（终态，reset() 才回 WAIT） @@ -45,15 +52,19 @@ class TaskState(Enum): class TaskStateConfig: """状态机配置（全外置；数值均为占位，须台架/实飞标定后回填）。""" - switch_radius_m: float = 30.0 # GOTO→TRACK 切换半径（米） - require_vision: bool = False # GOTO→TRACK 是否需视觉第二判据（S3 起开） + arrival_radius_m: float = 30.0 # GOTO→SEARCH 到达参考点半径（米） arm_takeoff_timeout_s: float = 60.0 # 起飞阶段超时 - goto_timeout_s: float = 120.0 # 坐标引导超时 - track_timeout_s: float = 120.0 # 追踪超时 - center_hold_s: float = 3.0 # TRACK→APPROACH 居中稳定时长（秒） - approach_hover_s: float = 10.0 # APPROACH 悬停时长（到点 → RTL） + goto_timeout_s: float = 120.0 # 飞向参考点超时 + search_timeout_s: float = 20.0 # 搜索超时（未发现目标 → 返航） + track_timeout_s: float = 20.0 # 追踪/靠近段上限（到点 → 返航） + approach_hover_s: float = 10.0 # APPROACH 悬停时长（到点 → RTL；已非主流程） takeoff_altitude_m: float = 5.0 # 起飞目标高度（runner 起飞 + takeoff_reached 判据） + takeoff_reached_tolerance_m: float = 0.5 # 起飞到达判据容差（低于目标不超过此值也算到达； + # 悬停/高度估计误差下避免差厘米误判"起飞超时"） center_threshold_deg: float = 1.0 # 脱靶量居中阈值（|off_target| < 此值判居中） + sweep_rate_dps: float = 15.0 # SEARCH 原地扫视角速度（度/秒） + sweep_reverse_s: float = 4.0 # SEARCH 扫视换向周期（秒）；0 = 单向扫视 + rtl_timeout_s: float = 180.0 # RTL 等待落地上限（秒）；超时即结束任务，防无限等待 loop_frequency_hz: float = 20.0 # runner 主循环频率（bridge.step 节拍） @@ -66,12 +77,11 @@ class TaskInputs: """ target_injected: bool = False # 地面坐标有效注入（新鲜、未超龄） - distance_to_target_m: float = math.inf # 到注入目标的水平距离（GOTO 用） + distance_to_target_m: float = math.inf # 到参考点的水平距离（GOTO 用） takeoff_reached: bool = False # 已到达起飞目标高度 - target_locked: bool = False # 吊舱锁定（bit2 + 遥测新鲜） - vision_has_target: bool = False # 我方视觉第二判据（可选，require_vision 时必看） - target_centered: bool = False # 目标居中（脱靶量长期≈0） - centered_duration_s: float = 0.0 # 居中持续时长（外部累加，秒） + target_locked: bool = False # 目标有效（吊舱 bit2 + 遥测新鲜 + LOS 一致性，见 bridge） + target_centered: bool = False # 目标居中（脱靶量≈0）——仅供日志/诊断 + centered_duration_s: float = 0.0 # 居中持续时长（外部累加，秒）——不再作转移判据 rtl_requested: bool = False # 人工返航命令 emergency: bool = False # 紧急（丢控 / RC 接管 / 失控） @@ -103,6 +113,15 @@ class TaskStateMachine: self._state = TaskState.WAIT self._entered_ms = 0 + def force_state(self, state: TaskState, now_ms: int) -> None: + """强制落到某状态（仅供编排器异常兜底；不产生 TaskTransition）。 + + 正常转移一律走 update()。此入口用于「主循环抛异常、必须立刻进 RTL」这类 + 非判据驱动的安全路径——此时再喂一拍快照不可靠。 + """ + self._state = state + self._entered_ms = now_ms + def update(self, inputs: TaskInputs, now_ms: int) -> TaskTransition: """吃一份快照，返回转移结果；无转移时 occurred=False、to_state 保持原状态。""" state = self._state @@ -127,23 +146,31 @@ class TaskStateMachine: elif state is TaskState.GOTO: if not inputs.target_injected: return self._goto(TaskState.RTL, "注入坐标失效", now_ms) - if (inputs.distance_to_target_m <= self._cfg.switch_radius_m - and self._lock_ok(inputs)): - return self._goto(TaskState.TRACK, "进入追踪半径且目标锁定", now_ms) + # 到参考点即结束地理引导 → 转扫视搜索（参考点是搜索起点，不是终点） + if inputs.distance_to_target_m <= self._cfg.arrival_radius_m: + return self._goto(TaskState.SEARCH, "已到参考点，开始搜索", now_ms) if self._elapsed_s(now_ms) >= self._cfg.goto_timeout_s: - return self._goto(TaskState.RTL, "坐标引导超时", now_ms) + return self._goto(TaskState.RTL, "飞向参考点超时", now_ms) + + elif state is TaskState.SEARCH: + # 扫视中一旦发现并锁定目标 → 进追踪（缓慢靠近） + if self._lock_ok(inputs): + return self._goto(TaskState.TRACK, "搜索发现目标并锁定", now_ms) + if not inputs.target_injected: + return self._goto(TaskState.RTL, "注入坐标失效", now_ms) + if self._elapsed_s(now_ms) >= self._cfg.search_timeout_s: + return self._goto(TaskState.RTL, "搜索超时未发现目标", now_ms) elif state is TaskState.TRACK: if not inputs.target_locked: - # 丢目标：坐标仍有效 → 退回 GOTO 重新引导；否则终止返航。 + # 丢目标：坐标仍有效 → 回参考点附近重新扫视；否则终止返航。 if inputs.target_injected: - return self._goto(TaskState.GOTO, "目标丢失，退回坐标引导", now_ms) + return self._goto(TaskState.SEARCH, "目标丢失，重新搜索", now_ms) return self._goto(TaskState.RTL, "目标丢失且坐标失效", now_ms) - if (inputs.target_centered - and inputs.centered_duration_s >= self._cfg.center_hold_s): - return self._goto(TaskState.APPROACH, "目标居中稳定", now_ms) + # 注意：不再有「居中稳定 → APPROACH」分支。居中表示吊舱已对准目标， + # 正是应当继续按 slow_approach 缓慢靠近的时刻，不是终止条件。 if self._elapsed_s(now_ms) >= self._cfg.track_timeout_s: - return self._goto(TaskState.RTL, "追踪超时", now_ms) + return self._goto(TaskState.RTL, "追踪/靠近超时", now_ms) elif state is TaskState.APPROACH: if self._elapsed_s(now_ms) >= self._cfg.approach_hover_s: @@ -154,15 +181,13 @@ class TaskStateMachine: # --- 内部辅助 ----------------------------------------------------------- def _lock_ok(self, inputs: TaskInputs) -> bool: - """GOTO→TRACK 的目标锁定判据（复用 L1 方案 §2.3）。 + """进 TRACK 的目标判据（GOTO/SEARCH→TRACK）。 - 需吊舱锁定 bit2；若 require_vision 还需我方视觉第二判据（bit2 粘滞兜底）。 + `target_locked` 已由编排层合成：吊舱 bit2 + 遥测新鲜 + **LOS 运动学一致性** + （见 bridge.pod_locked / los_consistency）。状态机不再重复做视觉判据 —— + 吊舱已取代双目+YOLO 成为唯一目标源，"我方视觉"不复存在。 """ - if not inputs.target_locked: - return False - if self._cfg.require_vision and not inputs.vision_has_target: - return False - return True + return inputs.target_locked def _elapsed_s(self, now_ms: int) -> float: """进入当前状态的时长（秒）。_entered_ms=0（未打戳）时返回 0。""" diff --git a/utils/tests/ground_station/app_test.py b/utils/tests/ground_station/app_test.py new file mode 100644 index 0000000..3f1d100 --- /dev/null +++ b/utils/tests/ground_station/app_test.py @@ -0,0 +1,299 @@ +"""app_test.py —— 地面站 Flask 端点测试（test_client + 真实 UDP 回环监听）。 + +覆盖：POST /api/inject 合法/非法（UDP 实收断言）、局部坐标注入换算与拒绝、 +POST /api/command 白名单校验与 UDP 下发、POST /api/continuous 服务端持续注入启停、 +GET /api/link 链路目标读写、GET /api/state 空日志目录兜底与 continuous 标志。 +""" + +import json +import math +import socket +import sys +import time +from pathlib import Path + +import pytest + +_ROOT = Path(__file__).resolve().parents[3] # utils/tests/ground_station → 仓库根 +if str(_ROOT) not in sys.path: + sys.path.insert(0, str(_ROOT)) + +from ground_station.app import create_app # noqa: E402 + + +@pytest.fixture +def udp_listener(): + """真实 UDP 回环监听：返回 (port, recv_one)。""" + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + s.bind(("127.0.0.1", 0)) + s.settimeout(1.5) + + def recv_one(): + data, _addr = s.recvfrom(2048) + return json.loads(data.decode("utf-8")) + + yield s.getsockname()[1], recv_one + s.close() + + +@pytest.fixture +def client(tmp_path, udp_listener): + port, _recv = udp_listener + app = create_app(udp_port=port, log_dir=str(tmp_path), + inject_interval_s=0.05) # 测试用短周期 + app.config["TESTING"] = True + return app.test_client() + + +# --- /api/inject ------------------------------------------------------------ +def test_inject_ok_sends_udp(client, udp_listener): + _port, recv_one = udp_listener + r = client.post("/api/inject", json={"lat": 1.5, "lon": 2.5, "alt": 10}) + assert r.get_json()["ok"] is True + assert recv_one() == {"lat": 1.5, "lon": 2.5, "alt": 10.0} + + +def test_inject_missing_lat_400(client): + r = client.post("/api/inject", json={"lon": 2.5}) + assert r.status_code == 400 + assert r.get_json()["ok"] is False + + +def test_inject_non_numeric_400(client): + r = client.post("/api/inject", json={"lat": "x", "lon": 2.5}) + assert r.status_code == 400 + + +# --- 局部坐标注入 ------------------------------------------------------------- +_TEL_HEADER = ("t_s,state,pos_valid,fix_type,num_satellites,latitude_deg," + "longitude_deg,relative_altitude_m,yaw_deg,target_injected," + "distance_to_target_m,target_locked,off_target_x_deg," + "off_target_y_deg,mode,yaw_rate_dps,vx_body_mps,vz_mps\n") + + +def _write_telemetry(log_dir, lat=47.0, lon=8.0, pos_valid="True"): + with open(Path(log_dir) / "task_telemetry.csv", "w", encoding="utf-8") as f: + f.write(_TEL_HEADER) + f.write(f"1.0,wait,{pos_valid},3,10,{lat},{lon},0,0,False,inf,False," + "0,0,,0,0,0\n") + + +def test_inject_local_converts_from_vehicle_position(client, udp_listener, tmp_path): + _write_telemetry(tmp_path, lat=47.0, lon=8.0) + _port, recv_one = udp_listener + r = client.post("/api/inject", json={"north_m": 30, "east_m": 15, "alt": 0}) + j = r.get_json() + assert j["ok"] is True + exp_lat = 47.0 + 30 / 111320.0 + exp_lon = 8.0 + 15 / (111320.0 * math.cos(math.radians(47.0))) + msg = recv_one() + assert msg["lat"] == pytest.approx(exp_lat, abs=1e-9) + assert msg["lon"] == pytest.approx(exp_lon, abs=1e-9) + assert j["local"]["north_m"] == 30 and j["local"]["east_m"] == 15 + assert j["local"]["origin_lat"] == pytest.approx(47.0) + + +def test_inject_local_rejected_without_telemetry(client): + r = client.post("/api/inject", json={"north_m": 30, "east_m": 15}) + assert r.status_code == 400 + assert "无法换算" in r.get_json()["error"] + + +def test_inject_local_rejected_when_pos_invalid(client, tmp_path): + _write_telemetry(tmp_path, pos_valid="False") + r = client.post("/api/inject", json={"north_m": 30, "east_m": 15}) + assert r.status_code == 400 + + +def test_inject_local_bad_numbers_400(client, tmp_path): + _write_telemetry(tmp_path) + assert client.post("/api/inject", + json={"north_m": "x", "east_m": 15}).status_code == 400 + + +def test_inject_local_home_origin_uses_session_start(client, udp_listener, tmp_path): + # 同一段：起始有效定位 47.0/8.0（≈起飞点），之后移动到 47.5/8.5 + with open(tmp_path / "task_telemetry.csv", "w", encoding="utf-8") as f: + f.write(_TEL_HEADER) + f.write("1.0,wait,True,3,10,47.0,8.0,0,0,False,inf,False,0,0,,0,0,0\n") + f.write("60.0,goto,True,3,10,47.5,8.5,0,0,False,inf,False,0,0,,0,0,0\n") + _port, recv_one = udp_listener + + j = client.post("/api/inject", + json={"north_m": 0, "east_m": 0, "origin": "home"}).get_json() + assert j["ok"] is True and j["local"]["origin"] == "home" + assert j["local"]["origin_lat"] == pytest.approx(47.0) # 段首而非最后行 + assert j["local"]["origin_lon"] == pytest.approx(8.0) + recv_one() + + j2 = client.post("/api/inject", + json={"north_m": 0, "east_m": 0}).get_json() # current + assert j2["local"]["origin"] == "current" + assert j2["local"]["origin_lat"] == pytest.approx(47.5) # 当前位置 + recv_one() + + +def test_inject_local_home_origin_skips_invalid_rows(client, tmp_path): + with open(tmp_path / "task_telemetry.csv", "w", encoding="utf-8") as f: + f.write(_TEL_HEADER) + f.write("0.5,wait,False,0,0,0.0,0.0,0,0,False,inf,False,0,0,,0,0,0\n") + f.write("1.0,wait,True,3,10,47.0,8.0,0,0,False,inf,False,0,0,,0,0,0\n") + j = client.post("/api/inject", + json={"north_m": 30, "east_m": 15, + "origin": "home"}).get_json() + assert j["ok"] is True + assert j["local"]["origin_lat"] == pytest.approx(47.0) # 取首个有效定位 + + +def test_inject_local_bad_origin_400(client, tmp_path): + _write_telemetry(tmp_path) + r = client.post("/api/inject", + json={"north_m": 1, "east_m": 1, "origin": "mars"}) + assert r.status_code == 400 + + +def test_inject_local_home_origin_rejected_without_telemetry(client): + r = client.post("/api/inject", + json={"north_m": 30, "east_m": 15, "origin": "home"}) + assert r.status_code == 400 + assert "起飞点" in r.get_json()["error"] + + +# --- /api/command ----------------------------------------------------------- +def test_command_whitelist_ok_sends_udp(client, udp_listener): + _port, recv_one = udp_listener + r = client.post("/api/command", json={"cmd": "rtl"}) + assert r.get_json() == {"ok": True, "sent": {"cmd": "rtl"}} + assert recv_one() == {"cmd": "rtl"} + + +def test_command_all_whitelist_entries_accepted(client, udp_listener): + _port, recv_one = udp_listener + for cmd in ("rtl", "emergency", "pod_detect_on", "pod_auto_lock", + "pod_stop_track", "pod_detect_off", "pod_center"): + r = client.post("/api/command", json={"cmd": cmd}) + assert r.get_json()["ok"] is True + assert recv_one() == {"cmd": cmd} + + +def test_command_unknown_rejected_no_udp(client, udp_listener): + _port, recv_one = udp_listener + r = client.post("/api/command", json={"cmd": "reboot"}) + assert r.status_code == 400 + assert r.get_json()["ok"] is False + with pytest.raises(socket.timeout): + recv_one() # 未下发任何 UDP → 监听超时即证明 + + +def test_command_non_string_rejected(client): + r = client.post("/api/command", json={"cmd": 1}) + assert r.status_code == 400 + + +# --- /api/continuous -------------------------------------------------------- +def test_continuous_requires_prior_inject(client): + r = client.post("/api/continuous", json={"on": True}) + assert r.status_code == 400 + assert r.get_json()["ok"] is False + + +def test_continuous_non_bool_rejected(client): + r = client.post("/api/continuous", json={"on": "yes"}) + assert r.status_code == 400 + + +def test_continuous_sends_repeatedly_and_stops(client, udp_listener): + _port, recv_one = udp_listener + client.post("/api/inject", json={"lat": 1.0, "lon": 2.0}) + assert recv_one() == {"lat": 1.0, "lon": 2.0, "alt": 0.0} # 手动单发 + + r = client.post("/api/continuous", json={"on": True}) + assert r.get_json() == {"ok": True, "continuous": True} + assert recv_one() == {"lat": 1.0, "lon": 2.0, "alt": 0.0} # 服务端自动重发 + assert recv_one() == {"lat": 1.0, "lon": 2.0, "alt": 0.0} + + r = client.post("/api/continuous", json={"on": False}) + assert r.get_json() == {"ok": True, "continuous": False} + time.sleep(0.2) + while True: # 排空在途包 + try: + recv_one() + except socket.timeout: + break + with pytest.raises(socket.timeout): + recv_one() # 停止后不再发 + + +def test_continuous_uses_latest_inject(client, udp_listener): + _port, recv_one = udp_listener + client.post("/api/inject", json={"lat": 1.0, "lon": 2.0}) + recv_one() + client.post("/api/inject", json={"lat": 3.0, "lon": 4.0}) # 中途改坐标 + recv_one() + + client.post("/api/continuous", json={"on": True}) + assert recv_one() == {"lat": 3.0, "lon": 4.0, "alt": 0.0} + client.post("/api/continuous", json={"on": False}) + + +# --- /api/link -------------------------------------------------------------- +def test_link_get_default_and_options(client, udp_listener): + port, _recv = udp_listener + j = client.get("/api/link").get_json() + assert j["host"] == "127.0.0.1" + assert j["port"] == port + assert any(o["host"] == "127.0.0.1" for o in j["options"]) + + +def test_link_set_bad_host_or_port_400(client): + assert client.post("/api/link", json={"host": " "}).status_code == 400 + assert client.post("/api/link", + json={"host": "1.2.3.4", "port": "x"}).status_code == 400 + assert client.post("/api/link", + json={"host": "1.2.3.4", "port": 70000}).status_code == 400 + + +def test_link_switch_redirects_inject_and_command(client, udp_listener): + _port_a, recv_a = udp_listener + s_b = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + s_b.bind(("127.0.0.1", 0)) + s_b.settimeout(1.5) + port_b = s_b.getsockname()[1] + try: + r = client.post("/api/link", json={"host": "127.0.0.1", "port": port_b}) + assert r.get_json() == {"ok": True, "host": "127.0.0.1", "port": port_b} + + client.post("/api/inject", json={"lat": 1.0, "lon": 2.0}) + data, _addr = s_b.recvfrom(2048) + assert json.loads(data.decode("utf-8")) == {"lat": 1.0, "lon": 2.0, + "alt": 0.0} + client.post("/api/command", json={"cmd": "rtl"}) + data, _addr = s_b.recvfrom(2048) + assert json.loads(data.decode("utf-8")) == {"cmd": "rtl"} + + with pytest.raises(socket.timeout): + recv_a() # 旧目标不再收到任何包 + finally: + s_b.close() + + +# --- /api/state ------------------------------------------------------------- +def test_state_empty_log_dir(client): + j = client.get("/api/state").get_json() + assert j["state"] == "unknown" + assert j["telemetry"] is None + assert j["continuous"] is False + + +def test_video_disabled_by_default(client): + assert client.get("/api/state").get_json()["video"] == { + "enabled": False, "online": False} + assert client.get("/video_feed").status_code == 503 + + +def test_state_reports_continuous(client): + client.post("/api/inject", json={"lat": 1.0, "lon": 2.0}) + client.post("/api/continuous", json={"on": True}) + assert client.get("/api/state").get_json()["continuous"] is True + client.post("/api/continuous", json={"on": False}) + assert client.get("/api/state").get_json()["continuous"] is False diff --git a/utils/tests/ground_station/video_source_test.py b/utils/tests/ground_station/video_source_test.py new file mode 100644 index 0000000..0bf7492 --- /dev/null +++ b/utils/tests/ground_station/video_source_test.py @@ -0,0 +1,138 @@ +"""video_source_test.py —— RTSP→MJPEG 视频源单元测试（注入假采集器，不依赖真流）。 + +覆盖：拉帧→JPEG 编码缓存、宽度限制（等比缩放）、断线重连（重新调用工厂）、 +打开失败退避重试、online 判定、MJPEG 分块格式。 +""" + +import sys +import time +from pathlib import Path + +import numpy as np + +_ROOT = Path(__file__).resolve().parents[3] # utils/tests/ground_station → 仓库根 +if str(_ROOT) not in sys.path: + sys.path.insert(0, str(_ROOT)) + +from ground_station.video_source import RtspVideoSource # noqa: E402 + + +class FakeCapture: + """假采集器：可注入读失败次数；帧为纯色小图。""" + + def __init__(self, fail_reads=0, height=60, width=120): + self.fail_reads = fail_reads + self._frame = np.full((height, width, 3), 128, dtype=np.uint8) + self.released = False + + def isOpened(self): + return True + + def read(self): + if self.fail_reads > 0: + self.fail_reads -= 1 + return False, None + return True, self._frame + + def release(self): + self.released = True + + +def _wait(pred, timeout_s=4.0, step=0.02): + t0 = time.monotonic() + while time.monotonic() - t0 < timeout_s: + if pred(): + return True + time.sleep(step) + return False + + +def _decode(jpeg): + import cv2 + return cv2.imdecode(np.frombuffer(jpeg, dtype=np.uint8), cv2.IMREAD_COLOR) + + +def test_frames_encoded_and_online(): + src = RtspVideoSource("rtsp://fake", width=120, fps=50, + capture_factory=lambda: FakeCapture()) + assert src.online is False # 尚无帧 → 离线 + src.start() + try: + assert _wait(lambda: src.latest()[1] is not None) + seq, jpeg = src.latest() + assert seq >= 1 + assert jpeg[:2] == b"\xff\xd8" # JPEG SOI 头 + assert _decode(jpeg).shape[1] == 120 # 宽度不超限 + assert src.online is True + finally: + src.stop() + + +def test_downscale_to_max_width(): + src = RtspVideoSource("rtsp://fake", width=60, fps=50, + capture_factory=lambda: FakeCapture(height=60, width=120)) + src.start() + try: + assert _wait(lambda: src.latest()[1] is not None) + img = _decode(src.latest()[1]) + assert img.shape[1] == 60 and img.shape[0] == 30 # 等比缩到 60 宽 + finally: + src.stop() + + +def test_reconnect_after_read_failure(): + opened = [] + + def factory(): + cap = FakeCapture(fail_reads=(10 ** 6 if not opened else 0)) + opened.append(cap) + return cap + + src = RtspVideoSource("rtsp://fake", width=120, fps=50, + capture_factory=factory) + src.start() + try: + assert _wait(lambda: src.latest()[1] is not None, timeout_s=5.0) + assert len(opened) >= 2 # 首采集器读失败 → 重新打开 + assert opened[0].released is True + finally: + src.stop() + + +def test_open_failure_keeps_retrying(): + calls = [] + + class ClosedCapture: + def isOpened(self): + return False + + def release(self): + pass + + def factory(): + calls.append(1) + return ClosedCapture() + + src = RtspVideoSource("rtsp://fake", capture_factory=factory) + src.start() + try: + assert _wait(lambda: len(calls) >= 2, timeout_s=4.0) # 1s 退避后重试 + assert src.latest()[1] is None + finally: + src.stop() + + +def test_mjpeg_stream_chunks(): + src = RtspVideoSource("rtsp://fake", width=120, fps=50, + capture_factory=lambda: FakeCapture()) + src.start() + try: + assert _wait(lambda: src.latest()[1] is not None) + gen = src.mjpeg_stream() + chunk = next(gen) + assert chunk.startswith(b"--frame\r\nContent-Type: image/jpeg\r\n") + assert b"Content-Length: " in chunk + assert chunk.endswith(b"\r\n") + gen.close() + finally: + src.stop() diff --git a/utils/tests/pod_bridge/bridge_test.py b/utils/tests/pod_bridge/bridge_test.py index 84c1818..8542323 100644 --- a/utils/tests/pod_bridge/bridge_test.py +++ b/utils/tests/pod_bridge/bridge_test.py @@ -6,6 +6,7 @@ start/stop 生命周期与停机刹车、run() 主循环异常护栏与故障恢 """ import asyncio +import math import time from dataclasses import replace @@ -29,6 +30,7 @@ class FakePx4: self.offboard_start_ok = True self.velocity_calls = [] self.brake_calls = [] + self.yaw_calls = [] def own_state(self): return replace(self._state) @@ -54,6 +56,10 @@ class FakePx4: self.brake_calls.append(time_boot_ms) return True + async def send_yaw_rate(self, yaw_rate_dps): + self.yaw_calls.append(yaw_rate_dps) + return True + class FakePod: def __init__(self): @@ -159,6 +165,116 @@ def test_attitude_downlink_throttled(): assert len(pod.sent) == 2 +def test_control_law_throttled_to_own_frequency(): + """生效律按自身频率推进：L2 配置 10Hz，外部 20Hz 调用时控制律只跑 10 次/秒。 + + 链路维持（表3 姿态下发）不受此节流影响——否则吊舱解算会断流。 + """ + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + cfg = BridgeConfig() + cfg.heading_servo.enabled = False # L2 地理引导（10Hz） + cfg.guidance.frequency_hz = 10.0 + cfg.link.attitude_downlink_hz = 20.0 + cfg.ground_inject.enabled = True + ground = GroundTarget(latitude_deg=0.01, longitude_deg=0.0, time_ms=1000) + b = PodBridge(cfg, px4, pod, now_ms_fn=lambda: 1000, + ground_target_fn=lambda: ground) + + # 统计控制律实际推进次数（包装 update，不改变行为） + law_updates = {"n": 0} + real_update = b._guidance.update + + def counting_update(inp, now): + law_updates["n"] += 1 + return real_update(inp, now) + + b._guidance.update = counting_update + + ticks = 10 # 20Hz 调用 10 拍 = 500ms + + async def scenario(): + for i in range(ticks): + await b.step(1000 + i * 50) + asyncio.run(scenario()) + + # 10Hz 下 500ms 只应推进 5 拍控制律；其余 5 拍重发上一拍输出保心跳 + assert law_updates["n"] == 5 + assert len(px4.velocity_calls) == ticks # offboard 设定点每拍都在下发 + assert len(pod.sent) == ticks # 表3 姿态下发不受控制律节流影响 + + +def test_control_law_throttle_resets_on_law_switch(): + """切律后立即出第一拍（不被上一拍的节流压住）。""" + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + cfg = BridgeConfig() + cfg.guidance.frequency_hz = 10.0 + b = _bridge(px4, pod, cfg) + + async def scenario(): + await b.step(1000) + b.set_heading_servo_enabled(True) + await b.step(1010) # 切律后 10ms 即应推进 + asyncio.run(scenario()) + assert b._last_law_ms == 1010 + + +def test_stop_is_idempotent(): + """重复 stop() 只真正停机一次（真机在环实测：RTL 与收尾各调一次）。""" + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + b = _bridge(px4, pod) + + async def scenario(): + await b.start() + await b.stop() + await b.stop() # 第二次应为 no-op + asyncio.run(scenario()) + + assert pod.closed is True + assert px4.offboard_stopped is True + assert len(px4.brake_calls) == 1 # 刹车只发一次 + + +def test_start_after_stop_reenables_stop(): + """停机后重新 start() 可再次生效（标志位复位）。""" + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + b = _bridge(px4, pod) + + async def scenario(): + await b.start() + await b.stop() + assert b._stopped is True + await b.start() + assert b._stopped is False + await b.stop() + asyncio.run(scenario()) + assert len(px4.brake_calls) == 2 # 两次停机各发一次刹车 + + +def test_landing_stop_keeps_telemetry_and_pod_open(): + """降落前只退 offboard：遥测与串口保留，供落地判定与高度曲线。""" + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + b = _bridge(px4, pod) + + async def scenario(): + await b.start() + await b.landing_stop() + assert b._landing_stopped is True + assert b._stopped is False # 未全停 + assert px4.offboard_stopped is True + assert px4.telemetry_stopped is False # 遥测仍在 + assert pod.closed is False # 串口仍在 + await b.stop() # 收尾才全停 + asyncio.run(scenario()) + assert px4.telemetry_stopped is True + assert pod.closed is True + assert len(px4.brake_calls) == 2 # 两次停机各发一次刹车 + + # --- 吊舱帧入站缓存 -------------------------------------------------------- def test_target_frame_updates_latest_and_flips_to_velocity(): px4 = FakePx4(_valid_state(1000)) @@ -423,6 +539,12 @@ def _locked_off_target(x=0.0, y=0.0, frame_yaw=0.0, frame_pitch=0.0, laser=50.0) frame_yaw_deg=frame_yaw, frame_pitch_deg=frame_pitch) +def _set_pose(px4, now_ms: int, yaw_deg: float): + """更新假飞控姿态与时间戳（保证控制律的姿态超龄判定通过）。""" + px4._state = replace(px4._state, yaw_rad=math.radians(yaw_deg), + time_ms=now_ms, position_time_ms=now_ms) + + def test_l1_off_target_beta_drives_yaw_rate(): px4 = FakePx4(_valid_state(1000)) pod = FakePod() @@ -503,26 +625,73 @@ def test_l1_no_telemetry_degrades_and_keeps_heartbeat(): assert len(px4.brake_calls) == 1 -def test_l1_require_vision_blocks_without_source(): - # §2.3 第二判据：require_vision=True 但未接视觉源 → fail-safe 视为无目标 → 刹车 - px4 = FakePx4(_valid_state(1000)) +def test_l1_los_consistent_keeps_tracking(): + """机头右转多少、脱靶量就左移多少（目标在世界系静止）→ 判一致，正常跟踪。""" + clock = {"ms": 1_000_000} + px4 = FakePx4(_valid_state(clock["ms"])) pod = FakePod() - pod.queue(_locked_off_target(x=10.0)) - b = _bridge(px4, pod, _l1_config(require_vision=True)) - out = asyncio.run(b.step(1000)) - assert out.mode.name == "BRAKE_HOVER" - assert px4.velocity_calls == [] + b = PodBridge(_l1_config(), px4, pod, now_ms_fn=lambda: clock["ms"]) + async def scenario(): + for i in range(80): # 4s，20°/s 右转、脱靶量 −20°/s + clock["ms"] += 50 + yaw = 20.0 * (i + 1) * 0.05 + beta = 10.0 - 20.0 * (i + 1) * 0.05 + _set_pose(px4, clock["ms"], yaw) + pod.queue(_locked_off_target(x=beta)) + out = await b.step(clock["ms"]) + return out -def test_l1_require_vision_passes_with_source(): - px4 = FakePx4(_valid_state(1000)) + out = asyncio.run(scenario()) + assert b._los_consistent is True + assert out.mode.name == "VELOCITY_HEADING" + assert b.pod_locked(clock["ms"]) is True + + +def test_l1_los_frozen_angle_blocks_tracking(): + """机头转了 60° 而脱靶量几乎不变 → 角度冻结/幽灵目标 → 判丢（刹车）。""" + clock = {"ms": 1_000_000} + px4 = FakePx4(_valid_state(clock["ms"])) pod = FakePod() - pod.queue(_locked_off_target(x=10.0)) - b = PodBridge(_l1_config(require_vision=True), px4, pod, - now_ms_fn=lambda: 0, vision_target_fn=lambda: True) - out = asyncio.run(b.step(1000)) + b = PodBridge(_l1_config(), px4, pod, now_ms_fn=lambda: clock["ms"]) + + async def scenario(): + out = None + for i in range(80): # 4s，右转 20°/s，脱靶量恒定 10° + clock["ms"] += 50 + yaw = 20.0 * (i + 1) * 0.05 + _set_pose(px4, clock["ms"], yaw) + pod.queue(_locked_off_target(x=10.0)) + out = await b.step(clock["ms"]) + return out + + out = asyncio.run(scenario()) + assert b._los_consistent is False + assert out.mode.name == "BRAKE_HOVER" # 控制律被喂 tracked=False + assert b.pod_locked(clock["ms"]) is False # 状态机侧也判为丢目标 + + +def test_l1_los_disabled_falls_back_to_bit2(): + """关掉一致性判据 → 退回"只信 bit2"（角度冻结也不再判丢）。""" + clock = {"ms": 1_000_000} + px4 = FakePx4(_valid_state(clock["ms"])) + pod = FakePod() + b = PodBridge(_l1_config(los_consistency_enabled=False), px4, pod, + now_ms_fn=lambda: clock["ms"]) + + async def scenario(): + out = None + for i in range(80): + clock["ms"] += 50 + yaw = 20.0 * (i + 1) * 0.05 + _set_pose(px4, clock["ms"], yaw) + pod.queue(_locked_off_target(x=10.0)) + out = await b.step(clock["ms"]) + return out + + out = asyncio.run(scenario()) + assert b._los_consistent is True assert out.mode.name == "VELOCITY_HEADING" - assert out.yaw_rate_dps == pytest.approx(10.0, abs=1e-9) # --- 地面坐标注入（A2）------------------------------------------------------- diff --git a/utils/tests/pod_bridge/ground_link_test.py b/utils/tests/pod_bridge/ground_link_test.py index 930cf86..6a39ac7 100644 --- a/utils/tests/pod_bridge/ground_link_test.py +++ b/utils/tests/pod_bridge/ground_link_test.py @@ -1,12 +1,14 @@ -"""ground_link_test.py —— 地面坐标注入通道单元测试。 +"""ground_link_test.py —— 地面站链路单元测试。 覆盖：parse_payload 解析（合法/缺字段/非法 JSON/非对象/类型错误）、 -is_valid 新鲜度判定（未注入/新鲜/超龄边界）。 +parse_message 双通道（坐标 + {"cmd"} 命令）、命令队列 drain、 +is_valid 新鲜度判定（未注入/新鲜/超龄边界）、run() 真 UDP 回环。 """ import pytest -from pod_bridge.ground_link import GroundInjectConfig, GroundLink, GroundTarget +from pod_bridge.ground_link import (GroundCommand, GroundInjectConfig, + GroundLink, GroundTarget) # --- parse_payload ---------------------------------------------------------- @@ -92,3 +94,108 @@ def test_latest_returns_injected_target(): _inject(link, 1000) assert link.latest().latitude_deg == pytest.approx(1.0) assert link.latest().time_ms == 1000 + + +# --- parse_message（坐标 + 命令双通道）--------------------------------------- +def test_parse_message_command(): + m = GroundLink.parse_message(b'{"cmd": "rtl"}') + assert isinstance(m, GroundCommand) + assert m.name == "rtl" + + +def test_parse_message_command_normalized(): + m = GroundLink.parse_message(b'{"cmd": " RTL "}') + assert isinstance(m, GroundCommand) + assert m.name == "rtl" + + +def test_parse_message_cmd_takes_precedence_over_coords(): + m = GroundLink.parse_message(b'{"cmd": "emergency", "lat": 1.0, "lon": 2.0}') + assert isinstance(m, GroundCommand) + assert m.name == "emergency" + + +def test_parse_message_coords_still_work(): + m = GroundLink.parse_message(b'{"lat": 1.0, "lon": 2.0}') + assert isinstance(m, GroundTarget) + assert m.latitude_deg == pytest.approx(1.0) + + +def test_parse_message_empty_cmd_falls_back_to_coords(): + m = GroundLink.parse_message(b'{"cmd": "", "lat": 1.0, "lon": 2.0}') + assert isinstance(m, GroundTarget) + + +def test_parse_message_non_string_cmd_falls_back_to_coords(): + m = GroundLink.parse_message(b'{"cmd": 5, "lat": 1.0, "lon": 2.0}') + assert isinstance(m, GroundTarget) + + +def test_parse_message_invalid_returns_none(): + assert GroundLink.parse_message(b"not json") is None + + +# --- drain_commands --------------------------------------------------------- +def test_drain_commands_empty(): + assert _link().drain_commands() == [] + + +def test_drain_commands_fifo_and_clear(): + link = _link() + link._commands.append("rtl") + link._commands.append("pod_center") + assert link.drain_commands() == ["rtl", "pod_center"] + assert link.drain_commands() == [] # 取后清空 + + +def test_commands_queue_drops_oldest_when_full(): + link = _link() + for i in range(20): + link._commands.append(f"c{i}") + cmds = link.drain_commands() + assert len(cmds) == 16 # maxlen=16 + assert cmds[0] == "c4" and cmds[-1] == "c19" + + +# --- run() 真 UDP 回环（坐标 + 命令）----------------------------------------- +def test_run_receives_coords_and_commands(): + import asyncio + import socket as socket_mod + + probe = socket_mod.socket(socket_mod.AF_INET, socket_mod.SOCK_DGRAM) + probe.bind(("127.0.0.1", 0)) + port = probe.getsockname()[1] + probe.close() + + link = GroundLink(GroundInjectConfig(host="127.0.0.1", port=port), + now_ms_fn=lambda: 1234) + addr = ("127.0.0.1", port) + + async def scenario(): + stop = asyncio.Event() + task = asyncio.ensure_future(link.run(stop)) + udp = socket_mod.socket(socket_mod.AF_INET, socket_mod.SOCK_DGRAM) + try: + for _ in range(200): # 重发防 bind 竞态 + udp.sendto(b'{"lat": 1.5, "lon": 2.5}', addr) + await asyncio.sleep(0.01) + if link.latest().time_ms == 1234: + break + udp.sendto(b'{"cmd": "rtl"}', addr) + for _ in range(100): + if link._commands: + break + await asyncio.sleep(0.01) + finally: + udp.close() + stop.set() + task.cancel() # 收包循环阻塞在 sock_recvfrom，须 cancel 唤醒（同 SITL 入口） + try: + await task + except asyncio.CancelledError: + pass + + asyncio.run(scenario()) + assert link.latest().latitude_deg == pytest.approx(1.5) + assert link.latest().time_ms == 1234 + assert link.drain_commands() == ["rtl"] diff --git a/utils/tests/pod_bridge/los_consistency_test.py b/utils/tests/pod_bridge/los_consistency_test.py new file mode 100644 index 0000000..8d6a618 --- /dev/null +++ b/utils/tests/pod_bridge/los_consistency_test.py @@ -0,0 +1,141 @@ +"""los_consistency_test.py —— 吊舱角度运动学一致性判据的纯逻辑测试。 + +背景：本吊舱无电机、相机刚性固连机体，机头转 Δψ 时真实目标脱靶量必变 ≈−Δψ。 +判据 `residual = Δβ + Δψ`：超限且激励足够 → 判不一致（角度冻结/幽灵目标）。 +""" + +import pytest + +from pod_bridge.los_consistency import (LosConsistencyConfig, + LosConsistencyMonitor, wrap_deg_180) + + +def _mon(cfg=None): + return LosConsistencyMonitor(cfg or LosConsistencyConfig()) + + +def _drive(mon, n, dt_ms, yaw0=0.0, beta0=0.0, yaw_rate=0.0, beta_rate=0.0, + t0=0): + """按固定角速度喂 n 拍，返回最后一次状态。""" + status = None + for i in range(n): + t = t0 + (i + 1) * dt_ms + yaw = yaw0 + yaw_rate * (i + 1) * dt_ms / 1000.0 + beta = beta0 + beta_rate * (i + 1) * dt_ms / 1000.0 + status = mon.update(t, yaw, beta) + return status + + +# --- 条件开关 --------------------------------------------------------------- +def test_disabled_always_consistent(): + mon = _mon(LosConsistencyConfig(enabled=False)) + status = _drive(mon, 100, 50, yaw_rate=30.0, beta_rate=0.0) + assert status.consistent is True + assert status.excited is False + + +def test_no_history_is_not_excited(): + mon = _mon() + status = mon.update(1000, 0.0, 10.0) + assert status.consistent is True + assert status.excited is False + + +def test_insufficient_span_is_not_excited(): + """窗口未积累到 3/4 之前不做判断（避免一开始就误判丢目标）。""" + mon = _mon(LosConsistencyConfig(window_ms=3000)) + status = _drive(mon, 20, 50, yaw_rate=40.0, beta_rate=0.0) # 仅 1s + assert status.excited is False + assert status.consistent is True + + +# --- 一致：机头转、脱靶量反向跟随 ------------------------------------------- +def test_consistent_rotation_is_ok(): + """右转 20°/s 且 β 以 −20°/s 变化 → 残差≈0。""" + mon = _mon() + status = _drive(mon, 80, 50, yaw_rate=20.0, beta_rate=-20.0) # 4s + assert status.excited is True + assert status.consistent is True + assert status.residual_deg == pytest.approx(0.0, abs=0.5) + + +def test_consistent_left_rotation_is_ok(): + """左转（负角速度）且 β 正向跟随 → 同样一致（符号对称）。""" + mon = _mon() + status = _drive(mon, 80, 50, yaw_rate=-20.0, beta_rate=20.0) + assert status.excited is True + assert status.consistent is True + + +def test_target_motion_within_residual_tolerance_is_ok(): + """目标自身在动（β 变化与 −Δψ 不完全相等）但残差在容差内 → 仍判一致。""" + mon = _mon(LosConsistencyConfig(max_residual_deg=8.0)) + # 4s 内 Δψ=80°，β 只反向变 76° → 残差 4°（< 8°） + status = _drive(mon, 80, 50, yaw_rate=20.0, beta_rate=-19.0) + assert status.excited is True + assert status.consistent is True + + +# --- 不一致：角度冻结/幽灵目标 --------------------------------------------- +def test_frozen_beta_while_yawing_is_inconsistent(): + """机头转 80° 而脱靶量纹丝不动 → 角度冻结 → 判不一致。 + + 比较窗口为 3s（默认），故 4s 数据里只比较最后 3s：Δψ = 20°/s × 3s = 60°。 + """ + mon = _mon() + status = _drive(mon, 80, 50, yaw_rate=20.0, beta_rate=0.0) + assert status.excited is True + assert status.consistent is False + assert status.frozen is True + assert status.residual_deg == pytest.approx(60.0, abs=2.0) # 窗口内 Δψ + + +def test_small_yaw_change_does_not_judge_even_if_residual_big(): + """转得不够多（< 激励门槛）时不判不一致 —— 避免误杀。""" + mon = _mon(LosConsistencyConfig(min_yaw_change_deg=20.0)) + # 窗口内只转 8°，虽然 β 没动（残差 8°）但未达激励门槛 + status = _drive(mon, 80, 50, yaw_rate=2.0, beta_rate=0.0) + assert status.excited is False + assert status.consistent is True + + +# --- 历史管理 --------------------------------------------------------------- +def test_reset_clears_history(): + mon = _mon() + _drive(mon, 80, 50, yaw_rate=20.0, beta_rate=0.0) + assert mon._samples # 有历史 + mon.reset() + assert not mon._samples + status = mon.update(9999, 0.0, 0.0) + assert status.excited is False + + +def test_old_samples_dropped(): + """超出历史保留期的样本被丢弃，不会永久污染判定。""" + mon = _mon(LosConsistencyConfig(window_ms=1000, history_factor=2.0)) + _drive(mon, 40, 50, yaw_rate=50.0, beta_rate=0.0) # 2s + # 再喂一拍"很久之后"，旧样本应被清掉 + status = mon.update(60_000, 0.0, 0.0) + assert status.excited is False + + +# --- 角度回绕 --------------------------------------------------------------- +@pytest.mark.parametrize("deg,expect", [ + (0.0, 0.0), (179.0, 179.0), (181.0, -179.0), + (-181.0, 179.0), (359.0, -1.0), +]) +def test_wrap_deg_180(deg, expect): + assert wrap_deg_180(deg) == pytest.approx(expect) + + +def test_wrap_around_north_is_consistent(): + """航向跨过 ±180 与 β 同步变化时，不应因回绕误判。""" + mon = _mon() + # 从 yaw=170 转到 190（即 -170），β 从 10 到 -10 + for i in range(80): + t = (i + 1) * 50 + yaw = 170.0 + 20.0 * (i + 1) * 0.05 + beta = 10.0 - 20.0 * (i + 1) * 0.05 + status = mon.update(t, wrap_deg_180(yaw), beta) + assert status.excited is True + assert status.consistent is True diff --git a/utils/tests/pod_bridge/px4_gateway_test.py b/utils/tests/pod_bridge/px4_gateway_test.py index 6e83885..722d3e5 100644 --- a/utils/tests/pod_bridge/px4_gateway_test.py +++ b/utils/tests/pod_bridge/px4_gateway_test.py @@ -43,11 +43,13 @@ class FakeOffboard: class FakeTelemetry: """各遥测流：先吐预设消息，再挂起（由 stop_telemetry 取消）。""" - def __init__(self, position=None, attitude=None, gps=None, velocity=None): + def __init__(self, position=None, attitude=None, gps=None, velocity=None, + status=None): self._position = position or [] self._attitude = attitude or [] self._gps = gps or [] self._velocity = velocity or [] + self._status = status or [] async def _stream(self, msgs): for m in msgs: @@ -66,6 +68,9 @@ class FakeTelemetry: def velocity_ned(self): return self._stream(self._velocity) + def status_text(self): + return self._stream(self._status) + class FakeDrone: def __init__(self, telemetry=None): @@ -197,6 +202,35 @@ def test_start_telemetry_populates_cache_then_stop(): asyncio.run(scenario()) +# --- 飞控状态文本（STATUSTEXT）---------------------------------------------- +class Sev(Enum): + INFO = 1 + WARNING = 2 + + +def test_update_fc_status_and_take_messages(): + gw = Px4Gateway(FakeDrone()) + gw.update_fc_status(SimpleNamespace(type=Sev.WARNING, text="Low battery")) + gw.update_fc_status(SimpleNamespace(type="INFO", text="Takeoff detected")) + assert gw.take_fc_messages() == [("WARNING", "Low battery"), + ("INFO", "Takeoff detected")] + assert gw.take_fc_messages() == [] # 取后清空 + + +def test_status_text_subscription_captures_messages(): + telemetry = FakeTelemetry(status=[ + SimpleNamespace(type=Sev.WARNING, text="EKF variance")]) + gw = Px4Gateway(FakeDrone(telemetry)) + + async def scenario(): + await gw.start_telemetry() + await asyncio.sleep(0.05) + assert gw.take_fc_messages() == [("WARNING", "EKF variance")] + await gw.stop_telemetry() + + asyncio.run(scenario()) + + def test_default_own_state_is_invalid(): s = OwnState() assert s.has_position is False and s.position_valid is False diff --git a/utils/tests/stereo_ranger/__pycache__/config_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/stereo_ranger/__pycache__/config_test.cpython-312-pytest-9.1.1.pyc deleted file mode 100644 index 512adf7..0000000 Binary files a/utils/tests/stereo_ranger/__pycache__/config_test.cpython-312-pytest-9.1.1.pyc and /dev/null differ diff --git a/utils/tests/stereo_ranger/__pycache__/consistency_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/stereo_ranger/__pycache__/consistency_test.cpython-312-pytest-9.1.1.pyc deleted file mode 100644 index d496e09..0000000 Binary files a/utils/tests/stereo_ranger/__pycache__/consistency_test.cpython-312-pytest-9.1.1.pyc and /dev/null differ diff --git a/utils/tests/stereo_ranger/__pycache__/distance_filter_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/stereo_ranger/__pycache__/distance_filter_test.cpython-312-pytest-9.1.1.pyc deleted file mode 100644 index 4fceab3..0000000 Binary files a/utils/tests/stereo_ranger/__pycache__/distance_filter_test.cpython-312-pytest-9.1.1.pyc and /dev/null differ diff --git a/utils/tests/stereo_ranger/__pycache__/geometry_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/stereo_ranger/__pycache__/geometry_test.cpython-312-pytest-9.1.1.pyc deleted file mode 100644 index 9c90b65..0000000 Binary files a/utils/tests/stereo_ranger/__pycache__/geometry_test.cpython-312-pytest-9.1.1.pyc and /dev/null differ diff --git a/utils/tests/stereo_ranger/__pycache__/matching_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/stereo_ranger/__pycache__/matching_test.cpython-312-pytest-9.1.1.pyc deleted file mode 100644 index 1bd698d..0000000 Binary files a/utils/tests/stereo_ranger/__pycache__/matching_test.cpython-312-pytest-9.1.1.pyc and /dev/null differ diff --git a/utils/tests/stereo_ranger/__pycache__/sampling_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/stereo_ranger/__pycache__/sampling_test.cpython-312-pytest-9.1.1.pyc deleted file mode 100644 index a4a0409..0000000 Binary files a/utils/tests/stereo_ranger/__pycache__/sampling_test.cpython-312-pytest-9.1.1.pyc and /dev/null differ diff --git a/utils/tests/stereo_ranger/__pycache__/stereo_ranger_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/stereo_ranger/__pycache__/stereo_ranger_test.cpython-312-pytest-9.1.1.pyc deleted file mode 100644 index 7327cc5..0000000 Binary files a/utils/tests/stereo_ranger/__pycache__/stereo_ranger_test.cpython-312-pytest-9.1.1.pyc and /dev/null differ diff --git a/utils/tests/stereo_ranger/__pycache__/target_store_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/stereo_ranger/__pycache__/target_store_test.cpython-312-pytest-9.1.1.pyc deleted file mode 100644 index 10e67c5..0000000 Binary files a/utils/tests/stereo_ranger/__pycache__/target_store_test.cpython-312-pytest-9.1.1.pyc and /dev/null differ diff --git a/utils/tests/stereo_ranger/__pycache__/warn_throttle_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/stereo_ranger/__pycache__/warn_throttle_test.cpython-312-pytest-9.1.1.pyc deleted file mode 100644 index 81acf03..0000000 Binary files a/utils/tests/stereo_ranger/__pycache__/warn_throttle_test.cpython-312-pytest-9.1.1.pyc and /dev/null differ diff --git a/utils/tests/stereo_yolo/__pycache__/config_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/stereo_yolo/__pycache__/config_test.cpython-312-pytest-9.1.1.pyc deleted file mode 100644 index ee6a2b5..0000000 Binary files a/utils/tests/stereo_yolo/__pycache__/config_test.cpython-312-pytest-9.1.1.pyc and /dev/null differ diff --git a/utils/tests/stereo_yolo/__pycache__/frontend_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/stereo_yolo/__pycache__/frontend_test.cpython-312-pytest-9.1.1.pyc deleted file mode 100644 index 03e2558..0000000 Binary files a/utils/tests/stereo_yolo/__pycache__/frontend_test.cpython-312-pytest-9.1.1.pyc and /dev/null differ diff --git a/utils/tests/stereo_yolo/__pycache__/sbs_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/stereo_yolo/__pycache__/sbs_test.cpython-312-pytest-9.1.1.pyc deleted file mode 100644 index 565f880..0000000 Binary files a/utils/tests/stereo_yolo/__pycache__/sbs_test.cpython-312-pytest-9.1.1.pyc and /dev/null differ diff --git a/utils/tests/target_tracker/__pycache__/kalman_box_filter_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/target_tracker/__pycache__/kalman_box_filter_test.cpython-312-pytest-9.1.1.pyc deleted file mode 100644 index 0d7262b..0000000 Binary files a/utils/tests/target_tracker/__pycache__/kalman_box_filter_test.cpython-312-pytest-9.1.1.pyc and /dev/null differ diff --git a/utils/tests/target_tracker/__pycache__/target_tracker_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/target_tracker/__pycache__/target_tracker_test.cpython-312-pytest-9.1.1.pyc deleted file mode 100644 index 2a62dd2..0000000 Binary files a/utils/tests/target_tracker/__pycache__/target_tracker_test.cpython-312-pytest-9.1.1.pyc and /dev/null differ diff --git a/utils/tests/target_tracker/__pycache__/tracklet_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/target_tracker/__pycache__/tracklet_test.cpython-312-pytest-9.1.1.pyc deleted file mode 100644 index 78c56a8..0000000 Binary files a/utils/tests/target_tracker/__pycache__/tracklet_test.cpython-312-pytest-9.1.1.pyc and /dev/null differ diff --git a/utils/tests/task_runner/log_test.py b/utils/tests/task_runner/log_test.py index c68cbad..866d5b1 100644 --- a/utils/tests/task_runner/log_test.py +++ b/utils/tests/task_runner/log_test.py @@ -1,7 +1,7 @@ """log_test.py —— 任务日志模块单元测试。 覆盖：open 建目录/写表头、事件 JSONL、遥测 CSV、telemetry_every_n 节流、 -disabled 时零副作用、close 收尾。 +disabled 时零副作用、close 收尾、简洁任务简报（task_brief.log）。 """ import csv @@ -121,3 +121,84 @@ def test_append_keeps_existing_rows(tmp_path): assert len(rows) == 2 assert rows[0]["state"] == "wait" assert rows[1]["state"] == "goto" + + +# --- 简洁任务简报（task_brief.log）------------------------------------------- +def test_brief_log_lines_and_summary(tmp_path): + logger = TaskLogger(_cfg(tmp_path)) + logger.open() + logger.log_event(1000, "state", from_state="wait", to_state="wait", + reason="任务启动") + logger.log_event(2000, "state", from_state="wait", to_state="arm_takeoff", + reason="收到地面坐标") + logger.log_event(3000, "command", name="rtl") + logger.log_event(4000, "fc_status", severity="WARNING", text="Low battery\t") + logger.log_event(5000, "takeoff", altitude_m=5.0) + logger.log_event(6000, "rtl") + logger.log_telemetry(1000, TelemetryRecord( + state="wait", relative_altitude_m=0.0, distance_to_target_m=33.5)) + logger.log_telemetry(1100, TelemetryRecord( + state="arm_takeoff", relative_altitude_m=5.2, distance_to_target_m=20.0)) + logger.log_telemetry(1200, TelemetryRecord( + state="arm_takeoff", relative_altitude_m=5.0, + distance_to_target_m=float("inf"))) + logger.close() + + text = (tmp_path / "task_brief.log").read_text(encoding="utf-8") + assert "任务开始" in text + assert "[00:00.0] 任务启动（当前: 待命）" in text + assert "[00:01.0] 阶段: 待命 → 起飞（收到地面坐标）" in text + assert "[00:02.0] 地面站命令: rtl" in text + assert "[00:03.0] 飞控 WARNING: Low battery" in text # 尾随 tab 已 strip + assert "[00:04.0] 起飞至 5.0 m" in text + assert "[00:05.0] 开始返航" in text + assert "本次运行汇总" in text + assert "时长 0.2s" in text # 最后一行遥测的 t_s + assert "最高 5.2m" in text + assert "最近距目标 20.0m" in text # inf 不计入 + assert "事件 6 条" in text + assert "状态链 待命→起飞" in text + + +def test_brief_required_no_file_when_disabled_or_closed(tmp_path): + logger = TaskLogger(_cfg(tmp_path, enabled=False)) + logger.open() + logger.log_event(1000, "rtl") + logger.close() + assert not (tmp_path / "task_brief.log").exists() + + +# --- 遥测落盘节流（2026-10-15：主循环 20Hz 不得每行同步 flush）---------------- +def test_telemetry_buffered_until_flush_interval(tmp_path): + """telemetry_flush_ms 内只写不 flush；close 兜底落盘，一行不丢。""" + logger = TaskLogger(_cfg(tmp_path, telemetry_flush_ms=1000)) + logger.open() + rec = TelemetryRecord(state="track") + logger.log_telemetry(1000, rec) # 首行：落盘一次并打戳 + for i in range(1, 19): # 其后 19 行都在 1s 内 → 不再 flush + logger.log_telemetry(1000 + i * 50, rec) + logger.close() + + with open(tmp_path / "task_telemetry.csv", encoding="utf-8") as f: + rows = list(csv.DictReader(f)) + assert len(rows) == 19 + + +def test_telemetry_flush_zero_flushes_every_row(tmp_path): + """telemetry_flush_ms=0 保留旧的「每行立即 flush」行为（兼容/调试）。""" + logger = TaskLogger(_cfg(tmp_path, telemetry_flush_ms=0)) + logger.open() + logger.log_telemetry(1000, TelemetryRecord(state="goto")) + assert logger._last_tel_flush_ms == 1000 + logger.close() + + +def test_events_still_flushed_immediately(tmp_path): + """事件（阶段切换）仍即时落盘：地面站页面实时性不受遥测节流影响。""" + logger = TaskLogger(_cfg(tmp_path, telemetry_flush_ms=10_000)) + logger.open() + logger.log_event(1000, "state", from_state="goto", to_state="search") + with open(tmp_path / "task_events.jsonl", encoding="utf-8") as f: + lines = [json.loads(x) for x in f if x.strip()] + assert len(lines) == 1 + logger.close() diff --git a/utils/tests/task_runner/pod_control_test.py b/utils/tests/task_runner/pod_control_test.py index d3075c7..0394569 100644 --- a/utils/tests/task_runner/pod_control_test.py +++ b/utils/tests/task_runner/pod_control_test.py @@ -1,7 +1,8 @@ """pod_control_test.py —— 吊舱目标识别/锁定控制器单元测试。 覆盖：arm_detection 发 0x07(识别,目标类型)+0x0F(自动锁定)、 -stop_tracking 发 0x0E(停止)+0x08(识别关)。 +stop_tracking 发 0x0E(停止)+0x08(识别关)、地面站手动命令 +stop_track(0x0E)/auto_lock(0x0F+0x10)/center(0x2B)。 """ from pod_bridge.protocol import ControlCode @@ -43,3 +44,31 @@ def test_stop_tracking_sends_stop_and_detect_off(): assert len(bridge.sent) == 2 assert bridge.sent[0].code == ControlCode.STOP_TRACK assert bridge.sent[1].code == ControlCode.TARGET_DETECT_OFF + + +def test_stop_track_sends_only_stop(): + """地面站「停跟踪」= 只发 0x0E（0x0F 常驻自动锁定仍生效，对齐按键 u）。""" + bridge = FakeBridge() + pc = PodController(bridge) + pc.stop_track() + + assert [c.code for c in bridge.sent] == [ControlCode.STOP_TRACK] + + +def test_auto_lock_sends_autolock_and_detect_to_track(): + """地面站「自动锁定」= 0x0F + 0x10（对齐按键 a）。""" + bridge = FakeBridge() + pc = PodController(bridge) + pc.auto_lock() + + assert [c.code for c in bridge.sent] == [ControlCode.AUTO_LOCK_ON_FOUND, + ControlCode.DETECT_TO_TRACK] + + +def test_center_sends_center(): + """地面站「归中」= 0x2B（对齐按键 c）。""" + bridge = FakeBridge() + pc = PodController(bridge) + pc.center() + + assert [c.code for c in bridge.sent] == [ControlCode.CENTER] diff --git a/utils/tests/task_runner/return_home_test.py b/utils/tests/task_runner/return_home_test.py new file mode 100644 index 0000000..ff73a96 --- /dev/null +++ b/utils/tests/task_runner/return_home_test.py @@ -0,0 +1,142 @@ +"""return_home_test.py —— 自定义返航（回起飞点 + 恢复初始航向）纯逻辑测试。 + +覆盖：转场速度/对准门控、到达判定、转场高度保持（含符号）、初始航向恢复、 +角度回绕、以及"不超过转场高度"的语义。 +""" + +import math + +import pytest + +from task_runner.return_home import (HomingController, ReturnConfig, + wrap_deg_180) + +HOME_LAT, HOME_LON = 47.397971, 8.546164 +_DEG_LAT_M = 111320.0 + + +def _offset(lat0, lon0, north_m, east_m): + return (lat0 + north_m / _DEG_LAT_M, + lon0 + east_m / (_DEG_LAT_M * math.cos(math.radians(lat0)))) + + +def _ctrl(cfg=None, initial_yaw_deg=0.0, transit_alt_m=5.0): + return HomingController(cfg or ReturnConfig(), HOME_LAT, HOME_LON, + math.radians(initial_yaw_deg), transit_alt_m) + + +# --- 角度回绕 --------------------------------------------------------------- +@pytest.mark.parametrize("deg,expect", [ + (0.0, 0.0), (179.0, 179.0), (181.0, -179.0), + (-181.0, 179.0), (360.0, 0.0), (-360.0, 0.0), +]) +def test_wrap_deg_180(deg, expect): + assert wrap_deg_180(deg) == pytest.approx(expect) + + +# --- 转场：朝起飞点飞 ------------------------------------------------------- +def test_far_away_aligned_commands_full_speed(): + """远在起飞点北侧、机头朝南（对准回家方向）→ 满速前进、几乎不转。""" + cfg = ReturnConfig(speed_mps=2.0) + c = _ctrl(cfg) + # 在起飞点正北 50m，机头朝南（yaw=180°）→ 应朝南飞（北向速度为负） + lat, lon = _offset(HOME_LAT, HOME_LON, 50.0, 0.0) + out = c.update(lat, lon, math.radians(180.0), 5.0) + assert out.arrived is False + assert out.distance_m == pytest.approx(50.0, rel=5e-2) + assert out.vx_mps == pytest.approx(2.0, rel=1e-2) # 对准 → 全速 + assert abs(out.yaw_rate_dps) < 1e-6 + + +def test_misaligned_gates_forward_speed(): + """机头与回家方向垂直（90°）→ 前向速度被 cos(err) 门控为 0，只转向。""" + c = _ctrl(ReturnConfig(speed_mps=2.0, yaw_rate_limit_dps=20.0)) + lat, lon = _offset(HOME_LAT, HOME_LON, 0.0, 50.0) # 起飞点正东 50m + out = c.update(lat, lon, math.radians(0.0), 5.0) # 机头朝北 + assert out.vx_mps == pytest.approx(0.0, abs=1e-3) # cos(90°)=0（含地理近似误差） + assert out.bearing_err_deg == pytest.approx(-90.0, abs=1e-2) # 回家方向在西 + assert out.yaw_rate_dps < 0.0 # 往左转（目标在西侧） + + +def test_yaw_rate_limited(): + c = _ctrl(ReturnConfig(yaw_kp=5.0, yaw_rate_limit_dps=20.0)) + lat, lon = _offset(HOME_LAT, HOME_LON, 0.0, 50.0) + out = c.update(lat, lon, math.radians(0.0), 5.0) + assert out.yaw_rate_dps == pytest.approx(-20.0) # 被限幅（左转） + + +def test_arrival_stops_forward_motion(): + """进入到达半径 → arrived，且不再有前向速度。""" + c = _ctrl(ReturnConfig(speed_mps=2.0, arrival_radius_m=3.0)) + lat, lon = _offset(HOME_LAT, HOME_LON, 1.0, 0.0) # 距起飞点 1m + out = c.update(lat, lon, math.radians(180.0), 5.0) + assert out.arrived is True + assert out.vx_mps == 0.0 + + +def test_distance_measured_to_home(): + c = _ctrl() + lat, lon = _offset(HOME_LAT, HOME_LON, 30.0, 40.0) # 3-4-5 三角形 → 50m + out = c.update(lat, lon, 0.0, 5.0) + assert out.distance_m == pytest.approx(50.0, rel=5e-2) + + +# --- 转场高度保持（NED 下正）------------------------------------------------ +def test_alt_hold_climbs_when_below_transit(): + c = _ctrl(transit_alt_m=5.0) + vz = c.alt_hold_vz(3.0) # 低于转场高度 + assert vz < 0.0 # NED 下正 → 负 = 爬升 + + +def test_alt_hold_descends_when_above_transit(): + c = _ctrl(transit_alt_m=5.0) + vz = c.alt_hold_vz(7.0) + assert vz > 0.0 # 正 = 下降 + + +def test_alt_hold_zero_at_target(): + assert _ctrl(transit_alt_m=5.0).alt_hold_vz(5.0) == pytest.approx(0.0) + + +def test_alt_hold_rate_limited(): + cfg = ReturnConfig(alt_kp=10.0, alt_limit_mps=0.5) + c = _ctrl(cfg, transit_alt_m=5.0) + assert c.alt_hold_vz(0.0) == pytest.approx(-0.5) # 限幅 + assert c.alt_hold_vz(100.0) == pytest.approx(0.5) + + +# --- 恢复初始航向 ----------------------------------------------------------- +def test_yaw_align_rate_and_direction(): + c = _ctrl(ReturnConfig(align_kp=1.0, align_rate_limit_dps=15.0), + initial_yaw_deg=0.0) + err, rate = c.yaw_align(math.radians(10.0)) # 当前偏右 10° + assert err == pytest.approx(-10.0) # 需要往左转 + assert rate == pytest.approx(-10.0) # 负 = 左转 + + +def test_yaw_align_limited(): + c = _ctrl(ReturnConfig(align_kp=10.0, align_rate_limit_dps=15.0), + initial_yaw_deg=0.0) + _, rate = c.yaw_align(math.radians(90.0)) + assert rate == pytest.approx(-15.0) + + +def test_yaw_aligned_within_tolerance(): + c = _ctrl(ReturnConfig(yaw_align_tolerance_deg=5.0), initial_yaw_deg=30.0) + assert c.yaw_aligned(math.radians(33.0)) is True # 差 3° < 5° + assert c.yaw_aligned(math.radians(40.0)) is False # 差 10° + + +def test_yaw_align_wraps_around_north(): + """初始航向 179°、当前 -179° → 误差 2°（不是 358°）。""" + c = _ctrl(initial_yaw_deg=179.0) + err, _ = c.yaw_align(math.radians(-179.0)) + assert abs(err) == pytest.approx(2.0, abs=1e-6) + + +# --- 配置默认值 ------------------------------------------------------------- +def test_default_config_is_non_climbing_and_enabled(): + cfg = ReturnConfig() + assert cfg.enabled is True + assert cfg.alt_limit_mps > 0.0 + assert cfg.speed_mps > 0.0 diff --git a/utils/tests/task_runner/runner_test.py b/utils/tests/task_runner/runner_test.py index a2c78ed..a24d471 100644 --- a/utils/tests/task_runner/runner_test.py +++ b/utils/tests/task_runner/runner_test.py @@ -25,10 +25,17 @@ class FakePx4: self.telemetry_started = 0 self.offboard_started = False self.brake_calls = [] + self.yaw_calls = [] # SEARCH 扫视下发记录 + self.velocity_calls = [] # send_velocity_heading 记录（返航转场用） + self.fc_msgs = [] def own_state(self): return replace(self._state) + def take_fc_messages(self): + msgs, self.fc_msgs = self.fc_msgs, [] + return msgs + async def start_telemetry(self): self.telemetry_started += 1 @@ -43,19 +50,26 @@ class FakePx4: pass async def send_velocity_heading(self, out, t=None): + self.velocity_calls.append(out) return True async def send_brake_hover(self, t=None): self.brake_calls.append(t) return True + async def send_yaw_rate(self, yaw_rate_dps): + self.yaw_calls.append(yaw_rate_dps) + return True + class FakeBridge: def __init__(self): self.started = False self.stopped = False + self.landing_stopped = False # landing_stop()：只退 offboard、保留遥测 self.law = None # set_heading_servo_enabled 最后值 self.steps = 0 + self.search_steps = [] # SEARCH 态收到的扫视角速度 self._locked = False self._telemetry = None @@ -66,6 +80,9 @@ class FakeBridge: async def stop(self): self.stopped = True + async def landing_stop(self): + self.landing_stopped = True + def set_heading_servo_enabled(self, enabled): self.law = enabled @@ -75,6 +92,9 @@ class FakeBridge: async def step(self, now_ms): self.steps += 1 + async def search_step(self, now_ms, yaw_rate_dps): + self.search_steps.append(yaw_rate_dps) + @property def latest_telemetry(self): return self._telemetry @@ -83,10 +103,16 @@ class FakeBridge: class FakeGround: def __init__(self, target=None): self._target = target + self.commands = [] # 地面站命令队列（供 drain_commands 取走） def latest(self): return self._target or GroundTarget() + def drain_commands(self): + cmds = list(self.commands) + self.commands.clear() + return cmds + class FakeDrone: def __init__(self): @@ -96,6 +122,25 @@ class FakeDrone: self.rtl = False self.landed = False self.disarmed = False + self.telemetry = FakeDrone._Telemetry() + + class _Telemetry: + """模仿 MAVSDK `in_air()`：**异步生成器**，finalize 时取消底层流。 + + 真实实现在 finally 里 `in_air_stream.cancel()`；「取一帧就 break」只是 + 把生成器挂起，复用同一生成器即可避免反复订阅。 + """ + + def __init__(self, frames=(False,)): + self._frames = frames + self.cancelled = False + + async def in_air(self): + try: + for value in self._frames: + yield value + finally: + self.cancelled = True class _Action: def __init__(self, d): @@ -132,7 +177,8 @@ def _state(now_ms, lat=0.0, lon=0.0, rel_alt=0.0): def _make_runner(px4=None, bridge=None, ground=None, drone=None, - sm_cfg=None, vision_fn=None, pod_control=None): + sm_cfg=None, pod_control=None, + land_instead_of_rtl=False, now_ms_fn=None): cfg = BridgeConfig() cfg.ground_inject.enabled = True cfg.ground_inject.timeout_ms = 5000 @@ -143,9 +189,9 @@ def _make_runner(px4=None, bridge=None, ground=None, drone=None, TaskStateMachine(sm_cfg or TaskStateConfig()), ground or FakeGround(), drone or FakeDrone(), - vision_fn=vision_fn, - now_ms_fn=lambda: 1000, + now_ms_fn=now_ms_fn or (lambda: 1000), pod_control=pod_control, + land_instead_of_rtl=land_instead_of_rtl, ) @@ -180,6 +226,15 @@ def test_collect_takeoff_reached(): assert r._collect_inputs(1000).takeoff_reached is True +def test_collect_takeoff_reached_tolerance(): + """容差内（目标 5.0m，实际 4.6m ≥ 5.0-0.5）判到达；容差外（4.4m）判未到达。""" + sm_cfg = TaskStateConfig(takeoff_altitude_m=5.0) + px4_low = FakePx4(_state(1000, rel_alt=4.6)) + assert _make_runner(px4=px4_low, sm_cfg=sm_cfg)._collect_inputs(1000).takeoff_reached is True + px4_below = FakePx4(_state(1000, rel_alt=4.4)) + assert _make_runner(px4=px4_below, sm_cfg=sm_cfg)._collect_inputs(1000).takeoff_reached is False + + def test_collect_locked_from_bridge(): bridge = FakeBridge() bridge._locked = True @@ -188,14 +243,6 @@ def test_collect_locked_from_bridge(): assert r._collect_inputs(1000).target_locked is True -def test_collect_vision_second_judgement(): - r = _make_runner(vision_fn=lambda: True) - assert r._collect_inputs(1000).vision_has_target is True - - r2 = _make_runner(vision_fn=lambda: False) - assert r2._collect_inputs(1000).vision_has_target is False - - # --- 居中累加 --------------------------------------------------------------- def test_centered_accumulates_duration(): bridge = FakeBridge() @@ -257,6 +304,15 @@ def test_on_enter_rtl_stops_bridge_and_returns(): assert drone.rtl is True +def test_land_instead_of_rtl_lands(): + """--land 模式：收尾调 land 而不是 return_to_launch（真机就地回收）。""" + drone = FakeDrone() + r = _make_runner(drone=drone, land_instead_of_rtl=True) + asyncio.run(r._on_enter(TaskState.RTL, 1000)) + assert drone.landed is True + assert drone.rtl is False + + # --- 持续动作 --------------------------------------------------------------- def test_tick_goto_steps_bridge(): bridge = FakeBridge() @@ -281,12 +337,24 @@ class FakePodControl: def __init__(self): self.armed = False self.stopped = False + self.calls = [] def arm_detection(self): self.armed = True + self.calls.append("arm_detection") def stop_tracking(self): self.stopped = True + self.calls.append("stop_tracking") + + def stop_track(self): + self.calls.append("stop_track") + + def auto_lock(self): + self.calls.append("auto_lock") + + def center(self): + self.calls.append("center") def test_on_enter_goto_arms_detection(): @@ -309,3 +377,596 @@ def test_no_pod_control_is_safe(): r = _make_runner(pod_control=None) asyncio.run(r._on_enter(TaskState.GOTO, 1000)) asyncio.run(r._on_enter(TaskState.RTL, 1000)) + + +# --- 地面站命令分派 --------------------------------------------------------- +def test_ground_command_rtl_sets_requested(): + ground = FakeGround() + ground.commands.append("rtl") + r = _make_runner(ground=ground) + + r._apply_ground_commands() + assert r._collect_inputs(1000).rtl_requested is True + + +def test_ground_command_emergency_sets_flag(): + ground = FakeGround() + ground.commands.append("emergency") + r = _make_runner(ground=ground) + + r._apply_ground_commands() + assert r._collect_inputs(1000).emergency is True + + +def test_ground_command_pod_dispatch(): + pod_control = FakePodControl() + ground = FakeGround() + ground.commands.extend(["pod_detect_on", "pod_auto_lock", "pod_stop_track", + "pod_detect_off", "pod_center"]) + r = _make_runner(ground=ground, pod_control=pod_control) + + r._apply_ground_commands() + assert pod_control.calls == ["arm_detection", "auto_lock", "stop_track", + "stop_tracking", "center"] + + +def test_ground_command_pod_without_control_is_ignored(): + """SITL（无 pod_control）→ 吊舱命令记警告忽略，不抛异常。""" + ground = FakeGround() + ground.commands.append("pod_center") + r = _make_runner(ground=ground, pod_control=None) + + r._apply_ground_commands() # 不抛异常即通过 + + +def test_ground_command_unknown_is_ignored(): + ground = FakeGround() + ground.commands.append("unknown_cmd") + r = _make_runner(ground=ground) + + r._apply_ground_commands() # 不抛异常即通过 + + +def test_run_drains_ground_commands(): + """run() 主循环每拍排空命令队列：RTL 命令 → 状态机转 RTL → 调 return_to_launch。""" + ground = FakeGround() + ground.commands.append("rtl") + drone = FakeDrone() + r = _make_runner(ground=ground, drone=drone) + + async def scenario(): + stop = asyncio.Event() + task = asyncio.ensure_future(r.run(stop)) + try: + for _ in range(100): + if drone.rtl: + break + await asyncio.sleep(0.01) + finally: + stop.set() + await task + + asyncio.run(scenario()) + assert r.state is TaskState.RTL + assert drone.rtl is True + + +# --- 飞控状态文本落日志 ----------------------------------------------------- +class FakeLogger: + def __init__(self): + self.events = [] + + def open(self): + pass + + def close(self): + pass + + def log_event(self, now_ms, event_type, **fields): + self.events.append((event_type, fields)) + + def log_telemetry(self, now_ms, rec): + pass + + +def test_fc_messages_logged_as_events(): + px4 = FakePx4(_state(1000)) + px4.fc_msgs = [("WARNING", "Low battery"), ("INFO", "Takeoff detected")] + logger = FakeLogger() + r = _make_runner(px4=px4) + r._logger = logger + + r._log_fc_messages() + assert [e[0] for e in logger.events] == ["fc_status", "fc_status"] + assert logger.events[0][1] == {"severity": "WARNING", "text": "Low battery"} + assert px4.fc_msgs == [] # 已排空，不重复记录 + + +# --- SEARCH 搜索扫视（2026-10-15 新增）-------------------------------------- +def test_on_enter_search_disables_servo_and_resets_sweep(): + bridge = FakeBridge() + r = _make_runner(bridge=bridge) + r._sweep_dir = 9 # 脏值，应被重置 + asyncio.run(r._on_enter(TaskState.SEARCH, 1000)) + assert bridge.law is False # 搜索不用控制律（不由 L1/L2 出指令） + assert r._sweep_dir == 0 # 每次进搜索从右向起扫 + assert r._sweep_since_ms == 1000 + + +def test_search_yaw_rate_sweeps_both_directions(): + """扫视按 sweep_reverse_s 左右交替，幅值为配置角速度。""" + sm_cfg = TaskStateConfig(sweep_rate_dps=15.0, sweep_reverse_s=4.0) + r = _make_runner(sm_cfg=sm_cfg) + assert r._search_yaw_rate(1000) == pytest.approx(15.0) # 先向右 + assert r._search_yaw_rate(4999) == pytest.approx(15.0) # 未到换向点 + assert r._search_yaw_rate(5000) == pytest.approx(-15.0) # 换向左 + assert r._search_yaw_rate(9000) == pytest.approx(15.0) # 再换回右 + + +def test_search_yaw_rate_zero_disables_sweep(): + sm_cfg = TaskStateConfig(sweep_rate_dps=0.0, sweep_reverse_s=4.0) + r = _make_runner(sm_cfg=sm_cfg) + assert r._search_yaw_rate(1000) == 0.0 + + +def test_search_yaw_rate_continuous_when_reverse_disabled(): + """sweep_reverse_s=0 → 单向持续扫视（不换向）。""" + sm_cfg = TaskStateConfig(sweep_rate_dps=10.0, sweep_reverse_s=0.0) + r = _make_runner(sm_cfg=sm_cfg) + assert r._search_yaw_rate(1000) == pytest.approx(10.0) + assert r._search_yaw_rate(600_000) == pytest.approx(10.0) + + +def test_tick_search_sends_sweep_step(): + bridge = FakeBridge() + sm_cfg = TaskStateConfig(sweep_rate_dps=12.0, sweep_reverse_s=4.0) + r = _make_runner(bridge=bridge, sm_cfg=sm_cfg) + r._sm.force_state(TaskState.SEARCH, 1000) + asyncio.run(r._tick(1000)) + assert bridge.search_steps == [pytest.approx(12.0)] + assert bridge.steps == 0 # 搜索不走控制律 step + + +def test_search_step_polls_pod_so_lock_can_be_fresh(): + """SEARCH 必须 poll 吊舱：否则遥测超龄，搜到目标也切不进 TRACK。""" + from pod_bridge.bridge import PodBridge + from pod_bridge.protocol import Telemetry + + class FakePod: + def __init__(self): + self.polls = 0 + + def poll(self): + self.polls += 1 + return [Telemetry(status1=0b100, off_target_x_deg=1.0, + off_target_y_deg=0.0)] + + def send(self, data): + pass + + def close(self): + pass + + pod = FakePod() + px4 = FakePx4(_state(1000)) + cfg = BridgeConfig() + cfg.link.require_laser_valid = False # 本吊舱无激光（与部署配置一致） + bridge = PodBridge(cfg, px4, pod, now_ms_fn=lambda: 1000, + ground_target_fn=lambda: None) + asyncio.run(bridge.search_step(1000, -15.0)) + + assert pod.polls == 1 + assert bridge.pod_locked(1000) is True # 本拍锁定位已刷新 + assert px4.yaw_calls == [pytest.approx(-15.0)] # 扫视角速度已下发 + + +# --- 进入动作只执行一次（防重入导致的自激）---------------------------------- +class _CountingPodControl: + def __init__(self): + self.arm_calls = 0 + self.stop_calls = 0 + + def arm_detection(self): + self.arm_calls += 1 + + def stop_tracking(self): + self.stop_calls += 1 + + def stop_track(self): + pass + + def auto_lock(self): + pass + + def center(self): + pass + + +def test_pod_detection_armed_only_once_across_reentry(): + """丢目标回 SEARCH 再进 TRACK 时，不得重发 0x07/0x0F（会打断正在进行的跟踪）。""" + pod_control = _CountingPodControl() + r = _make_runner(pod_control=pod_control) + asyncio.run(r._on_enter(TaskState.GOTO, 1000)) + asyncio.run(r._on_enter(TaskState.SEARCH, 2000)) + asyncio.run(r._on_enter(TaskState.TRACK, 3000)) + asyncio.run(r._on_enter(TaskState.SEARCH, 4000)) # 丢目标回搜索 + asyncio.run(r._on_enter(TaskState.TRACK, 5000)) # 再次锁定 + assert pod_control.arm_calls == 1 + + +def test_rtl_stops_pod_tracking_even_after_reentry(): + pod_control = _CountingPodControl() + r = _make_runner(pod_control=pod_control) + asyncio.run(r._on_enter(TaskState.GOTO, 1000)) + asyncio.run(r._on_enter(TaskState.RTL, 2000)) + assert pod_control.stop_calls == 1 + + +# --- 主循环异常兜底：必须返航，不能静默停摆 --------------------------------- +def test_iteration_exception_triggers_rtl(): + """主循环单周期异常 → 必须转紧急返航，不能只停机静默停摆。 + + 异常注入在**每拍都跑**的输入采集路径（own_state），与当前状态无关。 + """ + px4 = FakePx4(_state(1000)) + drone = FakeDrone() + r = _make_runner(px4=px4, drone=drone) + calls = {"n": 0} + + def boom(): + calls["n"] += 1 + raise RuntimeError("模拟瞬态异常（遥测/串口抖动）") + + px4.own_state = boom + + async def scenario(): + stop = asyncio.Event() + task = asyncio.ensure_future(r.run(stop)) + for _ in range(300): + if drone.rtl: + break + await asyncio.sleep(0.005) + stop.set() + task.cancel() + try: + await task + except asyncio.CancelledError: + pass + + asyncio.run(scenario()) + assert calls["n"] >= 1 + assert drone.rtl is True # 异常兜底确实触发了返航 + assert r.state is TaskState.RTL + + +def test_iteration_exception_records_event(): + """异常兜底要在任务日志留痕，便于事后定位。""" + px4 = FakePx4(_state(1000)) + drone = FakeDrone() + r = _make_runner(px4=px4, drone=drone) + logger = FakeLogger() + r._logger = logger + + def boom(): + raise RuntimeError("串口拔线") + + px4.own_state = boom + + async def scenario(): + stop = asyncio.Event() + task = asyncio.ensure_future(r.run(stop)) + for _ in range(300): + if drone.rtl: + break + await asyncio.sleep(0.005) + stop.set() + task.cancel() + try: + await task + except asyncio.CancelledError: + pass + + asyncio.run(scenario()) + types = [e[0] for e in logger.events] + assert "iteration_error" in types + assert "rtl" in types # 兜底返航事件也落盘 + + +def test_safe_abort_is_noop_when_already_rtl(): + """已在 RTL 时兜底不重复触发返航（避免重复 RTL 指令）。""" + bridge = FakeBridge() + drone = FakeDrone() + r = _make_runner(bridge=bridge, drone=drone) + r._sm.force_state(TaskState.RTL, 0) + asyncio.run(r._safe_abort()) + assert drone.rtl is False + assert bridge.stopped is False + + +# --- in_air 遥测流复用（降低服务端订阅churn）-------------------------------- +def test_in_air_stream_reused_across_ticks(): + """in_air 流只订阅一次：每拍新建会在服务端反复订阅/退订同一条流。 + + 注意必须在**同一个事件循环**内连续调用（与生产一致：主循环是单 loop）； + 跨 loop 复用 async generator 会被 asyncio 拒绝，实现里也就丢弃重建。 + """ + drone = FakeDrone() + calls = {"n": 0} + + class CountingTelemetry: + def in_air(self): + calls["n"] += 1 + return FakeDrone._Telemetry().in_air() + + drone.telemetry = CountingTelemetry() + r = _make_runner(drone=drone) + + async def scenario(): + for _ in range(5): + await r._check_landed() + + asyncio.run(scenario()) + assert calls["n"] == 1 # 只订阅一次 + assert r.rtl_done is True # in_air=False → 判落地并 disarm + + +def test_in_air_stream_rebuilt_when_broken(): + """流建立/消费失败时丢弃缓存，下拍重建（不永久卡在坏流上）。""" + drone = FakeDrone() + calls = {"n": 0} + + def broken(): + calls["n"] += 1 + raise RuntimeError("stream broken") + + class FlakyTelemetry: + def in_air(self): + return broken() + + drone.telemetry = FlakyTelemetry() + r = _make_runner(drone=drone) + + async def scenario(): + await r._check_landed() + assert calls["n"] == 1 + assert r._in_air_stream is None # 已丢弃 + await r._check_landed() + assert calls["n"] == 2 # 重建后再试 + + asyncio.run(scenario()) + + +# --- RTL 落地等待上限（真机在环发现：判不出落地会无限等）-------------------- +def test_rtl_timeout_ends_task_when_landing_unobservable(): + """in_air 始终不报落地 → 超过 rtl_timeout_s 后结束任务，不无限等待。""" + now = {"ms": 1_000_000} + drone = FakeDrone() + + class AlwaysAirborne: + async def in_air(self): + while True: + yield True + + drone.telemetry = AlwaysAirborne() + sm_cfg = TaskStateConfig(rtl_timeout_s=5.0) + r = _make_runner(drone=drone, sm_cfg=sm_cfg, + now_ms_fn=lambda: now["ms"]) + r._rtl_entered_ms = now["ms"] + r._rtl_phase = "land" + + async def scenario(): + now["ms"] += 4_000 # 未到上限：不应结束 + await r._tick_rtl(now["ms"]) + assert r.rtl_done is False + now["ms"] += 2_000 # 超过上限：结束任务 + await r._tick_rtl(now["ms"]) + assert r.rtl_done is True + + asyncio.run(scenario()) + + +def test_rtl_timeout_not_triggered_after_landing(): + """已判落地后不再触发超时逻辑（rtl_done 保持）。""" + now = {"ms": 1_000_000} + drone = FakeDrone() # 默认 in_air 首帧 False + sm_cfg = TaskStateConfig(rtl_timeout_s=1.0) + r = _make_runner(drone=drone, sm_cfg=sm_cfg, + now_ms_fn=lambda: now["ms"]) + r._rtl_entered_ms = now["ms"] + r._rtl_phase = "land" + + async def scenario(): + await r._tick_rtl(now["ms"]) + assert r.rtl_done is True + now["ms"] += 60_000 # 大幅超过上限 + await r._tick_rtl(now["ms"]) + assert r.rtl_done is True + + asyncio.run(scenario()) + + +# --- 自定义返航：回起飞点 → 恢复初始航向 → 降落 ----------------------------- +def _armed_runner(px4=None, drone=None, return_cfg=None): + """走完 解锁→GOTO（记录起飞点 + 进入 offboard）的 runner。""" + px4 = px4 or FakePx4(_state(1000, lat=0.0, lon=0.0)) + r = _make_runner(px4=px4, drone=drone or FakeDrone()) + if return_cfg is not None: + r._return_cfg = return_cfg + asyncio.run(r._on_enter(TaskState.ARM_TAKEOFF, 1000)) + asyncio.run(r._on_enter(TaskState.GOTO, 1000)) + return r, px4 + + +def test_custom_return_selected_after_takeoff(): + """已起飞 + 有起飞点 → 进入 RTL 走自定义返航，不调 PX4 RTL。""" + drone = FakeDrone() + r, _ = _armed_runner(drone=drone) + asyncio.run(r._on_enter(TaskState.RTL, 2000)) + assert r._rtl_phase == "return" + assert drone.rtl is False # 不再交给 PX4 原生 RTL + assert r._bridge_started is True + + +def test_custom_return_records_home_and_initial_yaw(): + r, px4 = _armed_runner() + assert r._home_lat == pytest.approx(0.0) + assert r._home_lon == pytest.approx(0.0) + assert r._initial_yaw_rad == pytest.approx(0.0) + + +def test_return_step_commands_velocity_toward_home(): + px4 = FakePx4(_state(1000, lat=0.0, lon=0.0)) + r, _ = _armed_runner(px4=px4) + asyncio.run(r._on_enter(TaskState.RTL, 2000)) + + # 飞到起飞点正北 ~111m，机头朝南（对准回家方向） + px4._state = _state(3000, lat=0.001, lon=0.0, rel_alt=5.0) + px4._state.yaw_rad = 3.141592653589793 + px4.velocity_calls.clear() + asyncio.run(r._tick_rtl(3000)) + + assert r._rtl_phase == "return" + assert len(px4.velocity_calls) == 1 + out = px4.velocity_calls[0] + assert out.vx_body_mps > 0.0 # 朝起飞点前进 + assert out.vz_mps == pytest.approx(0.0) # 转场高度 5m = 当前高度 + + +def test_return_arrival_switches_to_yaw_phase(): + px4 = FakePx4(_state(1000, lat=0.0, lon=0.0)) + r, _ = _armed_runner(px4=px4) + asyncio.run(r._on_enter(TaskState.RTL, 2000)) + + px4._state = _state(3000, lat=0.000005, lon=0.0, rel_alt=5.0) # ~0.5m + asyncio.run(r._tick_rtl(3000)) + assert r._rtl_phase == "yaw" + + +def test_yaw_phase_restores_heading_then_lands(): + drone = FakeDrone() + px4 = FakePx4(_state(1000, lat=0.0, lon=0.0)) + r, _ = _armed_runner(px4=px4, drone=drone) + asyncio.run(r._on_enter(TaskState.RTL, 2000)) + r._rtl_phase = "yaw" + r._yaw_started_ms = 2000 + + px4._state = _state(3000, lat=0.0, lon=0.0, rel_alt=5.0) # 航向已回 0 + px4.velocity_calls.clear() + asyncio.run(r._tick_rtl(3000)) + + assert len(px4.velocity_calls) == 1 # 先发了一拍"原地保持"指令 + assert r._rtl_phase == "land" + assert drone.landed is True # 航向对齐后降落 + assert r._land_cmd_sent is True + + +def test_landing_keeps_telemetry_running(): + """降落阶段只退 offboard、保留遥测（否则日志冻结在降落前高度）。""" + drone = FakeDrone() + bridge = FakeBridge() + r, _ = _armed_runner(drone=drone) + r._bridge = bridge + asyncio.run(r._start_land()) + assert bridge.landing_stopped is True + assert bridge.stopped is False # 尚未全停：遥测/串口仍在 + assert drone.landed is True + + +def test_land_phase_completes_on_landed(): + drone = FakeDrone() # in_air -> False + r, _ = _armed_runner(drone=drone) + asyncio.run(r._on_enter(TaskState.RTL, 2000)) + r._rtl_phase = "land" + asyncio.run(r._tick_rtl(3000)) + assert r.rtl_done is True + assert drone.disarmed is True + + +def test_return_timeout_falls_back_to_px4_rtl(): + drone = FakeDrone() + r, px4 = _armed_runner(drone=drone) + asyncio.run(r._on_enter(TaskState.RTL, 2000)) + + px4._state = _state(3000, lat=0.001, lon=0.0, rel_alt=5.0) + # 超过转场超时 + late = 2000 + int(r._return_cfg.timeout_s * 1000) + 1000 + asyncio.run(r._tick_rtl(late)) + assert r._rtl_phase == "fallback" + assert drone.rtl is True # 退回 PX4 原生 RTL + + +def test_custom_return_disabled_uses_px4_rtl(): + from task_runner.return_home import ReturnConfig + + drone = FakeDrone() + r, _ = _armed_runner(drone=drone, + return_cfg=ReturnConfig(enabled=False)) + asyncio.run(r._on_enter(TaskState.RTL, 2000)) + assert r._rtl_phase == "fallback" + assert drone.rtl is True + + +def test_rtl_before_takeoff_uses_px4_rtl(): + """未起飞（还在待命/地面）时不能自主返航 → 退回 PX4 RTL。""" + drone = FakeDrone() + r = _make_runner(drone=drone) # 未记录起飞点、未进 offboard + asyncio.run(r._on_enter(TaskState.RTL, 2000)) + assert r._rtl_phase == "fallback" + assert drone.rtl is True + + +def test_safe_abort_prefers_px4_fallback(): + """异常兜底不再信任自身状态：强制走 PX4 RTL。""" + drone = FakeDrone() + r, _ = _armed_runner(drone=drone) + asyncio.run(r._safe_abort()) + assert r._rtl_phase == "fallback" + assert drone.rtl is True + + +# --- 起飞点记录时机（SITL 实测：解锁时 gps_info 可能还没到）------------------ +def test_home_recorded_when_position_becomes_valid_during_takeoff(): + """解锁瞬间定位无效 → 起飞阶段每拍补记，不丢起飞点。""" + px4 = FakePx4(_state(1000, lat=0.0, lon=0.0)) + px4._state.position_valid = False + r = _make_runner(px4=px4) + r._sm.force_state(TaskState.ARM_TAKEOFF, 0) + + r._collect_inputs(1000) + assert r._home_lat is None # 定位无效 → 暂不记录 + + px4._state = _state(1000, lat=0.0012, lon=0.0034) + r._collect_inputs(1100) + assert r._home_lat == pytest.approx(0.0012) # 补记成功 + assert r._home_lon == pytest.approx(0.0034) + + +def test_home_not_recorded_once_airborne(): + """进入 GOTO（已在空中）后不再把当前位置当作起飞点。""" + px4 = FakePx4(_state(1000, lat=0.005, lon=0.0)) + r = _make_runner(px4=px4) + r._sm.force_state(TaskState.GOTO, 0) + r._collect_inputs(1000) + assert r._home_lat is None + + +def test_home_recorded_only_once(): + px4 = FakePx4(_state(1000, lat=0.0, lon=0.0)) + r = _make_runner(px4=px4) + r._sm.force_state(TaskState.ARM_TAKEOFF, 0) + r._collect_inputs(1000) + first = (r._home_lat, r._home_lon, r._initial_yaw_rad) + + px4._state = _state(1000, lat=0.009, lon=0.009) # 位置已变（飞机在爬升/移动） + r._collect_inputs(1100) + assert (r._home_lat, r._home_lon, r._initial_yaw_rad) == first + + +def test_record_home_returns_false_without_fix(): + px4 = FakePx4(_state(1000)) + px4._state.position_valid = False + r = _make_runner(px4=px4) + assert r._record_home() is False + assert r._home_lat is None + + diff --git a/utils/tests/task_runner/state_machine_test.py b/utils/tests/task_runner/state_machine_test.py index 95e8b1f..42e1a60 100644 --- a/utils/tests/task_runner/state_machine_test.py +++ b/utils/tests/task_runner/state_machine_test.py @@ -1,8 +1,12 @@ """state_machine_test.py —— 任务级状态机单元测试。 -覆盖：完整状态链 WAIT→ARM_TAKEOFF→GOTO→TRACK→APPROACH→RTL、各状态超时、 -GOTO→TRACK 的锁定/视觉守卫、TRACK 丢目标退回 GOTO、紧急/返航全局终止、 -RTL 终态与 reset。 +覆盖（2026-10-15 按「到参考点 → 视觉搜索 → 缓慢靠近」重构后的契约）： + * 完整状态链 WAIT→ARM_TAKEOFF→GOTO→SEARCH→TRACK→(超时)RTL + * GOTO 只按「到参考点半径」退出（不看锁定）；SEARCH 才看锁定/视觉守卫 + * 搜索超时 → RTL；追踪（靠近）超时 → RTL + * TRACK 丢目标 → 回 SEARCH（不回 GOTO）；坐标同时失效 → RTL + * TRACK **不**因「目标居中」退出（居中即应继续靠近） + * 紧急/返航全局终止、RTL 终态与 reset、force_state 兜底 """ import pytest @@ -19,6 +23,25 @@ def _sm(cfg=None) -> TaskStateMachine: return TaskStateMachine(cfg or TaskStateConfig()) +def _to_search(cfg=None, t0: int = 1000): + """推进到 SEARCH 态：WAIT→ARM_TAKEOFF→GOTO→SEARCH。返回 (sm, now_ms)。""" + sm = _sm(cfg) + sm.update(_inp(target_injected=True), t0) + sm.update(_inp(target_injected=True, takeoff_reached=True), t0 + 1000) + tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0), t0 + 2000) + assert tr.to_state is TaskState.SEARCH + return sm, t0 + 2000 + + +def _to_track(cfg=None, t0: int = 1000): + """推进到 TRACK 态：WAIT→…→SEARCH→TRACK。返回 (sm, now_ms)。""" + sm, t = _to_search(cfg, t0) + tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0, + target_locked=True), t + 1000) + assert tr.to_state is TaskState.TRACK + return sm, t + 1000 + + # --- 初始态与 reset --------------------------------------------------------- def test_initial_state_is_wait(): assert _sm().state is TaskState.WAIT @@ -32,6 +55,13 @@ def test_reset_returns_to_wait(): assert sm.state is TaskState.WAIT +def test_force_state_jumps_without_transition(): + """force_state 供异常兜底直接落状态（不产生 TaskTransition）。""" + sm = _sm() + sm.force_state(TaskState.RTL, 5000) + assert sm.state is TaskState.RTL + + # --- WAIT ------------------------------------------------------------------- def test_wait_without_coords_stays(): tr = _sm().update(_inp(), 1000) @@ -60,12 +90,11 @@ def test_arm_takeoff_timeout_goes_rtl(): cfg = TaskStateConfig(arm_takeoff_timeout_s=60.0) sm = _sm(cfg) sm.update(_inp(target_injected=True), 1000) - # 60s 未到高度 → 超时返航 tr = sm.update(_inp(target_injected=True), 1000 + 61_000) assert tr.occurred and tr.to_state is TaskState.RTL -# --- GOTO ------------------------------------------------------------------- +# --- GOTO：只按「到参考点半径」退出（不看锁定）------------------------------- def test_goto_far_away_stays(): sm = _sm() sm.update(_inp(target_injected=True), 1000) @@ -76,45 +105,31 @@ def test_goto_far_away_stays(): assert sm.state is TaskState.GOTO -def test_goto_within_radius_and_locked_goes_track(): +def test_goto_within_radius_goes_search_even_if_locked(): + """到参考点即转搜索——参考点是搜索起点，锁定与否不影响该转移。""" sm = _sm() sm.update(_inp(target_injected=True), 1000) sm.update(_inp(target_injected=True, takeoff_reached=True), 2000) tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0, target_locked=True), 3000) - assert tr.occurred and tr.to_state is TaskState.TRACK + assert tr.occurred and tr.to_state is TaskState.SEARCH + assert "参考点" in tr.reason -def test_goto_within_radius_but_unlocked_stays(): +def test_goto_within_radius_unlocked_still_goes_search(): sm = _sm() sm.update(_inp(target_injected=True), 1000) sm.update(_inp(target_injected=True, takeoff_reached=True), 2000) tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0, target_locked=False), 3000) - assert not tr.occurred - assert sm.state is TaskState.GOTO - - -def test_goto_requires_vision_when_enabled(): - cfg = TaskStateConfig(require_vision=True) - sm = _sm(cfg) - sm.update(_inp(target_injected=True), 1000) - sm.update(_inp(target_injected=True, takeoff_reached=True), 2000) - # 有锁定但无视觉第二判据 → 不切换 - tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0, - target_locked=True, vision_has_target=False), 3000) - assert not tr.occurred - # 补上视觉 → 切换 - tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0, - target_locked=True, vision_has_target=True), 4000) - assert tr.occurred and tr.to_state is TaskState.TRACK + assert tr.occurred and tr.to_state is TaskState.SEARCH def test_goto_loses_coords_goes_rtl(): sm = _sm() sm.update(_inp(target_injected=True), 1000) sm.update(_inp(target_injected=True, takeoff_reached=True), 2000) - tr = sm.update(_inp(target_injected=False, distance_to_target_m=10.0), 3000) + tr = sm.update(_inp(target_injected=False, distance_to_target_m=100.0), 3000) assert tr.occurred and tr.to_state is TaskState.RTL @@ -123,87 +138,113 @@ def test_goto_timeout_goes_rtl(): sm = _sm(cfg) sm.update(_inp(target_injected=True), 1000) sm.update(_inp(target_injected=True, takeoff_reached=True), 2000) - tr = sm.update(_inp(target_injected=True, distance_to_target_m=100.0), 2000 + 121_000) + tr = sm.update(_inp(target_injected=True, distance_to_target_m=100.0), + 2000 + 121_000) assert tr.occurred and tr.to_state is TaskState.RTL -# --- TRACK ------------------------------------------------------------------ -def test_track_centered_stable_goes_approach(): - sm = _sm(TaskStateConfig(center_hold_s=3.0)) - sm.update(_inp(target_injected=True), 1000) - sm.update(_inp(target_injected=True, takeoff_reached=True), 2000) - sm.update(_inp(target_injected=True, distance_to_target_m=10.0, - target_locked=True), 3000) - assert sm.state is TaskState.TRACK - # 居中但未满时长 → 停留 - tr = sm.update(_inp(target_injected=True, target_locked=True, - target_centered=True, centered_duration_s=1.0), 4000) +# --- SEARCH：搜索发现目标才进 TRACK ----------------------------------------- +def test_search_sees_target_goes_track(): + sm, t = _to_search() + tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0, + target_locked=True), t + 1000) + assert tr.occurred and tr.to_state is TaskState.TRACK + assert "发现目标" in tr.reason + + +def test_search_without_target_stays(): + sm, t = _to_search() + tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0, + target_locked=False), t + 1000) assert not tr.occurred - # 居中稳定满 3s → APPROACH - tr = sm.update(_inp(target_injected=True, target_locked=True, - target_centered=True, centered_duration_s=3.0), 5000) - assert tr.occurred and tr.to_state is TaskState.APPROACH + assert sm.state is TaskState.SEARCH -def test_track_lost_with_coords_goes_back_to_goto(): - sm = _sm() - sm.update(_inp(target_injected=True), 1000) - sm.update(_inp(target_injected=True, takeoff_reached=True), 2000) - sm.update(_inp(target_injected=True, distance_to_target_m=10.0, - target_locked=True), 3000) - tr = sm.update(_inp(target_injected=True, target_locked=False), 4000) - assert tr.occurred and tr.to_state is TaskState.GOTO - - -def test_track_lost_without_coords_goes_rtl(): - sm = _sm() - sm.update(_inp(target_injected=True), 1000) - sm.update(_inp(target_injected=True, takeoff_reached=True), 2000) - sm.update(_inp(target_injected=True, distance_to_target_m=10.0, - target_locked=True), 3000) - tr = sm.update(_inp(target_injected=False, target_locked=False), 4000) +def test_search_timeout_goes_rtl(): + cfg = TaskStateConfig(search_timeout_s=20.0) + sm, t = _to_search(cfg) + tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0), + t + 21_000) assert tr.occurred and tr.to_state is TaskState.RTL + assert "搜索超时" in tr.reason + + +def test_search_loses_coords_goes_rtl(): + sm, t = _to_search() + tr = sm.update(_inp(target_injected=False), t + 1000) + assert tr.occurred and tr.to_state is TaskState.RTL + + +# --- TRACK：缓慢靠近，只受超时/返航/紧急控制 --------------------------------- +def test_track_centered_does_not_exit(): + """目标居中**不再**触发退出：居中正是应当继续缓慢靠近的时刻。""" + sm, t = _to_track() + tr = sm.update(_inp(target_injected=True, target_locked=True, + target_centered=True, centered_duration_s=99.0), t + 1000) + assert not tr.occurred + assert sm.state is TaskState.TRACK + + +def test_track_holds_while_locked(): + sm, t = _to_track() + for dt in (1000, 2000, 3000): + tr = sm.update(_inp(target_injected=True, target_locked=True), t + dt) + assert not tr.occurred + assert sm.state is TaskState.TRACK def test_track_timeout_goes_rtl(): - cfg = TaskStateConfig(track_timeout_s=120.0) - sm = _sm(cfg) - sm.update(_inp(target_injected=True), 1000) - sm.update(_inp(target_injected=True, takeoff_reached=True), 2000) - sm.update(_inp(target_injected=True, distance_to_target_m=10.0, - target_locked=True), 3000) - tr = sm.update(_inp(target_injected=True, target_locked=True), 3000 + 121_000) + cfg = TaskStateConfig(track_timeout_s=20.0) + sm, t = _to_track(cfg) + tr = sm.update(_inp(target_injected=True, target_locked=True), t + 21_000) + assert tr.occurred and tr.to_state is TaskState.RTL + assert "超时" in tr.reason + + +def test_track_lost_with_coords_goes_back_to_search(): + """丢目标回 SEARCH 重新扫视（不回 GOTO：飞机本就在参考点附近）。""" + sm, t = _to_track() + tr = sm.update(_inp(target_injected=True, target_locked=False), t + 1000) + assert tr.occurred and tr.to_state is TaskState.SEARCH + assert "重新搜索" in tr.reason + + +def test_track_lost_without_coords_goes_rtl(): + sm, t = _to_track() + tr = sm.update(_inp(target_injected=False, target_locked=False), t + 1000) assert tr.occurred and tr.to_state is TaskState.RTL -# --- APPROACH --------------------------------------------------------------- +def test_reacquire_after_loss_returns_to_track(): + """丢目标→SEARCH→重新锁定→TRACK 全链路可循环。""" + sm, t = _to_track() + tr = sm.update(_inp(target_injected=True, target_locked=False), t + 1000) + assert tr.to_state is TaskState.SEARCH + tr = sm.update(_inp(target_injected=True, target_locked=True), t + 2000) + assert tr.to_state is TaskState.TRACK + + +# --- APPROACH（保留态：真机可选收尾）---------------------------------------- def test_approach_hover_done_goes_rtl(): - cfg = TaskStateConfig(center_hold_s=0.0, approach_hover_s=10.0) + cfg = TaskStateConfig(approach_hover_s=10.0) sm = _sm(cfg) - sm.update(_inp(target_injected=True), 1000) - sm.update(_inp(target_injected=True, takeoff_reached=True), 2000) - sm.update(_inp(target_injected=True, distance_to_target_m=10.0, - target_locked=True), 3000) - sm.update(_inp(target_injected=True, target_locked=True, target_centered=True, - centered_duration_s=0.0), 4000) - assert sm.state is TaskState.APPROACH - # 悬停未满 10s → 停留 - tr = sm.update(_inp(target_injected=True, target_locked=True), 4000 + 5000) + sm.force_state(TaskState.APPROACH, 1000) + tr = sm.update(_inp(), 1000 + 5000) assert not tr.occurred - # 满 10s → RTL - tr = sm.update(_inp(target_injected=True, target_locked=True), 4000 + 10_000) + tr = sm.update(_inp(), 1000 + 10_000) assert tr.occurred and tr.to_state is TaskState.RTL # --- 全局终止（紧急 / 返航）------------------------------------------------- -@pytest.mark.parametrize("state_kw", [ - {}, # WAIT - {"target_injected": True}, # 会先转 ARM_TAKEOFF -]) -def test_emergency_goes_rtl_from_any_state(state_kw): - sm = _sm() - sm.update(_inp(**state_kw), 1000) - tr = sm.update(_inp(emergency=True), 2000) +def test_emergency_from_search_goes_rtl(): + sm, t = _to_search() + tr = sm.update(_inp(emergency=True), t + 1000) + assert tr.occurred and tr.to_state is TaskState.RTL + + +def test_emergency_from_track_goes_rtl(): + sm, t = _to_track() + tr = sm.update(_inp(target_locked=True, emergency=True), t + 1000) assert tr.occurred and tr.to_state is TaskState.RTL @@ -218,7 +259,6 @@ def test_rtl_is_terminal(): sm = _sm() sm.update(_inp(rtl_requested=True), 1000) assert sm.state is TaskState.RTL - # 终态不再转移（连紧急/坐标也不动） tr = sm.update(_inp(target_injected=True, emergency=True), 2000) assert not tr.occurred assert sm.state is TaskState.RTL @@ -231,3 +271,11 @@ def test_transition_dataclass_defaults(): assert tr.to_state is TaskState.WAIT assert tr.occurred is False assert tr.reason == "" + + +# --- 配置默认值（搜索/靠近超时口径）------------------------------------------ +def test_default_config_has_search_and_pursuit_timeouts(): + cfg = TaskStateConfig() + assert cfg.search_timeout_s == pytest.approx(20.0) + assert cfg.track_timeout_s == pytest.approx(20.0) + assert cfg.sweep_rate_dps > 0.0 diff --git a/utils/tests/visual_track_control/__pycache__/frame_transform_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/visual_track_control/__pycache__/frame_transform_test.cpython-312-pytest-9.1.1.pyc deleted file mode 100644 index 1b9b9a8..0000000 Binary files a/utils/tests/visual_track_control/__pycache__/frame_transform_test.cpython-312-pytest-9.1.1.pyc and /dev/null differ diff --git a/utils/tests/visual_track_control/__pycache__/line_of_sight_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/visual_track_control/__pycache__/line_of_sight_test.cpython-312-pytest-9.1.1.pyc deleted file mode 100644 index 27138b3..0000000 Binary files a/utils/tests/visual_track_control/__pycache__/line_of_sight_test.cpython-312-pytest-9.1.1.pyc and /dev/null differ diff --git a/utils/tests/visual_track_control/__pycache__/mavsdk_gateway_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/visual_track_control/__pycache__/mavsdk_gateway_test.cpython-312-pytest-9.1.1.pyc deleted file mode 100644 index 0351912..0000000 Binary files a/utils/tests/visual_track_control/__pycache__/mavsdk_gateway_test.cpython-312-pytest-9.1.1.pyc and /dev/null differ diff --git a/utils/tests/visual_track_control/__pycache__/pid_controller_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/visual_track_control/__pycache__/pid_controller_test.cpython-312-pytest-9.1.1.pyc deleted file mode 100644 index ab6d52b..0000000 Binary files a/utils/tests/visual_track_control/__pycache__/pid_controller_test.cpython-312-pytest-9.1.1.pyc and /dev/null differ diff --git a/utils/tests/visual_track_control/__pycache__/rate_limiter_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/visual_track_control/__pycache__/rate_limiter_test.cpython-312-pytest-9.1.1.pyc deleted file mode 100644 index c204b9f..0000000 Binary files a/utils/tests/visual_track_control/__pycache__/rate_limiter_test.cpython-312-pytest-9.1.1.pyc and /dev/null differ diff --git a/utils/tests/visual_track_control/__pycache__/staleness_checker_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/visual_track_control/__pycache__/staleness_checker_test.cpython-312-pytest-9.1.1.pyc deleted file mode 100644 index 228501e..0000000 Binary files a/utils/tests/visual_track_control/__pycache__/staleness_checker_test.cpython-312-pytest-9.1.1.pyc and /dev/null differ diff --git a/utils/tests/visual_track_control/__pycache__/tracking_control_law_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/visual_track_control/__pycache__/tracking_control_law_test.cpython-312-pytest-9.1.1.pyc deleted file mode 100644 index 71535d5..0000000 Binary files a/utils/tests/visual_track_control/__pycache__/tracking_control_law_test.cpython-312-pytest-9.1.1.pyc and /dev/null differ diff --git a/utils/tests/visual_track_control/__pycache__/vtc_config_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/visual_track_control/__pycache__/vtc_config_test.cpython-312-pytest-9.1.1.pyc deleted file mode 100644 index 24c532d..0000000 Binary files a/utils/tests/visual_track_control/__pycache__/vtc_config_test.cpython-312-pytest-9.1.1.pyc and /dev/null differ diff --git a/utils/visual_track_control/__pycache__/__init__.cpython-312.pyc b/utils/visual_track_control/__pycache__/__init__.cpython-312.pyc deleted file mode 100644 index f4b882a..0000000 Binary files a/utils/visual_track_control/__pycache__/__init__.cpython-312.pyc and /dev/null differ diff --git a/utils/visual_track_control/__pycache__/command_sender.cpython-312.pyc b/utils/visual_track_control/__pycache__/command_sender.cpython-312.pyc deleted file mode 100644 index bb2691a..0000000 Binary files a/utils/visual_track_control/__pycache__/command_sender.cpython-312.pyc and /dev/null differ diff --git a/utils/visual_track_control/__pycache__/config.cpython-312.pyc b/utils/visual_track_control/__pycache__/config.cpython-312.pyc deleted file mode 100644 index 35c5618..0000000 Binary files a/utils/visual_track_control/__pycache__/config.cpython-312.pyc and /dev/null differ diff --git a/utils/visual_track_control/__pycache__/frame_transform.cpython-312.pyc b/utils/visual_track_control/__pycache__/frame_transform.cpython-312.pyc deleted file mode 100644 index 6352063..0000000 Binary files a/utils/visual_track_control/__pycache__/frame_transform.cpython-312.pyc and /dev/null differ diff --git a/utils/visual_track_control/__pycache__/line_of_sight.cpython-312.pyc b/utils/visual_track_control/__pycache__/line_of_sight.cpython-312.pyc deleted file mode 100644 index 3553ad1..0000000 Binary files a/utils/visual_track_control/__pycache__/line_of_sight.cpython-312.pyc and /dev/null differ diff --git a/utils/visual_track_control/__pycache__/mavsdk_gateway.cpython-312.pyc b/utils/visual_track_control/__pycache__/mavsdk_gateway.cpython-312.pyc deleted file mode 100644 index 5ae297f..0000000 Binary files a/utils/visual_track_control/__pycache__/mavsdk_gateway.cpython-312.pyc and /dev/null differ diff --git a/utils/visual_track_control/__pycache__/pid_controller.cpython-312.pyc b/utils/visual_track_control/__pycache__/pid_controller.cpython-312.pyc deleted file mode 100644 index 7fb683d..0000000 Binary files a/utils/visual_track_control/__pycache__/pid_controller.cpython-312.pyc and /dev/null differ diff --git a/utils/visual_track_control/__pycache__/rate_limiter.cpython-312.pyc b/utils/visual_track_control/__pycache__/rate_limiter.cpython-312.pyc deleted file mode 100644 index 2a401df..0000000 Binary files a/utils/visual_track_control/__pycache__/rate_limiter.cpython-312.pyc and /dev/null differ diff --git a/utils/visual_track_control/__pycache__/staleness_checker.cpython-312.pyc b/utils/visual_track_control/__pycache__/staleness_checker.cpython-312.pyc deleted file mode 100644 index e4535a7..0000000 Binary files a/utils/visual_track_control/__pycache__/staleness_checker.cpython-312.pyc and /dev/null differ diff --git a/utils/visual_track_control/__pycache__/tracking_control_law.cpython-312.pyc b/utils/visual_track_control/__pycache__/tracking_control_law.cpython-312.pyc deleted file mode 100644 index 19ae3d7..0000000 Binary files a/utils/visual_track_control/__pycache__/tracking_control_law.cpython-312.pyc and /dev/null differ diff --git a/utils/visual_track_control/__pycache__/types.cpython-312.pyc b/utils/visual_track_control/__pycache__/types.cpython-312.pyc deleted file mode 100644 index 09cc3b1..0000000 Binary files a/utils/visual_track_control/__pycache__/types.cpython-312.pyc and /dev/null differ diff --git a/utils/visual_track_control/mavsdk_gateway.py b/utils/visual_track_control/mavsdk_gateway.py index 6cf7bea..6b0c460 100644 --- a/utils/visual_track_control/mavsdk_gateway.py +++ b/utils/visual_track_control/mavsdk_gateway.py @@ -30,7 +30,13 @@ class MavsdkCommandGateway: self._started = False async def start(self) -> bool: - """进入 offboard：PX4 要求先流式发送首条设定点再请求启动。""" + """进入 offboard：PX4 要求先流式发送首条设定点再请求启动。 + + 幂等：已启动（或重入）时**不重发** offboard.start()，避免任务状态机重入 + 动作（丢目标后回 SEARCH/参考点）反复重启 offboard 造成模式抖动。 + """ + if self._started: + return True try: await self._offboard.set_velocity_body( self._velocity_body(0.0, 0.0, 0.0, 0.0)) @@ -42,6 +48,23 @@ class MavsdkCommandGateway: logger.info("[MavsdkGateway] offboard 已启动") return True + async def send_yaw_rate(self, yaw_rate_dps: float, + forward_mps: float = 0.0, + down_mps: float = 0.0) -> bool: + """原地偏航扫视：零前向/侧向速度 + 指定偏航角速度（SEARCH 态扫视用）。 + + 与 send_brake_hover 同类，但带角速度，故机头缓慢旋转、位置保持—— + 吊舱相机刚性固连机体，转弯即改变观察方向，以此覆盖参考点周边。 + 异常一律捕获返回 False（与其它指令一致，不向外抛）。 + """ + try: + await self._offboard.set_velocity_body( + self._velocity_body(forward_mps, 0.0, down_mps, yaw_rate_dps)) + except Exception as exc: + logger.warning("[MavsdkGateway] 扫视指令发送失败: %s", exc) + return False + return True + async def send_velocity_heading(self, output: ControlOutput, time_boot_ms: Optional[int] = None) -> bool: try: diff --git a/工作进度.md b/工作进度.md index 440dfe6..04b886a 100644 --- a/工作进度.md +++ b/工作进度.md @@ -1,6 +1,6 @@ # 工作进度 -> 更新日期：2026-10-13 +> 更新日期：**2026-10-21** > 项目：无人机视觉目标追踪与控制系统 > （`reference-c++/` C++ 参考实现 ＋ `utils/` Python 移植与新实现） > @@ -8,6 +8,9 @@ > 架构全貌与必须遵守的铁律见 `.workbuddy/memory/ARCHITECTURE.md`； > 「预定流程」全链路的操作指南与待办清单见 `预定流程-操作指南.md`； > 权威规格见 `docs/` 下各技术路线 md（代码注释用 `§章节号` 反向引用）。 +> +> ⚠ **架构口径（2026-10-19）**：光电吊舱已取代双目 + YOLO，成为**唯一目标源**； +> 双目三包已**归档到 `legacy/`**（见 §1.8），运行链路测试 446 条、归档测试 175 条。 --- @@ -17,22 +20,28 @@ | C++ 源工程 | Python 目标包 | 状态 | |-----------|--------------|------| -| `reference-c++/target_tracker` | `utils/target_tracker` | ✅ 完成（28 条用例） | -| `reference-c++/visual_track_control` | `utils/visual_track_control` | ✅ 完成（81 条 + L1 增量 5 条 = 86） | +| `reference-c++/target_tracker` | ~~`utils/target_tracker`~~ → **`legacy/utils/target_tracker`** | ✅ 完成（28 条用例，**已归档**） | +| `reference-c++/visual_track_control` | `utils/visual_track_control` | ✅ 完成（86 条，运行链路复用） | - 逐构件对照移植：测试断言逐条搬运（`EXPECT_DOUBLE_EQ` → 精确 `==`， `EXPECT_NEAR` → `pytest.approx`）。 -- 两包**互不依赖**，唯一转换点在胶水层 `utils/demo_track_control.py`。 +- `visual_track_control` 与追踪器**互不依赖**，唯一转换点是胶水层 + （胶水 demo 已归档到 `legacy/utils/demo_track_control.py`）。 - MAVLink 部分改为 **MAVSDK 语义映射**（offboard API，非字节透传）， 三分支映射保留 C++ A2 回归语义。 - 行为修复项（C1/C2/C4、A1/A2/A4、S6）已在测试文件头注明对应章节。 -### 1.2 双目链路新实现（非移植，按规格新写） +### 1.2 双目链路实现（非移植，按规格新写）—— 📦 已归档 -| Python 包 | 规格来源 | 状态 | +> 2026-10-20 起三包与配套测试/demo/配置/权重/规格全部移到 `legacy/`（默认不跑测试）。 +> 原因：吊舱已成为唯一目标源，这套链路无生产调用方；保留是为了日后接回"独立旁证/距离源"。 +> 详见 `legacy/README.md`。 + +| Python 包 | 原位置 → 现位置 | 状态 | |----------|---------|------| -| `utils/stereo_ranger`（稀疏目标测距） | `docs/双目测距技术路线.md` | ✅ 完成（111 条） | -| `utils/stereo_yolo`（双目 YOLO 检测前端） | `docs/双目YOLO目标识别需求说明.md` | ✅ 完成（36 条） | +| `stereo_ranger`（稀疏目标测距） | `utils/` → `legacy/utils/` | ✅ 完成（111 条，随包归档） | +| `stereo_yolo`（双目 YOLO 检测前端） | `utils/` → `legacy/utils/` | ✅ 完成（36 条，随包归档） | +| `target_tracker`（多目标追踪） | `utils/` → `legacy/utils/` | ✅ 完成（28 条，随包归档） | - `stereo_ranger` 按规格五步流程实现，**纯标准库**，无第三方依赖； 越界即有效性门、视差域权威，偏差项已在其包 README 记录。 @@ -49,65 +58,123 @@ 支持运行时 L1/L2 切换（切换即 reset 清积分，防输出跳变）。 - **硬件结论（厂商答复 + 实测）**：本吊舱**不允许二次开发**；无激光 → **表4 永不出**， 目标坐标改走地面注入；`0x0F` 自动锁定是常驻模式（`0x0E` 只停当次，`0x08` 才退出识别）； - bit2 锁定位目标丢失后不复位，必须加第二判据（我方视觉）。 + bit2 锁定位目标丢失后不复位 → **第二判据已改为 LOS 运动学一致性** + （`pod_bridge/los_consistency.py`，2026-10-19 取代旧「我方视觉」方案）。 - **S1 台架标定已完成（10-09）**：`beta_sign=+1`、`alpha_sign=-1` 已回填 `pod_bridge.json`，并有标定回归测试守护（`test_deployed_config_has_calibrated_signs`）。 - 配套工具：`virtual_pod`（模拟吊舱，无电机语义）、`verify_px4`（仅飞控验证，增量④）、 `pod_sitl_test.py`（L1 SITL 全流程）。 -### 1.4 预定流程全链路（`utils/task_runner`，10-12 落地） +### 1.4 预定流程全链路（`utils/task_runner`，10-12 落地；10-15 状态链重构；10-19 自定义返航 + LOS 判据） -「地面给坐标 → 天空解锁/接受坐标引导/追踪打击」全链路，**已在 PX4 SITL 端到端跑通（约 1m24s）**： +「地面给坐标 → 天空飞向参考点 → 搜索 → 缓慢靠近 → 自定义返航」全链路， +**已在 PX4 SITL 端到端跑通**，并完成**真吊舱（COM5）+ SITL** 硬件在环验证： + +状态链（10-15 重构后）：`WAIT → ARM_TAKEOFF → GOTO(L2) → SEARCH(原地扫视) → TRACK(L1 缓慢靠近) → RTL` | 模块 | 文件 | 作用 | |------|------|------| -| 任务状态机 | `utils/task_runner/state_machine.py` | 6 状态纯逻辑转移：WAIT→ARM_TAKEOFF→GOTO(L2)→TRACK(L1)→APPROACH→RTL | -| 编排器 | `utils/task_runner/runner.py` | 状态机驱动 + 快照采集 + 状态动作（起飞/切律/返航） | +| 任务状态机 | `utils/task_runner/state_machine.py` | 状态纯逻辑转移；参考点=搜索起点，搜索超时/追踪超时→RTL | +| 编排器 | `utils/task_runner/runner.py` | 状态机驱动 + 快照采集 + 状态动作（起飞/扫视/切律/返航）+ 异常兜底返航 | +| 返航控制 | `utils/task_runner/return_home.py` | 自定义返航：回起飞点 → 恢复初始航向 → 垂直降落（不爬高） | | 日志模块 | `utils/task_runner/log.py` | 事件 JSONL + 遥测 CSV 双层结构化日志 | -| 吊舱控制 | `utils/task_runner/pod_control.py` | 真机吊舱识别/锁定指令（0x07/0x0F/0x0E/0x08） | +| 吊舱控制 | `utils/task_runner/pod_control.py` | 真机吊舱识别/锁定指令（0x07/0x0F/0x0E/0x08，只下发一次） | | 地面注入 | `utils/pod_bridge/ground_link.py` | UDP 接收地面站坐标（超龄判无效，替代表4） | +| 目标有效性 | `utils/pod_bridge/los_consistency.py` | **LOS 运动学一致性**第二判据（10-19 取代 `require_vision`） | | SITL 入口 | `task_runner_sitl.py` | 仿真全流程（VirtualPod + 虚拟目标） | | 真机入口 | `task_runner_real.py` | 真串口吊舱 + UDP 注入 + 吊舱控制 | | 地面站 | `ground_station/` | 坐标注入 + 状态可视化（Flask） | +「缓慢靠近」由既有 `TrackingControlLaw` 的无距离降级通道实现：`distance.no_distance_action += slow_approach` + `no_distance_approach_limit_mps=0.2`（SITL 实测 `vx_body` 稳定 0.2 m/s）， +不需要新写控制律。 + +**自定义返航（10-19 新增）**：RTL 不再交给 PX4 原生 `return_to_launch()`（会先爬到 +`RTL_RETURN_ALT=30m` 再降），改为 `utils/task_runner/return_home.py` 驱动： +`RETURN`（保持转场高度飞回起飞点）→ `YAW`（原地转回起飞时刻航向）→ `LAND`（垂直降落）。 +SITL 实测：全程 4.91–5.06 m，距起飞点 16.1→1.6 m，航向恢复到 4° 内，高度 5.01→-0.03 m。 +兜底（未起飞/无起飞点/转场超时/主循环异常）仍退回 PX4 原生 RTL；`--land` 就地降落。 + +**目标有效性第二判据（10-19 换成 LOS 一致性）**：吊舱取代双目后"我方视觉"不存在， +旧 `require_vision` 已删除（要么恒假、要么自证）。改为 `pod_bridge/los_consistency.py`： +本吊舱无电机、相机刚性固连 → 机头转 Δψ 时真实目标脱靶量必变 ≈−Δψ；以 `Δβ+Δψ` +（3s 窗口 / 20° 激励 / 8° 残差）判"角度是活的还是冻住的"，超限即 `tracked=False` +且 `pod_locked()` False（状态机随之退出 TRACK 重搜）。 +⚠ 局限：只证"角度活着"，不证"是那个目标"；锁错物体但角度正常变化无法识别。 + ### 1.5 测试 -**483 条用例全部通过（实测，2026-10-13）**： +**运行链路 446 条全部通过（实测，2026-10-20；归档包另 175 条）**： | 测试组 | 用例数 | 覆盖内容 | |--------|-------|---------| -| `tests/stereo_ranger` | 111 | 规格 §11.4 构件级 + 全流程 | | `tests/visual_track_control` | 86 | 对应 C++ gtest + L1 角度直入增量 | -| `tests/pod_bridge` | 173 | 协议/网关/桥接/L1 + virtual_pod/ground_link | -| `tests/task_runner` | 49 | 状态机/编排器/日志/吊舱控制 | -| `tests/stereo_yolo` | 36 | 假检测器驱动 | -| `tests/target_tracker` | 28 | 对应 C++ gtest 用例 | -| **合计** | **483** | 较 09-15（256）新增 227 | +| `tests/pod_bridge` | 208 | 协议/网关/桥接/L1 + virtual_pod/ground_link + 节流 + **LOS 一致性** | +| `tests/task_runner` | 122 | 状态机（含 SEARCH）/编排器/日志/吊舱控制/自定义返航 | +| `tests/ground_station` | 30 | 地面站页面与日志解读 | +| **运行链路合计** | **446** | 默认 `cd utils && pytest` 只跑这些 | +| （归档）`legacy/utils/tests/{stereo_ranger,stereo_yolo,target_tracker}` | 175 | 见 `legacy/README.md`，默认不跑 | -- stereo_ranger / stereo_yolo 测试**不需要**真实图像、模型权重或硬件。 +> **架构口径（2026-10-19 / 10-20）**：双目 + YOLO 三包已**归档到 `legacy/`**； +> 运行链路（`task_runner_*` / `pod_bridge`）不依赖它们 —— 吊舱是唯一目标源。 +> 目标有效性第二判据为 **LOS 运动学一致性**（`pod_bridge/los_consistency.py`）， +> `require_vision` 已删除。 + +- 归档的双目测试**不需要**真实图像、模型权重或硬件。 - ⚠ codegraph 报告 `AssociateDetections` / `SelectPrimary` 等“无测试”属调用图误报，勿据此判断。 -### 1.6 仿真 / 演示 +### 1.6 仿真 / 运行入口 | 脚本 | 内容 | 结果 | |------|------|------| -| `utils/demo_track_control.py --speedup 10` | 模拟检测 → 追踪 → 控制律 → 发送，20 Hz 时间线 | ✅ | -| `utils/demo_stereo_ranger.py` | 合成框对，六场景 | ✅ | -| `utils/demo_stereo_yolo.py` | 真模型推理 + 合成素材五场景 | ✅ | -| `task_runner_sitl.py`（PX4 SITL） | 预定流程全链路 6 状态 + 地面站联动 | ✅ 端到端跑通（10-12，约 1m24s） | +| `task_runner_sitl.py`（PX4 SITL） | 全链路 `goto→search→track→rtl` + 自定义返航 | ✅ 端到端跑通（10-19 复跑） | +| `task_runner_real.py`（真吊舱 COM5 + SITL 飞控） | 同上，真串口吊舱在环 | ✅ 无目标安全路径 + 强制锁定 TRACK 路径（10-19） | +| `l1_sitl_test.py`（PX4 SITL） | 只覆盖 L1 控制律闭环（原 `pod_sitl_test.py`） | ✅ SITL 实跑 | +| （归档）`legacy/utils/demo_*.py` | 双目/追踪演示 | 📦 移入 legacy，默认不跑 | -- SITL 集成脚本：`test.py`（视觉闭环早前版本）/ `pod_sitl_test.py`（L1 吊舱版）已就位； - 当前全流程实跑走 `task_runner_sitl.py`。 +- 共享件 `now_ms` / `wait_until_ready` / `TelemetryHub` 等已从"测试脚本"拆到 + `utils/sitl_support.py`，两个入口不再 import 测试文件。 +- 吊舱硬件在环实测（10-19，COM5/CH340）：表2-2 **60.0 Hz**、`self_check=0x00`； + 表3 20Hz 下发不打断上行；表2-1 指令帧校验和与协议样例一致。 + 无目标时 `status1=0x00` 恒定（bit2=0）→ 全链路走"搜索 20s 超时→返航"安全路径。 + ⚠ **仍未验证**：吊舱真实目标识别（bit2 0→1），需视场内放目标。 ### 1.7 文档与索引 -- `docs/` 现有 10 份：双目测距技术路线、双目 YOLO 需求说明、视觉跟踪控制律技术路线、 +- `docs/` 现有 7 份（3 份双目规格已归档到 `legacy/docs/`）：视觉跟踪控制律技术路线、 视觉追踪技术路线、物理追踪思路、吊舱解算验证方案（模拟飞控喂表3）、树莓派 4B 部署注意事项、 - 双目相机模组到位作业指导、**L1 航向伺服实现方案**、光电吊舱控制协议 G 版-PB（docx）。 -- 根目录：`预定流程.md`（需求）+ `预定流程-操作指南.md`（操作指南与待办，链路权威）。 -- `.workbuddy/memory/ARCHITECTURE.md` + `MEMORY.md`：完整架构参考与精简铁律。 -- `.codegraph/` 索引 140 文件 / 2337 节点，可直接 codegraph 查询。 -- 相机标定已起步：根目录 `camera.py`（棋盘格采集脚本）+ `stereo_calib_params.npz`（未入库）。 + **L1 航向伺服实现方案**、光电吊舱控制协议 G 版-PB（docx）。 +- 根目录：`预定流程.md`（需求）+ `预定流程-操作指南.md`（操作指南与待办，链路权威）+ `README.md`（总览）。 +- `legacy/README.md`（归档说明）、`tools/README.md`（临时脚本说明）。 +- `.workbuddy/memory/ARCHITECTURE.md` + `MEMORY.md`：完整架构参考与精简铁律（已对齐新架构）。 +- `.codegraph/` 已于 10-20 重建（旧索引过期，清理后重新索引）。 +- 相机标定脚本已移到 `tools/`；标定结果归档在 `legacy/stereo_calib_params.npz`。 + +### 1.8 仓库结构精简（2026-10-20 完成，2026-10-21 提交） + +**动机**：子项目堆积了大量文件，其中双目/YOLO 一套在"吊舱取代双目"后已无生产调用方， +旧架构脚本与生成物也混在根目录，影响阅读与使用。 + +**做了什么**： + +| 处理 | 内容 | +|---|---| +| 归档 `legacy/` | `stereo_yolo` / `stereo_ranger` / `target_tracker` 三包 + 175 条测试 + 3 个 demo + `stereo_*.json` + 素材 + `models/yolo26n-drone.pt` + 三份双目规格 + `stereo_calib_params.npz` | +| 删除 | `test.py`（旧架构双目闭环，未实跑）、`trajectory_plot.png`、`新建 文本文档.txt`、空目录、全部 `__pycache__` / `.pytest_cache` | +| 移入 `tools/` | `camera.py` / `camera_test.py` / `video_test.py`（台架相机脚本） | +| 拆分改名 | `pod_sitl_test.py` → `utils/sitl_support.py`（入口共享件）+ `l1_sitl_test.py`（L1 测试）；两个入口不再 import 测试文件 | +| 从索引剔除 | 59 个历史误跟踪的 `.pyc`（已被 `.gitignore` 覆盖） | +| 新增 `.gitignore` | `__pycache__/`、`.pytest_cache/`、`logs/`、`.codegraph/`、`.workbuddy/`、`.idea/`、`legacy/models/*.pt` 等 | +| 依赖清单 | `requirements.txt` 按实际 import 重写（去 `ultralytics`/`matplotlib`，补 `flask`）；`requirements-pod.txt` 更新条数 | +| 文档 | 所有现行文档同步新路径/新计数（README、操作指南、ARCHITECTURE、MEMORY、utils/README、L1 方案、树莓派部署）；新增 `legacy/README.md`、`tools/README.md` | +| codegraph | 删除过期索引并重建（143 文件 / 2667 节点 / 6470 边） | + +**结果**： + +- 根目录从 18 个文件减到 **10 个**（3 个入口/测试脚本 + 4 份 md + 2 份 requirements + `.gitignore`） +- `utils/` 从 6 个包 + 3 个 demo 减到 **4 个包 + `sitl_support.py`**（体积 3.39MB → 1.78MB） +- 测试口径：运行链路 **446 passed**；归档 **175 passed**（`cd legacy/utils && PYTHONPATH="../../utils" pytest`） +- 运行链路代码对已归档包的引用：**0 处** --- @@ -121,15 +188,21 @@ ### 2.2 数值参数未标定（阻塞飞行决策） -- 预定流程链路：`switch_radius_m` / `center_threshold_deg` / `center_hold_s` / - `takeoff_altitude_m` 与 `heading.gain`、`guidance.speed_kp/yaw_kp` 均为占位 - （`beta_sign`/`alpha_sign` 已按 S1 标定回填）；**标定回填前不得实飞**。 -- `utils/config/stereo_ranger.json` 几何参数为占位；解禁路径见 - `docs/双目相机模组到位作业指导.md` P0→P6（已起步标定采图，未走完）。 +- **状态机**（`build_sm_config()`）：`arrival_radius_m` / `goto_timeout_s` / `search_timeout_s` / + `track_timeout_s` / `sweep_rate_dps` / `sweep_reverse_s` / `takeoff_altitude_m` 均为占位。 +- **返航**（`build_return_config()`）：`speed_mps` / `arrival_radius_m` / `yaw_align_tolerance_deg` / + `alt_kp` 及各类超时均为占位。 +- **控制律**：`heading.gain`、`guidance.speed_kp/yaw_kp` 为占位 + （`beta_sign`/`alpha_sign` 已按 S1 标定回填；LOS 判据阈值 `los_min_yaw_change_deg` / + `los_max_residual_deg` 亦待台架实测调整）。 +- **标定回填前不得实飞**。 +- （归档）`legacy/utils/config/stereo_ranger.json` 几何参数为占位；若日后接回双目， + 解禁路径见 `legacy/docs/双目相机模组到位作业指导.md` P0→P6（标定未走完）。 ### 2.3 任务级状态机规格对齐 -- 「预定流程」版状态机（6 态）已实现并 SITL 跑通； +- 「预定流程」版状态机已实现并 SITL + 真吊舱在环跑通（`WAIT→ARM_TAKEOFF→GOTO→SEARCH→ + TRACK→RTL`，2026-10-19 口径）； - 但 `docs/物理追踪思路.md` 引用的 S5~S8 版规格（`docs/状态机设计.md` 等）仍缺失， 两套状态集是否对齐/合并需总项目侧确认（见 §3.1）。 @@ -154,50 +227,54 @@ 未构建、未执行 ctest。C++ 测试用例文件齐全（11 个）， 需先 `cmake .. && make && ctest`。若以 Python 侧为验证基准，此点可暂不处理。 -### 3.3 环境不一致（可立即修） +### 3.3 环境口径（已统一，曾不一致） -`utils/README.md` 声明 conda env `pytorch`（`D:\program file\conda\envs\pytorch`）， -但实测该环境 **pytest 缺失**；全部测试通过隔离 venv -（`C:\Users\yimiao\.workbuddy\binaries\python\envs\drone-test`，基于该 conda 解释器 -`--system-site-packages` 仅加装 pytest）完成，**未改动 conda 环境**。 -建议择机 `pip install pytest` 回填，使 README 与实际一致。 +**现行唯一口径**：conda env `pytorch`（`D:\program file\conda\envs\pytorch\python.exe`）， +pytest 9.1.1 可用，全量 621 passed 直接跑。 +> 历史：曾一度认为该环境缺 pytest、改走隔离 venv `drone-test`。该结论**已过时**， +> 隔离 venv 不再需要；文中若见旧 venv 路径一律以 conda `pytorch` 为准。 ### 3.4 真机安全红线 标定回填前**不得实飞**；S3 起必须：不装桨 / 空旷场地 / RC 随时可切手动 / 有人值守。 -### 3.5 帧周期算力不可外推 +### 3.5 帧周期算力不可外推（归档项） `stereo_yolo` 实测帧周期 25~35 ms 为台架开发机数据，与 30 fps 预算同量级， -结论不可外推到机载平台。 +结论不可外推到机载平台（**这也是当前不接双目、距离优先用雷达的原因之一**）。 +该包已归档到 `legacy/`。 --- ## 4. 下一步（建议顺序） -1. **真机增量验证 ②~⑤**——对齐 `预定流程-操作指南.md` §2.4，③ 干跑优先， - 先把 β→yaw_rate 符号/量级钉死。 -2. **占位参数标定回填**——状态机 4 项 + L1/L2 增益。 -3. P1：摄像头接真视频流；`TaskStateConfig` 外置 JSON；地面站与 runner 实时打通。 -4. P2：`.gitignore` 补齐（`logs/`、`__pycache__/`、`.codegraph/` 等）；conda 回填 pytest； - C++ `cmake && ctest`。 -5. 相机模组到位后按作业指导走 P0→P6；确认 §3.1 四份上游规格可获取位置。 +1. **接距离源（激光雷达）**：给 `PodBridge` 加距离注入点 → 填 `distance_valid` → + 打开 `d_exp_m` 距离 PID，并把 TRACK 的"到点"判据从纯超时改为距离判据。 + （低算力、收益最大；双目暂不接。） +2. **真机目标识别验证**：视场内放目标（人/无人机）+ 0x07，确认 bit2 0→1 + 与 TRACK 真实闭环；顺带复核 LOS 判据阈值是否合适。 +3. **占位参数标定回填**：状态机 6 项 + 返航 6 项 + L1/L2 增益 + LOS 两阈值。 +4. P1：摄像头接真视频流；`TaskStateConfig`/`ReturnConfig` 外置 JSON；地面站与 runner 实时打通。 +5. P2：C++ `cmake && ctest`；codegraph 定期 `sync`（本次已重建）。 +6. 相机模组到位后按作业指导走 P0→P6（脚本在 `tools/`，文档在 `legacy/docs/`）； + 确认 §3.1 四份上游规格可获取位置。 --- ## 5. 验证命令（实测所用） ```bash -# Python 全部测试（483 条） +PY="D:/program file/conda/envs/pytorch/python.exe" + +# 运行链路测试（446 条） cd "D:/ProgramData/code_drone-test/utils" -"C:/Users/yimiao/.workbuddy/binaries/python/envs/drone-test/Scripts/python.exe" -m pytest -q +"$PY" -m pytest -q -# 三个演示 -"C:/Users/yimiao/.workbuddy/binaries/python/envs/drone-test/Scripts/python.exe" demo_track_control.py --speedup 10 -"C:/Users/yimiao/.workbuddy/binaries/python/envs/drone-test/Scripts/python.exe" demo_stereo_ranger.py -"C:/Users/yimiao/.workbuddy/binaries/python/envs/drone-test/Scripts/python.exe" demo_stereo_yolo.py +# 归档的可选后端测试（175 条，按需） +cd "D:/ProgramData/code_drone-test/legacy/utils" +PYTHONPATH="../../utils" "$PY" -m pytest -q -# SITL 全流程 / 地面站联动 / 真机：见 预定流程-操作指南.md §五「一键命令汇总」 +# 吊舱台架 / SITL 全流程 / 地面站 / 真机：见 预定流程-操作指南.md §五「一键命令汇总」 ``` --- @@ -206,16 +283,21 @@ cd "D:/ProgramData/code_drone-test/utils" | 工作项 | 状态 | 备注 | |--------|------|------| -| C++ `target_tracker` → Python | ✅ 完成 | 28 条用例 | -| C++ `visual_track_control` → Python | ✅ 完成 | 86 条（含 L1 增量） | -| `stereo_ranger` 双目测距 | ✅ 完成 | 111 条；几何参数待标定 | -| `stereo_yolo` 检测前端 | ✅ 完成 | 36 条；真实素材待联调 | -| 三路 demo 仿真验证 | ✅ 完成 | — | +| C++ `target_tracker` → Python | ✅ 完成 | 28 条用例；**已归档 legacy/** | +| C++ `visual_track_control` → Python | ✅ 完成 | 86 条（含 L1 增量）；运行链路复用 | +| `stereo_ranger` 双目测距 | 📦 归档（10-20） | 111 条随包；几何参数待标定 | +| `stereo_yolo` 检测前端 | 📦 归档（10-20） | 36 条随包；真实素材待联调 | +| 三路 demo 仿真验证 | 📦 归档（10-20） | 移入 `legacy/utils/` | | `pod_bridge` 桥接 + L1 航向伺服 | ✅ 完成（10-09） | S1 符号已标定；增益待标 | -| **预定流程全链路（task_runner）** | ✅ **SITL 跑通（10-12）** | 已提交 `ded17d7` | -| 真机增量验证 ②~⑤ | ❌ 未开始 | P0；实飞前必须 | -| 状态机/L1/L2 占位参数标定 | ❌ 未开始 | 回填前不得实飞 | +| **预定流程全链路（task_runner）** | ✅ **SITL 跑通（10-12）；10-15 重构；10-19 真吊舱在环** | 状态链含 SEARCH + 自定义返航 | +| LOS 运动学一致性判据 | ✅ 完成（10-19） | 取代 `require_vision`；阈值待台架调整 | +| 自定义返航（回点+复位航向+垂直降落） | ✅ SITL 验证（10-19） | 全程 4.91~5.06 m，不爬高 | +| 真机增量验证 ①②③③b | ✅ 已过 | 串口链路/干跑/真吊舱+SITL 全流程 | +| 真机增量验证 ④⑤（仅飞控 / 实飞地面联调） | ❌ 未开始 | P0；实飞前必须 | +| 吊舱**真实目标识别**（bit2 0→1） | ❌ 未验证 | 需视场内放目标 | +| 距离源接入（激光雷达） | ❌ 未开始 | 打开距离 PID 与到点判据 | +| 状态机/返航/L1/L2 占位参数标定 | ❌ 未开始 | 回填前不得实飞 | | S5~S8 版状态机（物理追踪思路） | ⚠ 规格缺失 | 待对齐（§3.1） | -| 相机模组标定 P0~P6 | ⏳ 起步 | `camera.py` 采集中 | +| 相机模组标定 P0~P6 | 📦 归档 | 脚本移到 `tools/`，标定数据在 `legacy/` | | C++ 侧构建与 ctest | ⚠ 未执行 | 无 build 目录 | -| conda 环境回填 pytest | ⚠ 待办 | 测试现走隔离 venv | +| ~~conda 环境回填 pytest~~ | ✅ 已解决 | 运行链路测试统一走 conda `pytorch`（446 条） | diff --git a/预定流程-操作指南.md b/预定流程-操作指南.md index fe94135..0b12448 100644 --- a/预定流程-操作指南.md +++ b/预定流程-操作指南.md @@ -1,29 +1,43 @@ -# 预定流程 · 操作指南与待办事项 +# 预定流程 · 操作指南与待办事项 -> 更新日期：2026-10-12 -> 覆盖：任务级状态机全链路（`utils/task_runner/`）＋ 地面站页面 ＋ 真机/仿真入口 +> 更新日期：**2026-10-19** +> 覆盖：任务级状态机全链路（`utils/task_runner/`）＋ 吊舱桥接与 LOS 判据 ＋ 地面站页面与控制命令通道 ＋ 真机/仿真入口 > -> 本链路对应 `预定流程.md` 的「地面给坐标 → 天空解锁/接受坐标引导/追踪打击」， -> 已在 **PX4 SITL 端到端跑通**。架构与铁律见 `.workbuddy/memory/MEMORY.md` / `ARCHITECTURE.md`。 +> 本链路对应 `预定流程.md` 的「地面给坐标 → 天空解锁/接受坐标引导/搜索/缓慢靠近」， +> 已在 **PX4 SITL 端到端跑通**，并完成**真吊舱（COM5）+ SITL** 硬件在环验证。 +> 架构与铁律见 `.workbuddy/memory/MEMORY.md` / `ARCHITECTURE.md`。 +> +> ⚠ 架构口径：吊舱取代双目 + YOLO 成为**唯一目标源**；`require_vision` 已删除， +> 第二判据改为 **LOS 运动学一致性**（见 §2.6）。 --- ## 一、本次落地了什么 +> **2026-10-15 按实机作业预期重构状态链**：参考点 = 搜索起点（不再是终点）， +> 到点后原地扫视搜索，锁定后按 `slow_approach` **缓慢靠近**，超时返航。 +> +> 新状态链：`WAIT → ARM_TAKEOFF → GOTO(L2 飞向参考点) → SEARCH(原地扫视) → TRACK(L1 缓慢靠近) → RTL` + | 模块 | 文件 | 作用 | |------|------|------| -| 任务状态机 | `utils/task_runner/state_machine.py` | 6 状态纯逻辑转移：WAIT→ARM_TAKEOFF→GOTO(L2)→TRACK(L1)→APPROACH→RTL | -| 编排器 | `utils/task_runner/runner.py` | 状态机驱动 + 采集快照 + 状态动作（起飞/切律/返航） | -| 日志模块 | `utils/task_runner/log.py` | 事件 JSONL + 遥测 CSV 双层结构化日志 | -| 吊舱控制 | `utils/task_runner/pod_control.py` | 真机吊舱识别/锁定指令（0x07/0x0F/0x0E/0x08） | +| 任务状态机 | `utils/task_runner/state_machine.py` | 状态纯逻辑转移：到点→SEARCH 扫视；搜索超时/追踪超时→RTL；TRACK 丢目标→回 SEARCH | +| 编排器 | `utils/task_runner/runner.py` | 状态机驱动 + 采集快照 + 状态动作（起飞/扫视/切律/返航）+ **主循环异常兜底返航** | +| 日志模块 | `utils/task_runner/log.py` | 事件 JSONL + 遥测 CSV（机器日志）＋ 简洁任务简报 `task_brief.log` | +| 吊舱控制 | `utils/task_runner/pod_control.py` | 真机吊舱识别/锁定指令（0x07/0x0F/0x0E/0x08）；识别开 **只下发一次**（防重入自激） | | 地面注入 | `utils/pod_bridge/ground_link.py` | UDP 接收地面站坐标（超龄判无效） | | SITL 入口 | `task_runner_sitl.py` | 仿真全流程（VirtualPod + 虚拟目标） | | 真机入口 | `task_runner_real.py` | 真串口吊舱 + UDP 注入 + 吊舱控制 | -| 地面站 | `ground_station/app.py` + `templates/index.html` | 坐标注入 + 状态可视化（Flask） | +| 地面站 | `ground_station/app.py` + `templates/index.html` | 坐标注入 + 任务/吊舱控制 + 状态可视化（Flask） | +| 命令通道 | `ground_link.py`（队列）+ `runner.py`（分派）+ `app.py`（`/api/command`） | 页面 `{"cmd":…}` → UDP 14555 → runner 逐拍分派 | 控制律本体复用既有 `visual_track_control.TrackingControlLaw`（L1 角度直入）与 `pod_bridge.GuidanceLaw`（L2 地理引导），**运行时通过 `bridge.set_heading_servo_enabled()` 切换**。 +**「缓慢靠近」不需要新写控制律**：既有 `TrackingControlLaw` 的无距离降级通道已实现—— +`distance.no_distance_action = "slow_approach"` + `no_distance_approach_limit_mps = 0.2`。 +SITL 实测 TRACK 段 `vx_body` 稳定 **0.2 m/s**，且经加速度限幅平滑起步。 + --- ## 二、操作指南 @@ -32,11 +46,12 @@ ```bash cd "D:/ProgramData/code_drone-test/utils" -"C:/Users/yimiao/.workbuddy/binaries/python/envs/drone-test/Scripts/python.exe" -m pytest -q -# 预期：483 passed +"D:/program file/conda/envs/pytorch/python.exe" -m pytest -q +# 预期：446 passed（2026-10-20，运行链路；归档 175 条另计） ``` -> ⚠ conda `pytorch` 环境缺 pytest（见 §三 待办 P2-2），测试用隔离 venv `drone-test`。 +> 环境口径：项目默认 Python 环境为 conda env `pytorch`（含 pytest/flask/mavsdk/pyserial）， +> 运行与测试统一使用；「conda 缺 pytest，测试走隔离 venv」的旧口径已废止。 ### 2.2 SITL 仿真全流程（独立模式，内置虚拟目标自动注入坐标） @@ -44,41 +59,96 @@ cd "D:/ProgramData/code_drone-test/utils" ```bash # 1. WSL 内启动 PX4 SITL -cd ~/PX4-Autopilot && make px4_sitl jmavsim +cd ~/PX4-Autopilot && make px4_sitl gz_x500 # 2. Windows 侧跑全流程（内置虚拟目标 home 北30m/东15m） cd "D:/ProgramData/code_drone-test" -"D:/program file/conda/envs/pytorch/python.exe" task_runner_sitl.py --conn udp://:14540 +"D:/program file/conda/envs/pytorch/python.exe" task_runner_sitl.py --conn udpin://0.0.0.0:14540 ``` -预期：`wait → arm_takeoff → goto → track → approach → rtl`，全程约 1m24s。 +预期：`wait → arm_takeoff → goto → search → track → rtl`。 + +> **返航已改为自定义流程（2026-10-19）**：不再调 PX4 原生 `return_to_launch()`。 +> 原生 RTL 会先爬到 `RTL_RETURN_ALT`（本 SITL 实测 **30 m**）、再降到 +> `RTL_DESCEND_ALT`（10 m）后落地，即"飞很高再降落"。自定义流程改为： +> **保持转场高度（取当前高度与起飞高度的较大者，绝不额外爬高）飞回起飞点 → +> 原地把机头转回起飞时刻航向 → 垂直降落**。SITL 实测全程高度 4.91–5.06 m。 +> +> 参数在 `task_runner_{sitl,real}.py` 的 `build_return_config()`（`ReturnConfig`）； +> 兜底：未起飞/无起飞点/转场超时/主循环异常 → 退回 PX4 原生 RTL； +> `--land` → 就地降落（不返航）。 ### 2.3 SITL 仿真（地面站联动模式，坐标由地面站 UDP 注入） ```bash # 终端 A：无人机（--ground-station 表示坐标走 UDP 接收，不再内置自动注入） -"D:/program file/conda/envs/pytorch/python.exe" task_runner_sitl.py --conn udp://:14540 --ground-station +"D:/program file/conda/envs/pytorch/python.exe" task_runner_sitl.py --conn udpin://0.0.0.0:14540 --ground-station -# 终端 B：地面站页面 -"D:/program file/conda/envs/pytorch/python.exe" ground_station/app.py --udp-port 14555 --log-dir logs -# 浏览器打开 http://127.0.0.1:5000，输入坐标点「注入坐标」 +# 终端 B：地面站页面（--rtsp-url 可选：接入吊舱实时视频，断流自动重连） +"D:/program file/conda/envs/pytorch/python.exe" ground_station/app.py --udp-port 14555 --log-dir logs --rtsp-url rtsp://192.168.144.119:8554/video +# 浏览器打开 http://127.0.0.1:5000，输入坐标点「注入坐标」（或勾选「持续注入」自动重发） ``` ⚠ 地面站需**持续**发坐标（每 0.5~1s 一次），否则 `ground_inject.timeout_ms`（默认 5s） -超龄后 GOTO 判「坐标失效」返航。页面上手动点一次只发一次，联调时建议用脚本循环注入。 +超龄后 GOTO 判「坐标失效」返航。页面勾选「持续注入」即可（**服务端后台线程**每 0.5s 重发， +切窗口/最小化不影响；实测浏览器会节流后台标签页的 JS 定时器 0.5s→约 1 次/分钟，故不能放页面侧）； +取消勾选或关闭地面站程序才停止。 -### 2.4 真机联调（增量流程，标定前不得实飞） +### 2.4 地面站页面控制功能（2026-10-14 新增） + +页面除坐标注入外，还支持以下控制（均走**同一 UDP 端口** 14555，消息 `{"cmd": ""}`， +由 `GroundLink` 入队、`TaskRunner` 主循环逐拍分派；未知命令记警告忽略）： + +| 区块 | 控件 | 命令 | 语义 | +|------|------|------|------| +| 链路目标 | 目标徽标 + 下拉选择 + 「切换目标」 | — | 坐标/命令的 UDP 目标地址（默认「本地回环」127.0.0.1:14555 跑仿真；可选本机网卡或自定义 IP，运行时切换，2026-10-14 新增） | +| 任务日志 | 滚动事件列表 | — | 阶段切换 / 地面站命令 / 飞控状态文本（STATUSTEXT，警告按级别着色）实时显示（读 `/api/state`，2026-10-14 新增） | +| 坐标注入 | 坐标形式（经纬度 / 局部北东 m；局部可选原点） | — | 局部按选定原点换算为经纬度后注入——「起飞点(home)」= 本次任务运行段起始位置（≈起飞点），「当前坐标」= 注入时刻无人机位置；页面显示换算结果与所用原点（2026-10-14 新增） | +| 坐标注入 | 「持续注入」勾选 | — | 服务端后台线程每 0.5s 重发当前坐标（替代循环脚本，与页面可见性无关） | +| 吊舱视频 | 「吊舱视频」面板 | — | 服务端 `--rtsp-url` 拉 RTSP → MJPEG 推流（浏览器原生显示）；断流自动重连 | +| 任务控制 | 请求返航 RTL | `rtl` | `runner.request_rtl()`：人工返航，任何状态立即转 RTL | +| 任务控制 | 紧急终止 | `emergency` | `runner.trigger_emergency()`：最高优先级立即返航 | +| 吊舱控制 | 开识别 | `pod_detect_on` | 0x07(目标类型)+0x0F（对齐 pod_console 按键 d） | +| 吊舱控制 | 自动锁定 | `pod_auto_lock` | 0x0F+0x10（对齐按键 a，识别已开时可用） | +| 吊舱控制 | 停跟踪 | `pod_stop_track` | 0x0E：仅解当前锁定，0x0F 常驻自动锁定仍生效（对齐按键 u） | +| 吊舱控制 | 关识别 | `pod_detect_off` | 0x0E+0x08：彻底退出识别（对齐按键 s） | +| 吊舱控制 | 归中 | `pod_center` | 0x2B（对齐按键 c） | + +说明：RTL/紧急在 SITL 可直接验证。**注意 `rtl` 命令触发的是自定义返航** +（回起飞点→恢复初始航向→垂直降落，不爬高；见 §2.2 说明），不是 PX4 原生 RTL。 +吊舱命令在 SITL（VirtualPod 无控制口）会被忽略并记警告， +真机经 runner → 吊舱串口下发（2026-10-19 台架实测：0x2B/0x07/0x0F/0x10/0x0E/0x08 全达真吊舱、校验和正确）。 + +### 2.5 真机联调（增量流程，标定前不得实飞） 按顺序逐级验证，每级过了再进下一级： | 级 | 动作 | 命令 | 通过判据 | |----|------|------|---------| -| ① 纯软单测 | 全量 pytest | `pytest -q` | 483 passed | +| ① 纯软单测 | 全量 pytest | `pytest -q` | 446 passed（2026-10-20） | | ② 串口回环 | 吊舱自环 | 台架工具 | 收发一致 | -| ③ 仅吊舱 | 干跑（不发 offboard） | `python utils/pod_bridge/run_bridge.py --config utils/config/pod_bridge.json --port /dev/ttyAMA0 --dry-run` | β→yaw_rate 符号/量级正确 | +| ③ 仅吊舱 | 干跑（不发 offboard） | `python utils/pod_bridge/run_bridge.py --config utils/config/pod_bridge.json --port COM5 --dry-run` | β→yaw_rate 符号/量级正确 | +| ③b 吊舱控制联测 | SITL 飞控 + 真吊舱直连本机（TTL 转 USB） | `python task_runner_real.py --port COM5 --conn udpin://0.0.0.0:14540 --log-dir logs/x` ＋ 页面吊舱按钮 | 命令记录无失败、吊舱按指令响应 | | ④ 仅飞控 | 飞控遥测/offboard | — | 遥测新鲜、offboard 可启停 | | ⑤ 地面联调 | 真机全流程 | `python task_runner_real.py --target-type 1` | 完整走完 | +**2026-10-19 真吊舱在环实测结论**（COM5 / CH340，SITL 飞控）： + +| 项 | 结果 | +|----|------| +| 表2-2 上行 | **60.0 Hz**，6s 359 帧；`self_check=0x00` 无告警 | +| 表3 下行 | 20Hz 持续下发时上行仍稳 60.0 Hz，**下行不打断上行** | +| 表2-1 指令 | 0x2B/0x07/0x0F/0x10/0x0E/0x08 均下发成功，帧校验和与协议样例一致（0x2B→`a6`） | +| 状态位 | 视场内无目标时 `status1=0x00` 恒定（**bit2 恒 0 = 搜索态**，符合预期） | +| 脱靶量 | 搜索态恒 `(0.00, 0.00)` —— 与文档「丢失表现为 0」一致 | +| 自身 IMU | `att_roll/att_pitch` 恒 0；`frame_*` 恒 0（本版无电机，符合厂商确认） | +| 全流程（无目标） | `arm_takeoff → goto → search(20s 超时) → rtl → 落地` ✅ | +| 全流程（强制锁定混合） | `goto → search → track(20s) → rtl`；TRACK 段 `vx` 0.05→**0.2 m/s**，距离 29.5→26.2 m ✅ | +| 扫视实测 | SEARCH 20s 内机头 yaw 跨度 **65°**，水平距离 26.6–29.9 m **无漂移**，vz 恒 0 ✅ | + +⚠ **仍未验证**：吊舱真实目标识别（bit2 由 0→1）。需视场内放置目标（人或无人机） +并下发 0x07，才能验证「搜索发现目标→锁定→缓慢靠近」的真实闭环。 + 真机全流程前置： 1. 填 `utils/config/pod_bridge.json` 的 `pod_serial.port`（如 `/dev/ttyAMA0`）、`px4.connection_string`； 2. 地面站往 `ground_inject.port`（默认 14555）持续发 `{"lat":..,"lon":..,"alt":..}`。 @@ -90,15 +160,19 @@ python task_runner_real.py --port /dev/ttyAMA0 # 覆盖吊舱串口 python task_runner_real.py --conn serial:///dev/ttyS1:57600 # 覆盖飞控连接串 ``` -### 2.5 硬件铁律（真机必读，来自厂商确认 + 实测） +### 2.6 硬件铁律（真机必读，来自厂商确认 + 实测） - 🔴 **本吊舱无激光 → 表4 目标经纬度永不出**，目标坐标必须走地面注入（`ground_link`）， `guidance` 吃注入坐标而非表4。 - **0x0F「发现目标自动锁定」是常驻模式**，`0x0E` 只停当前这次跟踪、不退出自动锁定； 只有 `0x08 目标识别关` 才真正退出识别。 - **bit2 锁定位在目标丢失后不复位**，且脱靶量无无效标志位（丢失表现为 0）——单靠 bit2 - 会朝过期坐标继续飞，必须加第二判据（我方视觉，`require_vision`）。 -- **自动解锁（`--auto-unlock`）在本设备禁用**（表4 恒不发导致误解锁），解锁一律用按键 `u`。 + 会朝过期坐标继续飞。**第二判据已改为 LOS 运动学一致性**（`pod_bridge/los_consistency.py`， + 2026-10-19 取代旧的 `require_vision`）：本吊舱无电机、相机刚性固连，机头转 Δψ 时真实 + 目标的脱靶量必变 ≈−Δψ；机头已明显转动而脱靶量不动 → 判丢（触发退出 TRACK 重搜）。 + ⚠ 它只能证明「角度是活的」，不能证明「是那个目标」。 +- **自动解锁（`--auto-unlock`）在本设备禁用**（表4 恒不发导致误解锁），解锁一律用 `0x0E`： + 台架走 `pod_console` 按键 `u`，地面站页面走「停跟踪」按钮（等效通道）。 --- @@ -106,30 +180,39 @@ python task_runner_real.py --conn serial:///dev/ttyS1:57600 # 覆盖飞控连 ### P0（实飞前必须） -- [ ] **真机增量验证**：按 §2.4 的 ②~⑤ 逐级跑，尤其 ③ 干跑确认 β→yaw_rate 符号/量级。 -- [ ] **状态机参数标定**：`task_runner_real.py` 里 `build_sm_config()` 的 - `switch_radius_m`（GOTO→TRACK 切换半径）、`center_threshold_deg`（居中阈值）、 - `center_hold_s`、`takeoff_altitude_m` 全是占位值，须台架标定后回填。 +- [ ] **真机增量验证**：按 §2.5 的 ④⑤ 继续（①②③③b 已于 2026-10-19 通过）。 +- [ ] **吊舱真实目标识别验证**：视场内放目标（人/无人机）+ 0x07，确认 bit2 0→1 与 + TRACK 真实闭环；这是目前唯一没走通的一环（其余均为强制锁定模拟）。 +- [ ] **状态机参数标定**：`task_runner_real.py` 的 `build_sm_config()` 中 + `arrival_radius_m`（到参考点转搜索的半径）、`search_timeout_s`、`track_timeout_s`、 + `sweep_rate_dps` / `sweep_reverse_s`（扫视）、`takeoff_altitude_m` 全是占位值。 +- [ ] **返航参数标定**：`build_return_config()` 的 `speed_mps`、`arrival_radius_m`、 + `yaw_align_tolerance_deg`、`alt_kp`、`timeout_s` 均为占位值。 - [ ] **L1/L2 增益标定**：`pod_bridge.json` 里 `heading.gain`、`guidance.speed_kp/yaw_kp` 等仍为占位，`beta_sign`/`alpha_sign` 已按 S1 标定（+1/-1），其余待标定。 +- [ ] **LOS 判据阈值标定**：`los_window_ms=3000`、`los_min_yaw_change_deg=20`、 + `los_max_residual_deg=8` 需按真机噪声与目标机动特性复核（过紧误判丢目标、过松漏判幽灵）。 ### P1（落地后尽快） -- [ ] **摄像头可视化接真视频流**：当前地面站是「脱靶量模拟视野」，真机需接吊舱视频流 - （吊舱视频输出 → 采集 → 页面叠加追踪框）。 -- [ ] **任务级状态机配置 JSON 化**：`TaskStateConfig` 目前是 dataclass 默认值 + 脚本内构造， - 应外置到配置（铁律「配置全外置」）。 +- [x] **摄像头可视化接真视频流**（2026-10-14 完成）：服务端 RTSP→MJPEG 接入页面「吊舱视频」面板 + （`--rtsp-url`；断流自动重连，拔插网线自愈已实测）；追踪框叠加未做（吊舱 OSD 已烧录脱靶量）。 +- [ ] **接入距离源（激光雷达，优先）**：给 `PodBridge` 加距离注入点 → 填 `distance_valid` → + 打开 `d_exp_m` 距离 PID，并把 TRACK 的「到点」判据从纯超时改为距离判据。 + 代价低（串口读数），直接改善"贴近"；**双目暂不接**（Pi 4 上 YOLO 开销大）。 +- [ ] **任务级配置 JSON 化**：`TaskStateConfig` 与 `ReturnConfig` 目前是 dataclass 默认值 + + 脚本内构造，应外置到配置（铁律「配置全外置」）。 - [ ] **地面站与 runner 状态实时打通**：当前地面站读日志文件（秒级延迟），可改为 runner 直接 UDP 上报遥测到地面站（低延迟）。 ### P2（工程收尾） -- [ ] **仓库补 `.gitignore`**：`logs/`、`__pycache__/`、`.codegraph/`、`models/*.pt`、 - `stereo_calib_params.npz` 等长期未跟踪项，提交时显式列路径、勿 `git add .`。 -- [ ] **conda `pytorch` 回填 pytest**：README 声明有 pytest 但实测缺失，测试目前走 - 隔离 venv `drone-test`；回填后使 README 与实际一致。 +- [x] **`.gitignore` 已补齐**（10-20）：`logs/`、`__pycache__/`、`.pytest_cache/`、`.codegraph/`、`.workbuddy/`、`legacy/models/*.pt` 等。 +- [x] **conda `pytorch` 回填 pytest**（pytest 9.1.1 可用；2026-10-20 运行链路 446 通过， + 运行与测试统一走 conda 环境）。 - [ ] **C++ 侧构建验证**：`reference-c++/target_tracker` 与 `visual_track_control` 无 `build/`，若以 C++ 为验收基准需 `cmake .. && make && ctest` 跑 11 个测试。 +- [ ] **codegraph 重新索引**：当前索引早于 10-15/10-19 重构（行号偏移），需 `codegraph sync`。 --- @@ -137,27 +220,61 @@ python task_runner_real.py --conn serial:///dev/ttyS1:57600 # 覆盖飞控连 | 段 | 关键字段 | 说明 | |----|---------|------| -| `pod_serial` | `port` | 吊舱串口设备号（真机填 `/dev/ttyAMA0`） | -| `px4` | `connection_string` | 飞控连接串（SITL `udp://:14540`，真机串口） | -| `link` | `require_laser_valid` | 无激光吊舱必须 `false` | +| `pod_serial` | `port` | 吊舱串口设备号（真机 `/dev/ttyAMA0`，USB 转串口常见 `COM5`/`/dev/ttyUSB0`） | +| `px4` | `connection_string` | 飞控连接串（SITL `udpin://0.0.0.0:14540`，真机串口） | +| `link` | `require_laser_valid` | 无激光吊舱必须 `false`（本机恒 `false`） | | `heading_servo` | `enabled` / `beta_sign` / `alpha_sign` | L1 开关 + 符号（±1，已标定 +1/-1） | -| `ground_inject` | `enabled` / `port` / `timeout_ms` | 地面坐标注入开关 / UDP 端口 / 超龄门限 | +| `heading_servo` | `los_consistency_enabled` / `los_window_ms` / `los_min_yaw_change_deg` / `los_max_residual_deg` | **目标有效性第二判据**（LOS 运动学一致性，2026-10-19 取代 `require_vision`） | +| `ground_inject` | `enabled` / `port` / `timeout_ms` | 地面坐标注入开关 / UDP 端口 / 超龄门限；**真机入口会用它统一 `guidance.target_stale_ms`** | + +> 任务级与返航参数不在 JSON 内，见入口脚本的 `build_sm_config()` / `build_return_config()` +> （JSON 化仍在 P1 待办）。 + +### 4.1 链路负荷与遥测速率（2026-10-15） + +遥测速率**不是越高越好**：PX4 的消息速率由最后一次 `SET_MESSAGE_INTERVAL` 决定， +多个订阅方用不同值会互相覆盖；速率越高 MAVSDK 回调队列压力越大。 + +| 流 | 速率 | 消费者/依据 | +|----|------|------------| +| `attitude_euler` | 20 Hz | 控制律（`attitude_stale_ms=300`） | +| `position` | 10 Hz | 起飞到位 / 到参考点距离 | +| `position_velocity_ned` | 10 Hz | 仅虚拟吊舱几何（SITL） | +| `gps_info` / `velocity_ned` / `in_air` | 2 Hz | 状态量，变化极慢 | + +速率集中定义在 `utils/sitl_support.py` 的 `RATE_*` 常量（唯一来源，各订阅方共用）。 + +**任务日志落盘策略**：事件（阶段切换/命令/告警）**即时 flush**（页面实时性不受影响）； +遥测 CSV 按 `TaskLogConfig.telemetry_flush_ms`（默认 1000ms）节流落盘 —— 主循环 +20Hz 若每行同步 flush，等于每秒 20 次磁盘 IO，在树莓派 SD 卡上会阻塞事件循环。 +`close()` 做最后 flush，正常退出不丢数据。 --- ## 五、一键命令汇总 ```bash -# 测试 -cd utils && "C:/Users/yimiao/.workbuddy/binaries/python/envs/drone-test/Scripts/python.exe" -m pytest -q +PY="D:/program file/conda/envs/pytorch/python.exe" + +# 运行链路测试（446 条） +cd "D:/ProgramData/code_drone-test/utils" && "$PY" -m pytest -q + +# 吊舱串口自检 / S2 干跑 +"$PY" utils/pod_bridge/listen_pod.py COM5 --seconds 5 +"$PY" utils/pod_bridge/run_bridge.py --port COM5 --dry-run --duration 10 # SITL 独立全流程 -"D:/program file/conda/envs/pytorch/python.exe" task_runner_sitl.py --conn udp://:14540 +"$PY" task_runner_sitl.py --conn udpin://0.0.0.0:14540 # SITL 地面站联动 -"D:/program file/conda/envs/pytorch/python.exe" task_runner_sitl.py --conn udp://:14540 --ground-station -"D:/program file/conda/envs/pytorch/python.exe" ground_station/app.py --udp-port 14555 --log-dir logs +"$PY" task_runner_sitl.py --conn udpin://0.0.0.0:14540 --ground-station +"$PY" ground_station/app.py --udp-port 14555 --log-dir logs --rtsp-url rtsp://192.168.144.119:8554/video -# 真机全流程（填好配置后） -"D:/program file/conda/envs/pytorch/python.exe" task_runner_real.py --target-type 1 +# 真机全流程（真吊舱串口 + UDP 注入 + PX4） +"$PY" task_runner_real.py --port COM5 --conn udpin://0.0.0.0:14540 --target-type 1 +"$PY" task_runner_real.py --port /dev/ttyAMA0 --target-type 1 # 树莓派 +"$PY" task_runner_real.py --port COM5 --land # 收尾就地降落 ``` + +> 真机入口的 `rtl`/超时/异常收尾都走**自定义返航**（回起飞点→恢复初始航向→垂直降落）； +> 仅"未起飞/无起飞点/转场超时/主循环异常"退回 PX4 原生 RTL。