新增sim_fc.py: 模拟飞控喂表3验证吊舱解算(第1阶段)
- 伪造载机姿态/位置/时戳持续喂表3,单进程收发一体观测表2-2/表4 - 回答Q1(无激光是否仍解算表4)/Q2(表4实际频率)/Q3(丢失后行为) - 自动判读「目标贴地」假设: 表4相对高度是否约等于喂入的载机高度 - 退出码0/1/2/3便于脚本串测; --json落逐帧采样供阶段2聚类分析 - 默认参数已敲定: lat=39.9042 lon=116.4074 rel-alt=50 rate=20Hz - 纯软单测21条(注入fake serial), 全量回归372 passed
This commit is contained in:
keita
@@ -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 <port> [options]
|
||||||
|
|
||||||
|
位置参数:
|
||||||
|
port 串口设备（默认 /dev/ttyAMA0）
|
||||||
|
|
||||||
|
可选:
|
||||||
|
--baud 115200
|
||||||
|
--rate 20 表3 发送频率 Hz（规格 1~60，越界拒绝启动）
|
||||||
|
--seconds 60 运行时长，0 = 一直跑到 Ctrl+C
|
||||||
|
--lat <deg> 载机纬度（默认 39.9042）
|
||||||
|
--lon <deg> 载机经度（默认 116.4074）
|
||||||
|
--abs-alt <m> 绝对高度（默认取 --rel-alt，即假设起飞点海拔 0）
|
||||||
|
--rel-alt <m> 相对高度（默认 50）
|
||||||
|
--roll --pitch --yaw <deg> 静态姿态（默认 0）
|
||||||
|
--yaw-rate <dps> 阶段3 用：方位匀速推进，360° 自动回绕
|
||||||
|
--speed <m/s> 地速 >0 时位置沿 --yaw 方向匀速直线推进
|
||||||
|
--freeze-time 时戳冻结在启动时刻（对照实验）
|
||||||
|
--detect {person,drone} 顺带发 目标识别开(0x07)
|
||||||
|
--auto-lock 在 0x07 后追加 0x0F + 0x10（须配合 --detect）
|
||||||
|
--motor-on 先发 开电机(0x27)
|
||||||
|
--unlock-on-exit 退出前发 0x0E 停止跟踪
|
||||||
|
--json <path> 同时落一份原始采样 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 往下调 |
|
||||||
+14
-5
@@ -115,16 +115,22 @@ pytest # 仅上机自测需要,部署运行可省
|
|||||||
## 7. 上机验证顺序(每步过了再下一步)
|
## 7. 上机验证顺序(每步过了再下一步)
|
||||||
|
|
||||||
1. **纯软件自测**(无需任何接线):
|
1. **纯软件自测**(无需任何接线):
|
||||||
`cd utils && ~/venv-pod/bin/python -m pytest tests/pod_bridge -q` → 应 92 全绿
|
`cd utils && ~/venv-pod/bin/python -m pytest tests/pod_bridge -q` → 应 116 全绿
|
||||||
(单测已打桩 mavsdk 设定点类型,与 libcmavsdk 原生库健康状态无关)。
|
(全量 372 条;pod_bridge 含 sim_fc 的 21 条。单测已打桩 mavsdk 设定点类型,
|
||||||
|
与 libcmavsdk 原生库健康状态无关,也无需真实串口)。
|
||||||
2. **串口回环**:USB 转串口模块 TX/RX 短接,跑一个 feed/echo 小脚本验证字节收发。
|
2. **串口回环**:USB 转串口模块 TX/RX 短接,跑一个 feed/echo 小脚本验证字节收发。
|
||||||
3. **只接吊舱**:用仓库自带诊断脚本
|
3. **只接吊舱**:用仓库自带诊断脚本
|
||||||
`python utils/pod_bridge/listen_pod.py /dev/ttyUSB0 --seconds 5`
|
`python utils/pod_bridge/listen_pod.py /dev/ttyUSB0 --seconds 5`
|
||||||
(加 `--center` 可同时下发归中 0x2B 验证下行);脚本自动统计帧数并判读,
|
(加 `--center` 可同时下发归中 0x2B 验证下行);脚本自动统计帧数并判读,
|
||||||
遥测帧率 ≥48Hz(标称 60Hz 的 80%)退出码为 0 即链路正常。
|
遥测帧率 ≥48Hz(标称 60Hz 的 80%)退出码为 0 即链路正常。
|
||||||
4. **只接飞控**:`Px4Gateway.connect` 后确认 `own_state().position_valid` 为 True。
|
4. **验证吊舱能否解算目标坐标**(不接飞控,避免绕过这一步):
|
||||||
5. **全链路地面联试**:电机不装桨,观察 offboard 心跳与引导输出。
|
`python -m pod_bridge.sim_fc /dev/ttyAMA0 --seconds 60 --json phase1.json`
|
||||||
6. 最后才考虑带动力测试。
|
由树莓派伪造 表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 更新)
|
## 8. 已知问题处置状态(2026-09-23 更新)
|
||||||
|
|
||||||
@@ -173,6 +179,9 @@ WantedBy=multi-user.target
|
|||||||
永久刹车(该闸门是为"有激光且锁定位置位粘滞"的机型准备的);
|
永久刹车(该闸门是为"有激光且锁定位置位粘滞"的机型准备的);
|
||||||
- ⚠ 需向厂商确认:无激光时吊舱如何解算表4 的目标绝对/相对高度
|
- ⚠ 需向厂商确认:无激光时吊舱如何解算表4 的目标绝对/相对高度
|
||||||
(是否假设目标在地面),这直接决定引导用的坐标精度。
|
(是否假设目标在地面),这直接决定引导用的坐标精度。
|
||||||
|
→ 已给出**不依赖厂商答复的自测方案**:用树莓派伪造飞控数据喂 表3,
|
||||||
|
逼吊舱吐 表4,见 `docs/吊舱解算验证方案-模拟飞控喂表3.md`。
|
||||||
|
该实验还顺带回答 §10.1 悬着的两个前置问题(表4 是否存在、实际频率)。
|
||||||
- **锁定位置位(表2-2 状态字节1 bit2)在目标丢失后不复位**——实测观察;
|
- **锁定位置位(表2-2 状态字节1 bit2)在目标丢失后不复位**——实测观察;
|
||||||
表2-2 脱靶量是定长字段且协议**没有无效标志位**,丢失时表现为 0,
|
表2-2 脱靶量是定长字段且协议**没有无效标志位**,丢失时表现为 0,
|
||||||
无法区分"不输出"与"输出 0"。
|
无法区分"不输出"与"输出 0"。
|
||||||
|
|||||||
@@ -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())
|
||||||
@@ -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)
|
||||||
Reference in New Issue
Block a user