Files
code-test-drone/预定流程-操作指南.md
keita df91b1110e 精简仓库结构并增强任务链路:归档双目可选后端、新增LOS判据与自定义返航
归档 legacy/:stereo_yolo/stereo_ranger/target_tracker 三包 + 175 条测试 + demo/配置/权重/三份规格;运行链路不再依赖,默认 pytest 446 条,归档测试按需运行。

目标有效性第二判据改为 LOS 运动学一致性(pod_bridge/los_consistency.py),删除 require_vision;pod_locked() 叠加该判据,状态机随之后退重搜。

自定义返航(task_runner/return_home.py):保持转场高度回起飞点 → 恢复初始航向 → 垂直降落,不再爬 RTL_RETURN_ALT;未起飞/无起飞点/超时/异常仍退回 PX4 原生 RTL。

任务链安全兜底:主循环异常→紧急返航、stop 幂等、landing_stop 保留遥测、in_air 流复用+超时、解锁/起飞与吊舱识别仅执行一次、rtl_timeout 上限。

入口共享件拆到 utils/sitl_support.py;pod_sitl_test.py→l1_sitl_test.py;删除旧架构 test.py;相机脚本移入 tools/。

新增 .gitignore 并剔除历史误跟踪的 59 个 .pyc;requirements 按实际依赖重写;文档全面对齐,新增 legacy/README.md 与 tools/README.md。
2026-10-21 14:48:41 +08:00

281 lines
18 KiB
Markdown
Raw Permalink Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
# 预定流程 · 操作指南与待办事项
> 更新日期:**2026-10-19**
> 覆盖:任务级状态机全链路(`utils/task_runner/`)+ 吊舱桥接与 LOS 判据 + 地面站页面与控制命令通道 + 真机/仿真入口
>
> 本链路对应 `预定流程.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` | 状态纯逻辑转移:到点→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_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**,且经加速度限幅平滑起步。
---
## 二、操作指南
### 2.1 纯软测试(无硬件,最快验证)
```bash
cd "D:/ProgramData/code_drone-test/utils"
"D:/program file/conda/envs/pytorch/python.exe" -m pytest -q
# 预期:446 passed(2026-10-20,运行链路;归档 175 条另计)
```
> 环境口径:项目默认 Python 环境为 conda env `pytorch`(含 pytest/flask/mavsdk/pyserial),
> 运行与测试统一使用;「conda 缺 pytest,测试走隔离 venv」的旧口径已废止。
### 2.2 SITL 仿真全流程(独立模式,内置虚拟目标自动注入坐标)
前置:WSL 里启动 PX4 SITL(监听 14540)。
```bash
# 1. WSL 内启动 PX4 SITL
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 udpin://0.0.0.0:14540
```
预期:`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 udpin://0.0.0.0:14540 --ground-station
# 终端 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 判「坐标失效」返航。页面勾选「持续注入」即可(**服务端后台线程**每 0.5s 重发,
切窗口/最小化不影响;实测浏览器会节流后台标签页的 JS 定时器 0.5s→约 1 次/分钟,故不能放页面侧);
取消勾选或关闭地面站程序才停止。
### 2.4 地面站页面控制功能(2026-10-14 新增)
页面除坐标注入外,还支持以下控制(均走**同一 UDP 端口** 14555,消息 `{"cmd": "<name>"}`,
由 `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` | 446 passed(2026-10-20) |
| ② 串口回环 | 吊舱自环 | 台架工具 | 收发一致 |
| ③ 仅吊舱 | 干跑(不发 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":..}`。
真机关键命令参数:
```bash
python task_runner_real.py --target-type 1 # 1=无人机,0=人/车
python task_runner_real.py --port /dev/ttyAMA0 # 覆盖吊舱串口
python task_runner_real.py --conn serial:///dev/ttyS1:57600 # 覆盖飞控连接串
```
### 2.6 硬件铁律(真机必读,来自厂商确认 + 实测)
- 🔴 **本吊舱无激光 → 表4 目标经纬度永不出**,目标坐标必须走地面注入(`ground_link`),
`guidance` 吃注入坐标而非表4。
- **0x0F「发现目标自动锁定」是常驻模式**,`0x0E` 只停当前这次跟踪、不退出自动锁定;
只有 `0x08 目标识别关` 才真正退出识别。
- **bit2 锁定位在目标丢失后不复位**,且脱靶量无无效标志位(丢失表现为 0)——单靠 bit2
会朝过期坐标继续飞。**第二判据已改为 LOS 运动学一致性**(`pod_bridge/los_consistency.py`,
2026-10-19 取代旧的 `require_vision`):本吊舱无电机、相机刚性固连,机头转 Δψ 时真实
目标的脱靶量必变 ≈−Δψ;机头已明显转动而脱靶量不动 → 判丢(触发退出 TRACK 重搜)。
⚠ 它只能证明「角度是活的」,不能证明「是那个目标」。
- **自动解锁(`--auto-unlock`)在本设备禁用**(表4 恒不发导致误解锁),解锁一律用 `0x0E`:
台架走 `pod_console` 按键 `u`,地面站页面走「停跟踪」按钮(等效通道)。
---
## 三、待办事项(按优先级)
### P0(实飞前必须)
- [ ] **真机增量验证**:按 §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(落地后尽快)
- [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(工程收尾)
- [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`。
---
## 四、关键配置速查(`utils/config/pod_bridge.json`)
| 段 | 关键字段 | 说明 |
|----|---------|------|
| `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) |
| `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
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 独立全流程
"$PY" task_runner_sitl.py --conn udpin://0.0.0.0:14540
# SITL 地面站联动
"$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
# 真机全流程(真吊舱串口 + 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。