diff --git a/docs/吊舱解算验证方案-模拟飞控喂表3.md b/docs/吊舱解算验证方案-模拟飞控喂表3.md new file mode 100644 index 0000000..17d2ec5 --- /dev/null +++ b/docs/吊舱解算验证方案-模拟飞控喂表3.md @@ -0,0 +1,262 @@ +# 用「模拟飞控」喂表3 验证吊舱目标解算能力的实验方案 + +> 状态：**代码已实现**（`sim_fc.py` 已落地，配套 21 个纯软单测）， +> 参数已敲定见 §7，**待上机实跑**得到 Q1/Q2/Q3 的实测答案。 +> 日期：2026-09-29　关联：`docs/树莓派4B部署注意事项.md` §10 / §10.1 + +--- + +## 0. 一句话目标 + +在**没有飞控、没有激光、无人机不动**的条件下，由树莓派单独向吊舱持续喂一套 +「假飞控数据」（表3），逼吊舱输出目标经纬度（表4），从而判定： + +**这台无激光吊舱到底能不能自主完成目标地理定位，以及精度是多少。** + +如果它能输出表4 → 引导链路有输入；如果不能 → 目标距离与坐标必须由我方 +`stereo_ranger` 双目视距链路提供，`pod_bridge` 的架构要改。 +所以这是**分叉点实验**，值得先做。 + +--- + +## 1. 要回答的三个问题（按重要性排序） + +| # | 问题 | 决定什么 | +|---|---|---| +| **Q1** | 无激光时，吊舱**是否仍然解算并输出表4** | 部署文档 §10 悬而未决项。若表4 根本不发 → 吊舱无法提供目标坐标，表4 路径作废 | +| **Q2** | 表4 的**实际发送频率** | 直接决定 `guidance.target_stale_ms` 取值，以及 `--auto-unlock SEC` 能否启用（§10.1） | +| **Q3** | 目标丢失后，表4 是**停止更新**还是**继续吐旧值** | 停止更新 → 现有 1000ms 超时已兜住；继续 → 必须新增停滞检测或外部判据 | + +顺带还能白捡两个信息：**表4 的绝对/相对高度怎么填**（回答"是否假设目标在地面"）， +以及**表3 是否需要我方按真实 UTC 递增时间戳**（否则吊舱可能判载机数据无效）。 + +--- + +## 2. 前置条件（做之前逐条打勾） + +- [ ] 串口链路已通：`python -m pod_bridge.listen_pod /dev/ttyAMA0 --seconds 5` + → 应得 **299~300 帧 / 5s ≈ 60.0 Hz**（已实测达标） +- [ ] PL011 已切到 ttyAMA0、内核 console 已清、`serial-getty` 已禁（部署文档 §4） +- [ ] 台架固定，吊舱视野内有可锁定的目标 +- [ ] 已确认 `--detect` + `--auto-lock` 能让 bit2 置位（`pod_console.py`） +- [ ] ⚠ **同一时刻只能有一个进程占用串口**（见 §9 风险第 1 条） + +--- + +## 3. 实验拓扑 + +``` + ┌──────────────┐ 表3 (EB91, 20Hz, 我方伪造) ┌──────────┐ + │ 树莓派 4B │ ───────────────────────────→ │ │ + │ sim_fc.py │ │ 光电吊舱 │ + │ (假飞控) │ ←─────────────────────────── │ │ + └──────────────┘ 表4 (EE18) + 表2-2 (EE16) └──────────┘ + /dev/ttyAMA0 @115200 RX ← 与 TX 争同一个串口，必须收发一体 +``` + +关键点：**不能一边 `listen_pod.py` 一边 `sim_fc.py`** —— 两个进程抢同一个 +`/dev/ttyAMA0`，后者直接 `EBUSY` / 前者读数错帧。所以 `sim_fc.py` **必须收发 +一体**：同进程里既发也收并自己做统计，不要拆成两个脚本。 + +--- + +## 4. 阶段1（里程碑）：静态喂表3，看表4 是否出来 + +### 4.1 做什么 + +以固定频率循环下发**一套完全静止的假载机数据**： + +| 字段（`CarrierAttitude`, `protocol.py:248`） | 阶段1 取值 | 说明 | +|---|---|---| +| roll / pitch / yaw | 全 0 | 台架静止 | +| latitude / longitude | 固定值（待定，见 §7） | 建议填台架近似真实位置，别填 0/0 | +| abs_alt / rel_alt | 固定值（待定） | 无激光，只能估喷雾高度 | +| UTC 时戳 | **建议用真实系统 UTC 实时推进** | 先跑递增时间的版本；若怀疑吊舱校验时间，再单独做「冻结时戳」对照 | +| airspeed / ground_speed | 0 | 台架静止 | + +同时对下行做非阻塞统计：表2-2 帧数、bit2 锁定状态、**是否出现表4**、表4 频率与内容。 + +### 4.2 跑法（`sim_fc.py` 已实现，参数即 §7 敲定值） + +```bash +cd utils +# 默认值已按 §7 设定，不必再写 lat/lon/rel-alt/rate +python -m pod_bridge.sim_fc /dev/ttyAMA0 --seconds 60 --json phase1.json + +# 台架上验证目标识别 + 自动锁定同时进行解算观测 +python -m pod_bridge.sim_fc /dev/ttyAMA0 \ + --detect person --auto-lock --seconds 60 --json phase1.json +``` + +> 全部参数表与退出码见 §8。 + +### 4.3 判读对照表 + +| 观测 | 判读 | 下一步 | +|---|---|---| +| **出现表4** 且经纬度非零 | ✅ 无激光仍能解算 | 进阶段2 测精度 | +| 表4 全 0（有帧但内容为 0） | ⚠ 收到了但解算不出 | 加发更高 `--rel-alt`、尝试不同 `pitch`，仍无则问厂商"绝对/相对高度如何解算" | +| **完全不出现表4** | ❌ 该接口依赖激光/未使能 | **立即停下**：要么问厂商，要么转自有双目视距链路（`stereo_ranger`）提供目标坐标 | +| 表4 频率 < 5Hz 或忽快忽慢 | ⚠ 记录实际频率 | `--auto-unlock` 的 SEC 必须 ≥3 倍周期，否则误杀（§10.1） | + +### 4.4 本阶段的隐藏产出 + +**即使 Q1 答案是"能"**，也要顺手记下这三条，它们后面都要用： + +1. 表4 的实际频率（填 `--auto-unlock` 的 SEC） +2. 表4 里 `abs_alt` / `rel_alt` 的填法 —— 若恒等于我方喂进去的高度 → + **证明吊舱假设"目标在地面"**，这个假设在城市/楼顶/空中目标上会失效 +3. 厂商那个黑盒测试软件显示的**表3 内容是否与我方编码一致** —— 若一致， + 即交叉验证通过，可直接确认 `protocol.py` 的编码已对齐 docx 权威规约 + +--- + +## 5. 阶段2：拿已知真值评估精度 + +前提：阶段1 确认表4 会出。 + +### 5.1 静态精度 + +- 让一个人/车站在**已知 GPS 坐标**处（手机 GPS 或 RTK，误差记下来），锁定它。 +- 采集 ≥60 组表4，看坐标**聚类半径**与**是否跳变**。 +- 判据：静态目标下聚类半径应远小于后续控制用的 `stop_radius_m`（现 5m）。 + 若静态抖动就有好几米，飞行场景不可用。 + +### 5.2 相对高度敏感性扫描（本设备特有，无激光必须做） + +因为**没有激光测距**，吊舱只能靠"目标在画面里的位置 + 我喂的相对高度 + 假设 +目标贴地"去反推水平距离。这意味着：**相对高度的误差会被几何放大成水平误差。** + +做法：固定同一个目标，把 `--rel-alt` 依次打成 `30 / 50 / 80 / 120 m`， +每档采 60 组，画出水平误差随高度的变化。 + +| 现象 | 判读 | +|---|---| +| 水平误差随高度线性放大 | 印证"目标贴地假设"，此假设已知且可控 | +| 误差与高度无关地跳动 | 另有误差源（视场标定、姿态零点），需要厂商参数 | +| 某个高度档误差突然爆炸 | 该档附近可能超出解算有效范围，飞行高度要避开 | + +**这一节的结论要回去更新部署文档 §10 的"待向厂商确认"条目。** + +--- + +## 6. 阶段3（可选）：动态姿态一致性 + +前两阶段通过后，把 yaw 打成匀速绕圈（`--yaw-rate 30`）、pitch 小幅摆动， +验证动态姿态下解算是否自洽： + +- 目标真实静止时，表4 解算出的坐标**是否跟着姿态漂**； +- 姿态到 -179°/+179° 跨越时有没有符号/回绕 bug（`_to_s16` 已 clamp，但要验证吊舱侧）； +- 姿态更新速率与吊舱解算的相位关系（要不要加大 `--rate`）。 + +排优先级：**阶段1 > 阶段2 > 阶段3**。阶段3 做不做取决于前两步结论是否已经足够决策。 + +--- + +## 7. 参数已敲定(2026-09-29) + +四个参数全部做成命令行参数(不写死),默认值即为本次拍板结果: + +| 参数 | 敲定值 | 理由 | +|---|---|---| +| 静态经纬度 | **北纬 39.9042° / 东经 116.4074°** | 不填 0/0,防止吊舱侧"经纬度无效"直接拒解算 | +| 相对高度 | **50 m** | 典型作业高度。阶段2 再扫 `--rel-alt 30/80/120` 做敏感性分析 | +| 发送频率 | **20 Hz** | 规格 1~60Hz 的中段。脚本已加守卫,越界直接拒绝启动 | +| 时戳策略 | 默认**真实 UTC 递增**,`--freeze-time` 做对照组 | 隔离"是不是时间不递增导致吊舱不理我"这个变量 | + +> `--abs-alt` 未指定时自动取 `--rel-alt`(即假设起飞点海拔 0),避免 abs=0/rel=50 +> 这种不合物理的组合吊舱看不懂。 + +--- + +## 8. `sim_fc.py`（已实现 · 2026-09-29） + +主体：`utils/pod_bridge/sim_fc.py`　单测：`utils/tests/pod_bridge/sim_fc_test.py` +（21 个用例，纯软注入 fake serial，无需硬件；全量回归 372 passed） + +``` +python -m pod_bridge.sim_fc [options] + +位置参数: + port 串口设备（默认 /dev/ttyAMA0） + +可选: + --baud 115200 + --rate 20 表3 发送频率 Hz（规格 1~60，越界拒绝启动） + --seconds 60 运行时长，0 = 一直跑到 Ctrl+C + --lat 载机纬度（默认 39.9042） + --lon 载机经度（默认 116.4074） + --abs-alt 绝对高度（默认取 --rel-alt，即假设起飞点海拔 0） + --rel-alt 相对高度（默认 50） + --roll --pitch --yaw 静态姿态（默认 0） + --yaw-rate 阶段3 用：方位匀速推进，360° 自动回绕 + --speed 地速 >0 时位置沿 --yaw 方向匀速直线推进 + --freeze-time 时戳冻结在启动时刻（对照实验） + --detect {person,drone} 顺带发 目标识别开(0x07) + --auto-lock 在 0x07 后追加 0x0F + 0x10（须配合 --detect） + --motor-on 先发 开电机(0x27) + --unlock-on-exit 退出前发 0x0E 停止跟踪 + --json 同时落一份原始采样 JSON（含逐帧 east/north 偏移） + --quiet 不打印每秒进度，只在结束输出汇总 + +退出码（便于脚本串测）: + 0 = 收到 表4 且经纬度有效（Q1 通过） 1 = 完全未收到 表4 + 2 = 收到 表4 但经纬度恒为 0 3 = 串口链路错误 / 参数非法 +``` + +### 8.1 上机就跑（用户已敲定的默认值） + +```bash +cd utils +python -m pod_bridge.sim_fc /dev/ttyAMA0 \ + --detect person --auto-lock --seconds 60 --json phase1.json +``` + +要在台架上跑阶段2 的高度扫描时，改 `--rel-alt` 重跑即可 +（`30 / 50 / 80 / 120` 各一次，`--json` 分别落盘后做聚类对比）。 + +### 8.2 行为约定（实现已满足） + +1. **单进程收发一体**——独占串口，同进程发 表3、收 表2-2/表4； +2. 下行统计在热路径不打印，只在结束时输出**一张汇总表**（帧计数、实测频率、 + 首次锁定/首次表4 时刻、经纬度均值与 σ、相对我方的 east/north 偏移与散布、 + 表4 相对高度是否 ≈ 喂入值）； +3. `--json` 落盘逐帧原始数据，供阶段2 做散点/聚类分析； +4. `Ctrl+C` 优雅退出仍打印汇总，可选发 `0x0E` 解锁； +5. 零新依赖（仅 `pyserial`），沿用 `pod_serial.py` 的非阻塞轮询； +6. **顺带回环自检**：若收到自己发的 表3 帧，提示 TX/RX 疑似短接。 + +### 8.3 自动给出的三条判读提示（人工不用自己算） + +- 表4 相对高度 ≈ 喂入的载机高度 → 判定吊舱**假设"目标在地面"**，并警告该假设 + 对空中/楼顶目标失效（回答 §4.4 第 2 条）； +- 未收到表4 时按可能性排序列出原因，直接指向下一动作； +- 出现回环帧时提示查接线。 + +--- + +## 9. 风险与已知的坑 + +1. **串口独占**（最容易踩）：阶段实验期间不要同时开 `listen_pod.py` / + `pod_console.py` / `bridge.py`。要"边喂边看仪表盘"就得先给 `sim_fc.py` + 自己加面板，而不是并行两个进程。 +2. **假数据被当真**：本脚本只走串口、不碰 PX4，**绝不能**与真实 offboard + 同时接入同一飞控。台架阶段物理断开飞控连线最保险。 +3. **闭环误解**：阶段1~3 全是**开环观测**，不产生任何控制输出。别指望它验证 + 控制律 —— 那是之后飞起来的事。 +4. **无激光是硬约束**：表2-2 的激光字段恒 0（厂商确认），任何依赖它的判据都无效。 + 这也是为什么本实验要专门替吊舱把高度喂进去。 +5. **`--auto-unlock` 仍然别贸然开**：本实验只是**测量**它的前置条件（表4 是否存在 + 与其频率），不等于批准启用。启用与否仍按 §10.1 的次序：先确认表4 在发，再谈 SEC。 + +--- + +## 10. 结论的落点（做完写回哪里） + +| 结论 | 落到哪个文件 | +|---|---| +| Q1 是否输出表4 | `docs/树莓派4B部署注意事项.md` §10 —— 删掉/回答"待向厂商确认"条目 | +| Q2 表4 频率 | `utils/config/pod_bridge.json` → `guidance.target_stale_ms`、§10.1 的 SEC 建议值 | +| Q3 丢失行为 | §8 / §10.1 的目标丢失判据（补第二判据） | +| 高度假设的证据 | §10 "无激光如何解算高度"小节 | +| 静态精度 | 决定是否敢把 `stop_radius_m` 从 5m 往下调 | diff --git a/docs/树莓派4B部署注意事项.md b/docs/树莓派4B部署注意事项.md index 73fe52b..84b96a8 100644 --- a/docs/树莓派4B部署注意事项.md +++ b/docs/树莓派4B部署注意事项.md @@ -115,16 +115,22 @@ pytest # 仅上机自测需要,部署运行可省 ## 7. 上机验证顺序(每步过了再下一步) 1. **纯软件自测**(无需任何接线): - `cd utils && ~/venv-pod/bin/python -m pytest tests/pod_bridge -q` → 应 92 全绿 - (单测已打桩 mavsdk 设定点类型,与 libcmavsdk 原生库健康状态无关)。 + `cd utils && ~/venv-pod/bin/python -m pytest tests/pod_bridge -q` → 应 116 全绿 + (全量 372 条;pod_bridge 含 sim_fc 的 21 条。单测已打桩 mavsdk 设定点类型, + 与 libcmavsdk 原生库健康状态无关,也无需真实串口)。 2. **串口回环**:USB 转串口模块 TX/RX 短接,跑一个 feed/echo 小脚本验证字节收发。 3. **只接吊舱**:用仓库自带诊断脚本 `python utils/pod_bridge/listen_pod.py /dev/ttyUSB0 --seconds 5` (加 `--center` 可同时下发归中 0x2B 验证下行);脚本自动统计帧数并判读, 遥测帧率 ≥48Hz(标称 60Hz 的 80%)退出码为 0 即链路正常。 -4. **只接飞控**:`Px4Gateway.connect` 后确认 `own_state().position_valid` 为 True。 -5. **全链路地面联试**:电机不装桨,观察 offboard 心跳与引导输出。 -6. 最后才考虑带动力测试。 +4. **验证吊舱能否解算目标坐标**(不接飞控,避免绕过这一步): + `python -m pod_bridge.sim_fc /dev/ttyAMA0 --seconds 60 --json phase1.json` + 由树莓派伪造 表3 喂给吊舱,看是否回 表4。这一步的结论直接决定后续架构取舍, + 详见 `docs/吊舱解算验证方案-模拟飞控喂表3.md`。 + ⚠ 跑它的时候关掉 listen_pod / pod_console / bridge —— 同一个串口不能两人占。 +5. **只接飞控**:`Px4Gateway.connect` 后确认 `own_state().position_valid` 为 True。 +6. **全链路地面联试**:电机不装桨,观察 offboard 心跳与引导输出。 +7. 最后才考虑带动力测试。 ## 8. 已知问题处置状态(2026-09-23 更新) @@ -173,6 +179,9 @@ WantedBy=multi-user.target 永久刹车(该闸门是为"有激光且锁定位置位粘滞"的机型准备的); - ⚠ 需向厂商确认:无激光时吊舱如何解算表4 的目标绝对/相对高度 (是否假设目标在地面),这直接决定引导用的坐标精度。 + → 已给出**不依赖厂商答复的自测方案**:用树莓派伪造飞控数据喂 表3, + 逼吊舱吐 表4,见 `docs/吊舱解算验证方案-模拟飞控喂表3.md`。 + 该实验还顺带回答 §10.1 悬着的两个前置问题(表4 是否存在、实际频率)。 - **锁定位置位(表2-2 状态字节1 bit2)在目标丢失后不复位**——实测观察; 表2-2 脱靶量是定长字段且协议**没有无效标志位**,丢失时表现为 0, 无法区分"不输出"与"输出 0"。 diff --git a/utils/pod_bridge/sim_fc.py b/utils/pod_bridge/sim_fc.py new file mode 100644 index 0000000..baf5316 --- /dev/null +++ b/utils/pod_bridge/sim_fc.py @@ -0,0 +1,457 @@ +"""sim_fc.py —— 模拟飞控喂 表3,验证吊舱目标解算能力(台架工具,非飞行组件)。 + +用途:在**没有飞控、机体不动**的条件下,由树莓派单独向吊舱持续喂一套伪造的 + 表3 载机姿态/位置/时间,看吊舱能否据此解算出目标经纬度(表4),用于回答 + docs/吊舱解算验证方案-模拟飞控喂表3.md 的三个问题: + + Q1 无激光时吊舱是否仍然解算并输出 表4 + Q2 表4 的实际发送频率(决定 target_stale_ms / --auto-unlock SEC) + Q3 目标丢失后 表4 是停止更新还是继续吐旧值 + +设计约束(务必遵守): + * **单进程收发一体**。/tx 与 rx 是同一个串口,不能一边跑 listen_pod.py + 一边跑本脚本 —— 后者会 EBUSY,前者会读到错帧。要看仪表盘请在结束后看汇总; + * **不碰飞控**。本脚本只走吊舱串口,不产生任何控制输出,是纯开环观测; + * 核心逻辑(Feeder / Collector / 判定)与串口隔离,可纯软单测,见 + utils/tests/pod_bridge/test_sim_fc.py。 + +用法: + python -m pod_bridge.sim_fc /dev/ttyAMA0 --seconds 60 + python -m pod_bridge.sim_fc /dev/ttyAMA0 --detect drone --auto-lock --seconds 60 + python -m pod_bridge.sim_fc /dev/ttyAMA0 --rel-alt 80 --json run.json + +退出码(便于脚本串测): + 0 = 收到 表4 且经纬度有效(Q1 通过,可进阶段2) + 1 = 完全未收到 表4(Q1 失败) + 2 = 收到 表4 但经纬度恒为 0(有帧无解算) + 3 = 串口链路错误(接线/控制台占用等) +""" + +import argparse +import json +import math +import sys +import time +from datetime import datetime, timezone +from pathlib import Path +from typing import List, Optional, Sequence + +sys.path.insert(0, str(Path(__file__).resolve().parents[1])) # utils/ 入 sys.path + +from pod_bridge.pod_serial import PodSerial # noqa: E402 +from pod_bridge.protocol import (CarrierAttitude, ControlCode, # noqa: E402 + ControlCommand, TargetGeo, Telemetry, + encode_attitude, encode_control) + +# 退出码:见模块 docstring +EXIT_OK = 0 +EXIT_NO_TARGET_FRAME = 1 +EXIT_ZERO_GEO = 2 +EXIT_LINK_ERROR = 3 + +# 表2-1 0x07 目标识别开:参数X 0=检测人或车,1=检测无人机(规格 §2.2.1) +_DETECT_PARAM_X = {"person": 0, "drone": 1} + +_M_PER_DEG_LAT = 111320.0 # 纬度每度约合米(局部平面近似,够台架判读) + + +# --- 纯逻辑:不碰串口,便于单测 --------------------------------------------- +class Feeder: + """按拍生成伪造的 表3 载机数据。""" + + def __init__(self, lat_deg: float, lon_deg: float, abs_alt_m: float, + rel_alt_m: float, roll_deg: float = 0.0, pitch_deg: float = 0.0, + yaw_deg: float = 0.0, yaw_rate_dps: float = 0.0, + ground_speed_mps: float = 0.0, + freeze_time: bool = False) -> None: + self.lat = lat_deg + self.lon = lon_deg + self.abs_alt = abs_alt_m + self.rel_alt = rel_alt_m + self.base_roll = roll_deg + self.base_pitch = pitch_deg + self.base_yaw = yaw_deg + self.yaw_rate = yaw_rate_dps + self.ground_speed = ground_speed_mps + self.freeze_time = freeze_time + if freeze_time: + self._frozen = self._utc_fields(datetime.now(timezone.utc)) + + @staticmethod + def _utc_fields(now: datetime) -> dict: + return { + "year": now.year, "month": now.month, "day": now.day, + "hour": now.hour, "minute": now.minute, "second": now.second, + "centisecond": now.microsecond // 10000, + } + + def build(self, elapsed_s: float, now: Optional[datetime] = None) -> CarrierAttitude: + """生成 elapsed_s 时刻的一帧 表3(默认 yaw 与时戳随时间推进)。""" + fields = (self._frozen if self.freeze_time + else self._utc_fields(now or datetime.now(timezone.utc))) + yaw = (self.base_yaw + self.yaw_rate * elapsed_s) % 360.0 + # 地速不为 0 时让位置沿 yaw 方向做匀速直线推进(阶段3 用) + d_north = d_east = 0.0 + if self.ground_speed: + rad = math.radians(self.base_yaw) + dist = self.ground_speed * elapsed_s + d_north = dist * math.cos(rad) / _M_PER_DEG_LAT + d_east = dist * math.sin(rad) / (_M_PER_DEG_LAT + * math.cos(math.radians(self.lat))) + return CarrierAttitude( + roll_deg=self.base_roll, + pitch_deg=self.base_pitch, + yaw_deg=yaw, + latitude_deg=self.lat + d_north, + longitude_deg=self.lon + d_east, + abs_alt_m=self.abs_alt, + rel_alt_m=self.rel_alt, + airspeed_mps=self.ground_speed, + ground_speed_mps=self.ground_speed, + **fields, + ) + + +class Collector: + """接收侧统计。热路径只调用 update(),不打日志。""" + + def __init__(self, ref_lat_deg: float, ref_lon_deg: float) -> None: + self.ref_lat = ref_lat_deg + self.ref_lon = ref_lon_deg + self.n_tele = 0 + self.n_tgt = 0 + self.n_other = 0 + self.n_loopback = 0 # 疑似自发自收(收到自己发的 表3) + self.first_tgt_s: Optional[float] = None + self.last_tgt_s: Optional[float] = None + self.first_lock_s: Optional[float] = None + self.n_lock_events = 0 + self._last_locked: Optional[bool] = None + self._lats: List[float] = [] + self._lons: List[float] = [] + self._abs_alts: List[float] = [] + self._rel_alts: List[float] = [] + self._east: List[float] = [] + self._north: List[float] = [] + self.records: List[dict] = [] + + # --- 每帧更新(热路径) --- + def update(self, frame, now_s: float) -> None: + if isinstance(frame, Telemetry): + self.n_tele += 1 + locked = frame.is_target_locked + if self._last_locked is not None and locked != self._last_locked: + self.n_lock_events += 1 + if locked and self.first_lock_s is None: + self.first_lock_s = now_s + self._last_locked = locked + elif isinstance(frame, TargetGeo): + self.n_tgt += 1 + if self.first_tgt_s is None: + self.first_tgt_s = now_s + self.last_tgt_s = now_s + east, north = offset_m(self.ref_lat, self.ref_lon, + frame.latitude_deg, frame.longitude_deg) + self._lats.append(frame.latitude_deg) + self._lons.append(frame.longitude_deg) + self._abs_alts.append(frame.abs_alt_m) + self._rel_alts.append(frame.rel_alt_m) + self._east.append(east) + self._north.append(north) + self.records.append({ + "t_s": round(now_s, 3), + "lat": frame.latitude_deg, "lon": frame.longitude_deg, + "abs_alt_m": frame.abs_alt_m, "rel_alt_m": frame.rel_alt_m, + "east_m": east, "north_m": north, + "locked": self._last_locked, + }) + elif isinstance(frame, CarrierAttitude): + self.n_loopback += 1 + else: + self.n_other += 1 + + @property + def has_valid_geo(self) -> bool: + """表4 是否出现过非零经纬度。""" + return any(lat != 0.0 or lon != 0.0 + for lat, lon in zip(self._lats, self._lons)) + + def summary(self, elapsed_s: float, n_sent: int, + ref_abs_alt: float, ref_rel_alt: float) -> dict: + """生成汇总 dict(elapsed_s <= 0 时频率按 0 处理)。""" + def _hz(n: int) -> float: + return n / elapsed_s if elapsed_s > 0 else 0.0 + + jitter = (ptpm(self._east), ptpm(self._north)) + return { + "elapsed_s": round(elapsed_s, 2), + "has_valid_geo": self.has_valid_geo, + "attitude_sent": n_sent, + "attitude_hz": round(_hz(n_sent), 2), + "telemetry_frames": self.n_tele, + "telemetry_hz": round(_hz(self.n_tele), 2), + "lock_events": self.n_lock_events, + "first_lock_s": self.first_lock_s, + "target_frames": self.n_tgt, + "target_hz": round(_hz(self.n_tgt), 2), + "first_target_s": self.first_tgt_s, + "last_target_s": self.last_tgt_s, + "target_stale_s": (round(elapsed_s - self.last_tgt_s, 2) + if self.last_tgt_s is not None else None), + "lat_mean": _mean(self._lats), + "lon_mean": _mean(self._lons), + "lat_stdev_deg": round(stddev(self._lats), 9), + "lon_stdev_deg": round(stddev(self._lons), 9), + "east_mean_m": round(_mean(self._east), 2), + "north_mean_m": round(_mean(self._north), 2), + "spread_east_m": round(jitter[0], 2), + "spread_north_m": round(jitter[1], 2), + "abs_alt_mean_m": round(_mean(self._abs_alts), 2), + "rel_alt_mean_m": round(_mean(self._rel_alts), 2), + "ref_abs_alt_m": ref_abs_alt, + "ref_rel_alt_m": ref_rel_alt, + "loopback_frames": self.n_loopback, + "other_frames": self.n_other, + } + + +def offset_m(ref_lat: float, ref_lon: float, lat: float, + lon: float) -> tuple: + """目标相对参考点的水平偏移(east_m, north_m),局部平面近似。""" + north = (lat - ref_lat) * _M_PER_DEG_LAT + east = (lon - ref_lon) * _M_PER_DEG_LAT * math.cos(math.radians(ref_lat)) + return east, north + + +def _mean(values: Sequence[float]) -> float: + return sum(values) / len(values) if values else 0.0 + + +def stddev(values: Sequence[float]) -> float: + """总体标准差(空序列返回 0)。""" + if len(values) < 2: + return 0.0 + mu = _mean(values) + return math.sqrt(sum((v - mu) ** 2 for v in values) / len(values)) + + +def ptpm(values: Sequence[float]) -> float: + """峰峰值 max-min,空序列返回 0。""" + return max(values) - min(values) if values else 0.0 + + +def verdict(summary: dict, link_error: Optional[BaseException]) -> tuple: + """判读:(退出码, 结论文本列表)。""" + lines = [] + if link_error is not None: + lines.append(f"❌ 串口链路中断: {link_error}") + lines.append(" 排查: ① 是否另有进程占用该串口(listen_pod/pod_console/bridge); " + "② 内核控制台未清(serial-getty / cmdline.txt 的 console=); " + "③ TX/RX 是否交叉、共地、波特率一致") + return EXIT_LINK_ERROR, lines + if summary["loopback_frames"]: + lines.append(f"⚠ 收到 {summary['loopback_frames']} 帧 表3 回环:" + " TX/RX 疑似短接或自发自收,请查接线") + + n_tgt = summary["target_frames"] + if n_tgt == 0: + lines.append("❌ Q1 失败: 完全未收到 表4(EE 18)。可能原因:") + lines.append(" ① 未锁定目标 —— 加 --detect person/drone --auto-lock 重试;") + lines.append(" ② 吊舱无激光,确实不做目标地理解算 —— 转自有双目视距链路;") + lines.append(" ③ 需要先给表3 喂若干秒后吊舱才开始解算(已跑 %.0fs)" + % summary["elapsed_s"]) + return EXIT_NO_TARGET_FRAME, lines + if not summary["has_valid_geo"]: + lines.append("⚠ Q1 存疑: 收到 %d 帧 表4 但经纬度恒为 0 —— 有帧无解算。" % n_tgt) + lines.append(" 建议: 换更高的 --rel-alt、确认目标已锁定," + "并准备向厂商确认「无激光时如何解算目标绝对/相对高度」") + return EXIT_ZERO_GEO, lines + + lines.append("✅ Q1 通过: 无激光仍能解算 表4," + f"实测 {summary['target_hz']:.2f} Hz") + if summary["first_target_s"] is not None: + lines.append(f" Q2 表4 频率 {summary['target_hz']:.2f}Hz," + f"首次出现于 {summary['first_target_s']:.1f}s") + if summary["first_lock_s"] is not None: + lines.append(f" 首次锁定 {summary['first_lock_s']:.1f}s →" + f" 表4 滞后 {summary['first_target_s'] - summary['first_lock_s']:.1f}s") + lines.append(f" 目标相对我方: 东 {summary['east_mean_m']:+.1f}m" + f" 北 {summary['north_mean_m']:+.1f}m," + f"散布 E/W {summary['spread_east_m']:.2f}m" + f" N/S {summary['spread_north_m']:.2f}m") + # 「目标贴地」假设的证据:表4 高度是否等于我方喂进去的载机高度 + if abs(summary["rel_alt_mean_m"] - summary["ref_rel_alt_m"]) < 1.0: + lines.append(" ⚠ 表4 相对高度 ≈ 我方喂入的载机相对高度" + f"({summary['ref_rel_alt_m']:.0f}m)" + " → 吊舱很可能假设「目标在地面」,该假设对空中/楼顶目标失效") + return EXIT_OK, lines + + +# --- 命令行入口 ------------------------------------------------------------- +def main(argv: Optional[Sequence[str]] = None) -> int: + ap = argparse.ArgumentParser(description="模拟飞控喂 表3,验证吊舱解算能力") + ap.add_argument("port", nargs="?", default="/dev/ttyAMA0", + help="串口设备号(默认 /dev/ttyAMA0)") + ap.add_argument("--baud", type=int, default=115200, help="波特率(默认 115200)") + ap.add_argument("--seconds", type=float, default=60.0, + help="运行秒数(默认 60;0 = 一直跑到 Ctrl+C)") + ap.add_argument("--rate", type=float, default=20.0, + help="表3 发送频率 Hz(规格 1~60,默认 20)") + ap.add_argument("--lat", type=float, default=39.9042, + help="伪造载机纬度(默认 39.9042)") + ap.add_argument("--lon", type=float, default=116.4074, + help="伪造载机经度(默认 116.4074)") + ap.add_argument("--abs-alt", type=float, default=None, + help="伪造绝对高度 m(默认取 --rel-alt,即假设起飞点海拔 0)") + ap.add_argument("--rel-alt", type=float, default=50.0, + help="伪造相对高度 m(默认 50,典型作业高度)") + ap.add_argument("--roll", type=float, default=0.0, help="静态横滚 °") + ap.add_argument("--pitch", type=float, default=0.0, help="静态俯仰 °") + ap.add_argument("--yaw", type=float, default=0.0, help="起始方位 °") + ap.add_argument("--yaw-rate", type=float, default=0.0, + help="方位角速度 °/s(阶段3 动态姿态验证用)") + ap.add_argument("--speed", type=float, default=0.0, + help="伪造地速 m/s(>0 时位置沿 --yaw 方向匀速推进)") + ap.add_argument("--freeze-time", action="store_true", + help="表3 时戳冻结在启动时刻(对照实验:排除吊舱校验时间)") + ap.add_argument("--detect", choices=sorted(_DETECT_PARAM_X), + help="启动时下发 目标识别开(0x07)") + ap.add_argument("--auto-lock", action="store_true", + help="在目标识别后追加 自动锁定(0x0F)+ 识别转跟踪(0x10)") + ap.add_argument("--motor-on", action="store_true", help="启动时先下发 开电机(0x27)") + ap.add_argument("--unlock-on-exit", action="store_true", + help="退出前发 0x0E 停止跟踪") + ap.add_argument("--json", metavar="PATH", help="同时落一份原始采样 JSON") + ap.add_argument("--quiet", action="store_true", help="不打印每秒进度,只在结束输出汇总") + args = ap.parse_args(argv) + + if not 1.0 <= args.rate <= 60.0: + print("[sim_fc] --rate 应在规格 1~60Hz 范围内(当前 %.1f)" % args.rate) + return EXIT_LINK_ERROR + if args.auto_lock and not args.detect: + print("[sim_fc] --auto-lock 需配合 --detect" + "(0x0F 是常驻模式,但要先开 0x07 才有识别源)") + return EXIT_LINK_ERROR + if not args.quiet: + print(f"[sim_fc] {args.port} @ {args.baud} — 喂表3 lat={args.lat} " + f"lon={args.lon} rel_alt={args.rel_alt}m @{args.rate}Hz, " + f"{args.seconds:.0f}s Ctrl+C 可提前结束") + + abs_alt = args.abs_alt if args.abs_alt is not None else args.rel_alt + feeder = Feeder(args.lat, args.lon, abs_alt, args.rel_alt, + args.roll, args.pitch, args.yaw, args.yaw_rate, + args.speed, args.freeze_time) + collector = Collector(args.lat, args.lon) + + try: + pod = PodSerial(args.port, args.baud) + except Exception as exc: + print(f"[sim_fc] 打开串口失败: {exc}") + return EXIT_LINK_ERROR + + if args.motor_on: + pod.send(encode_control(ControlCommand(code=ControlCode.MOTOR_ON))) + if not args.quiet: + print("[sim_fc] 已下发 开电机(0x27)") + if args.detect: + px = _DETECT_PARAM_X[args.detect] + pod.send(encode_control(ControlCommand( + code=ControlCode.TARGET_DETECT_ON, param_x=px))) + if not args.quiet: + print(f"[sim_fc] 已下发 目标识别开(0x07, X={px} " + f"{'人/车' if px == 0 else '无人机'})") + if args.auto_lock: + pod.send(encode_control( + ControlCommand(code=ControlCode.AUTO_LOCK_ON_FOUND))) + pod.send(encode_control( + ControlCommand(code=ControlCode.DETECT_TO_TRACK))) + if not args.quiet: + print("[sim_fc] 已追加 自动锁定(0x0F)+ 识别转跟踪(0x10)") + + link_error: Optional[BaseException] = None + t0 = time.monotonic() + next_send = 0.0 + next_report = 1.0 + n_sent = 0 + + try: + while True: + now_s = time.monotonic() - t0 + if args.seconds > 0 and now_s >= args.seconds: + break + + if now_s >= next_send: + try: + pod.send(encode_attitude(feeder.build(now_s))) + n_sent += 1 + except OSError as exc: + link_error = exc + break + next_send += 1.0 / args.rate + if next_send < now_s: # 落后太多(系统卡顿)则重置节拍 + next_send = now_s + 1.0 / args.rate + + try: + for frame in pod.poll(): + collector.update(frame, now_s) + except OSError as exc: + link_error = exc + break + + if not args.quiet and now_s >= next_report: + print(f"[sim_fc] t={now_s:5.1f}s 发表3={n_sent} " + f"遥测={collector.n_tele} 目标={collector.n_tgt} " + f"锁定次数={collector.n_lock_events}") + next_report += 1.0 + time.sleep(0.002) + except KeyboardInterrupt: + if not args.quiet: + print("\n[sim_fc] 用户中断,仍输出汇总") + + elapsed = time.monotonic() - t0 + if args.unlock_on_exit: + try: + pod.send(encode_control(ControlCommand(code=ControlCode.STOP_TRACK))) + except OSError: + pass + try: + pod.close() + except OSError: + pass + + summary = collector.summary(elapsed, n_sent, abs_alt, args.rel_alt) + code, lines = verdict(summary, link_error) + + print("\n[sim_fc] ===== 汇总 =====") + print(f" 运行时长 {summary['elapsed_s']:.1f}s") + print(f" 表3 发送 {summary['attitude_sent']} 帧 ({summary['attitude_hz']:.1f}Hz)") + print(f" 表2-2 遥测 {summary['telemetry_frames']} 帧 ({summary['telemetry_hz']:.1f}Hz)") + print(f" 锁定次数 {summary['lock_events']}" + + (f", 首次 {summary['first_lock_s']:.1f}s" + if summary['first_lock_s'] is not None else " (未观察到锁定)")) + print(f" 表4 目标 {summary['target_frames']} 帧 ({summary['target_hz']:.2f}Hz)") + if summary["target_frames"]: + print(f" 经纬度均值 {summary['lat_mean']:.7f}, {summary['lon_mean']:.7f}") + print(f" 散布 σ lat {summary['lat_stdev_deg']:.2e}° " + f"lon {summary['lon_stdev_deg']:.2e}°") + print(f" 相对我方 东 {summary['east_mean_m']:+.1f}m " + f"北 {summary['north_mean_m']:+.1f}m " + f"(散布 {summary['spread_east_m']:.2f}m / {summary['spread_north_m']:.2f}m)") + print(f" 绝对/相对高度 {summary['abs_alt_mean_m']:.1f} / " + f"{summary['rel_alt_mean_m']:.1f} m (喂入 {abs_alt:.0f} / {args.rel_alt:.0f})") + if summary["target_stale_s"] is not None: + print(f" 最后更新距今 {summary['target_stale_s']}s") + for ln in lines: + print(" " + ln) + + if args.json: + payload = {"args": vars(args), "summary": summary, + "samples": collector.records} + Path(args.json).write_text(json.dumps(payload, ensure_ascii=False, indent=2), + encoding="utf-8") + print(f"[sim_fc] 原始采样已写入 {args.json}") + return code + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/utils/tests/pod_bridge/sim_fc_test.py b/utils/tests/pod_bridge/sim_fc_test.py new file mode 100644 index 0000000..fbd5c77 --- /dev/null +++ b/utils/tests/pod_bridge/sim_fc_test.py @@ -0,0 +1,269 @@ +"""sim_fc_test.py —— 模拟飞控喂表3 工具的纯软单测(注入 fake serial,无硬件)。 + +覆盖: + * Feeder:静态姿态 / 时戳冻结 / yaw 匀速推进 / 地速位移; + * offset_m: East/North 换算方向; + * Collector:遥测与表4 计数、锁定跳变、贴地假设、回环帧识别; + * stddev / ptpm 统计; + * verdict 四种判读分支; + * main() 端到端(假串口回回放表4)→ 退出码 0。 +""" + +import sys +import types + +import pytest + +from pod_bridge.protocol import (CarrierAttitude, TargetGeo, Telemetry, + encode_attitude, encode_target) +from pod_bridge.sim_fc import (EXIT_LINK_ERROR, EXIT_NO_TARGET_FRAME, EXIT_OK, + EXIT_ZERO_GEO, Collector, Feeder, main, offset_m, + ptpm, stddev, verdict) + +_LAT, _LON = 39.9042, 116.4074 + + +class _FakeSerialInstance: + """pyserial.Serial 的最小替身:可预设 RX 字节,记录 TX 写入。""" + + def __init__(self, **kwargs): + self.kwargs = kwargs + self._buf = b"" + self.written = [] + self.closed = False + + def seed_rx(self, data: bytes) -> None: + self._buf += data + + @property + def in_waiting(self): + return len(self._buf) + + def read(self, n): + data, self._buf = self._buf[:n], self._buf[n:] + return data + + def write(self, data): + self.written.append(bytes(data)) + return len(data) + + def close(self): + self.closed = True + + +@pytest.fixture +def fake_serial(monkeypatch): + """注入 serial 模块并返回**共享实例**——PodSerial 内部 Serial(...) 必须拿到 + 同一个对象,否则外部 seed_rx 预置的字节 main() 读不到。""" + mod = types.ModuleType("serial") + mod.EIGHTBITS = 8 + mod.PARITY_NONE = "N" + mod.STOPBITS_ONE = 1 + shared = _FakeSerialInstance(port="FAKE") + mod.Serial = lambda **kwargs: shared + monkeypatch.setitem(sys.modules, "serial", mod) + return shared + + +def _locked_telemetry(locked: bool) -> Telemetry: + status1 = (1 << 2) if locked else 0 + return Telemetry(status1=status1) + + +# --- Feeder ----------------------------------------------------------------- +def test_feeder_static_defaults(): + f = Feeder(_LAT, _LON, abs_alt_m=50.0, rel_alt_m=50.0) + frame = f.build(0.0) + assert isinstance(frame, CarrierAttitude) + assert (frame.latitude_deg, frame.longitude_deg) == (_LAT, _LON) + assert (frame.roll_deg, frame.pitch_deg, frame.yaw_deg) == (0.0, 0.0, 0.0) + assert (frame.abs_alt_m, frame.rel_alt_m) == (50.0, 50.0) + assert 0 <= frame.centisecond <= 99 + + +def test_feeder_freeze_time_keeps_stamp(): + f = Feeder(_LAT, _LON, 50.0, 50.0, freeze_time=True) + assert f.build(0.0).second == f.build(30.0).second + + +def test_feeder_yaw_sweep_wraps(): + f = Feeder(_LAT, _LON, 50.0, 50.0, yaw_deg=350.0, yaw_rate_dps=20.0) + assert abs(f.build(0.0).yaw_deg - 350.0) < 1e-9 + assert abs(f.build(1.0).yaw_deg - 10.0) < 1e-9 # 360 → 回绕到 10° + + +def test_feeder_ground_speed_moves_position_north(): + f = Feeder(_LAT, _LON, 50.0, 50.0, yaw_deg=0.0, ground_speed_mps=10.0) + later = f.build(10.0) # 向北 100m + north_m = (later.latitude_deg - _LAT) * 111320.0 + assert 95.0 < north_m < 105.0 + assert abs((later.longitude_deg - _LON) * 111320.0) < 1.0 + + +# --- 几何 / 统计 ------------------------------------------------------------ +def test_offset_m_directions(): + east, north = offset_m(0.0, 0.0, 0.0, 0.0001) + assert north == pytest.approx(0.0, abs=1e-6) + assert east > 10.0 + east2, north2 = offset_m(0.0, 0.0, 0.0001, 0.0) + assert east2 == pytest.approx(0.0, abs=1e-6) + assert north2 > 10.0 + + +def test_stddev_and_ptpm(): + assert stddev([1.0]) == 0.0 + assert stddev([]) == 0.0 + assert stddev([2.0, 2.0, 2.0]) == 0.0 + assert stddev([1.0, 3.0]) == pytest.approx(1.0) + assert ptpm([]) == 0.0 + assert ptpm([1.0, 5.0, 3.0]) == pytest.approx(4.0) + + +# --- Collector -------------------------------------------------------------- +def test_collector_counts_and_lock_events(): + c = Collector(_LAT, _LON) + for t, lk in ((0.1, False), (0.2, True), (0.3, True), (0.4, False)): + c.update(_locked_telemetry(lk), t) + assert c.n_tele == 4 + assert c.n_lock_events == 2 # False→True, True→False + assert c.first_lock_s == pytest.approx(0.2) + assert c.n_tgt == 0 + + +def test_collector_target_stats_and_first_frame(): + c = Collector(_LAT, _LON) + assert c.has_valid_geo is False + c.update(TargetGeo(latitude_deg=_LAT, longitude_deg=_LON), 1.0) + c.update(TargetGeo(latitude_deg=_LAT + 0.0001, longitude_deg=_LON), 2.0) + assert c.has_valid_geo is True + assert c.first_tgt_s == pytest.approx(1.0) + assert c.last_tgt_s == pytest.approx(2.0) + north = c.records[-1]["north_m"] + assert north == pytest.approx(11.132, abs=0.5) + + +def test_collector_flags_loopback_and_unknown(): + c = Collector(_LAT, _LON) + c.update(CarrierAttitude(), 0.1) + c.update(object(), 0.2) + assert c.n_loopback == 1 + assert c.n_other == 1 + + +def test_collector_summary_includes_geo_validity(): + c = Collector(_LAT, _LON) + assert c.summary(1.0, 20, 50.0, 50.0)["has_valid_geo"] is False + c.update(TargetGeo(latitude_deg=_LAT, longitude_deg=_LON, rel_alt_m=50.0), 0.5) + summary = c.summary(1.0, 20, 50.0, 50.0) + assert summary["has_valid_geo"] is True + assert summary["target_hz"] == pytest.approx(1.0) + assert summary["target_stale_s"] == pytest.approx(0.5) + + +# --- verdict ---------------------------------------------------------------- +def _summary(**over): + base = { + "elapsed_s": 10.0, "target_frames": 0, "target_hz": 0.0, + "first_target_s": None, "first_lock_s": None, "has_valid_geo": False, + "loopback_frames": 0, "east_mean_m": 0.0, "north_mean_m": 0.0, + "spread_east_m": 0.0, "spread_north_m": 0.0, + "rel_alt_mean_m": 0.0, "ref_rel_alt_m": 50.0, + } + base.update(over) + return base + + +def test_verdict_no_target_frame(): + code, lines = verdict(_summary(), None) + assert code == EXIT_NO_TARGET_FRAME + assert any("Q1 失败" in ln for ln in lines) + + +def test_verdict_zero_geo(): + code, lines = verdict(_summary(target_frames=100), None) + assert code == EXIT_ZERO_GEO + assert any("有帧无解算" in ln for ln in lines) + + +def test_verdict_ok_and_ground_plane_warning(): + code, lines = verdict(_summary(target_frames=100, target_hz=5.5, + has_valid_geo=True, first_target_s=3.4, + first_lock_s=3.0, rel_alt_mean_m=50.0), None) + assert code == EXIT_OK + assert any("Q1 通过" in ln for ln in lines) + assert any("目标在地面" in ln for ln in lines) # 表4 相对高度 ≈ 喂入值 + + +def test_verdict_link_error_takes_priority(): + code, lines = verdict(_summary(target_frames=100, has_valid_geo=True), + OSError(5, "Input/output error")) + assert code == EXIT_LINK_ERROR + assert any("链路中断" in ln for ln in lines) + + +def test_verdict_flags_loopback(): + code, lines = verdict(_summary(target_frames=1, has_valid_geo=True, + loopback_frames=7), None) + assert code == EXIT_OK + assert any("回环" in ln for ln in lines) + + +# --- main() 端到端(假串口回环) --------------------------------------------- +def test_main_end_to_end_reports_target(fake_serial, tmp_path, capsys): + inst = fake_serial + target = TargetGeo(latitude_deg=_LAT + 0.0002, longitude_deg=_LON, + abs_alt_m=50.0, rel_alt_m=50.0) + inst.seed_rx(encode_target(target)) + + code = main(["FAKE", "--seconds", "0.05", "--rate", "60", + "--rel-alt", "50", "--quiet", "--json", str(tmp_path / "r.json")]) + out = capsys.readouterr().out + + assert code == EXIT_OK + assert inst.closed is True + assert len(inst.written) >= 1 # 至少发过一帧 表3 + assert inst.written[0].startswith(b"\xeb\x91") # 同步字 EB 91 + assert "Q1 通过" in out + assert (tmp_path / "r.json").exists() + + +def test_main_no_target_returns_error(fake_serial, capsys): + fake_serial # RX 为空 + code = main(["FAKE", "--seconds", "0.05", "--rate", "60", "--quiet"]) + assert code == EXIT_NO_TARGET_FRAME + + +def test_main_rejects_bad_rate(fake_serial, capsys): + code = main(["FAKE", "--rate", "100", "--quiet"]) + out = capsys.readouterr().out + assert code == EXIT_LINK_ERROR + assert "1~60Hz" in out + + +def test_main_auto_lock_requires_detect(fake_serial, capsys): + code = main(["FAKE", "--auto-lock", "--quiet"]) + out = capsys.readouterr().out + assert code == EXIT_LINK_ERROR + assert "--auto-lock 需配合" in out + + +def test_main_sends_detect_sequence(fake_serial, capsys): + inst = fake_serial + main(["FAKE", "--seconds", "0.02", "--rate", "60", "--quiet", + "--detect", "drone", "--auto-lock", "--motor-on"]) + codes = [w[2] for w in inst.written] + assert 0x27 in codes # 开电机 + assert 0x07 in codes # 目标识别开 + assert 0x0F in codes # 自动锁定 + assert 0x10 in codes # 识别转跟踪 + + +def test_feeder_frame_is_encodable(): + """Feeder 产物必须能通过 protocol 编码/解码往返(防止字段对不上规格)。""" + from pod_bridge.protocol import decode_attitude + f = Feeder(_LAT, _LON, 50.0, 50.0, roll_deg=1.5, pitch_deg=-2.0, yaw_deg=30.0) + frame = f.build(0.0) + back = decode_attitude(encode_attitude(frame)) + assert back.latitude_deg == pytest.approx(frame.latitude_deg, abs=1e-7) + assert back.rel_alt_m == pytest.approx(50.0, abs=0.2) # ALT_SCALE=0.2m + assert back.yaw_deg == pytest.approx(30.0, abs=0.01)