新增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. 上机验证顺序(每步过了再下一步)
|
||||
|
||||
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"。
|
||||
|
||||
Reference in New Issue
Block a user