新增pod_bridge吊舱桥接中间件(树莓派4B部署)

- pod_bridge: 协议G版编解码/地理位置引导律/PX4网关/串口链路/闭环主循环
- 含三项安全修复: position_time_ms拆分、表2-2锁定门控、run()异常护栏
- 测试92条全绿; 附树莓派4B(Ubuntu 24.04)部署注意事项与最小依赖清单

docs/树莓派4B部署注意事项.md

@@ -0,0 +1,143 @@
+# pod_bridge 移交树莓派 4B 部署注意事项
+
+> 目标平台:树莓派 4B + **Ubuntu 24.04(aarch64)**。
+> 范围:`utils/pod_bridge`(及其依赖的 `visual_track_control` 控制原语库)。
+> 核对日期:2026-09-23,对照规格《光电吊舱控制协议G版-PB.docx》;
+> 依赖轮子已按 PyPI 2026-09-23 上游信息复核。
+
+---
+
+## 1. 系统与 Python 版本(硬性)
+
+- **Ubuntu 24.04(aarch64)**:Ubuntu 的 Pi 镜像只有 64 位,天然满足 MAVSDK
+  对 aarch64 的要求(见 §3);不要自行改装任何 32 位系统。
+- **Python 3.12**(Ubuntu 24.04 自带)满足 PEP 604 标注(`float | None` /
+  `FrameParser | None`)要求的 ≥3.10,直接用,无需另装。
+- Ubuntu 24.04 系统 Python 受 PEP 668 保护,**禁止 `pip install` 进系统环境**
+  (报 externally-managed-environment),一律建 venv。注意 Ubuntu 默认
+  **不带 venv 支持**,先 apt 补齐:
+
+  ```bash
+  sudo apt update && sudo apt install -y python3-venv python3-pip
+  python3 -m venv ~/venv-pod
+  ~/venv-pod/bin/pip install -r requirements-pod.txt
+  ```
+
+## 2. 依赖裁剪(树莓派上别全装)
+
+`requirements.txt` 是全仓库清单,**吊舱桥接只需要其中两个**,务必裁剪,
+否则 torch/cv2 在 Pi 4B 上是几百 MB 且毫无意义:
+
+```text
+# requirements-pod.txt(桥接最小集)
+mavsdk
+pyserial
+pytest    # 仅上机自测需要,部署运行可省
+```
+
+- `pod_bridge` 本身纯 Python + 惰性导入:`pyserial` 只在 `PodSerial` 实例化时加载,
+  `mavsdk` 只在 `Px4Gateway.connect` / 指令发送时加载——单测全绿不需要任何硬件库。
+- **不要**在 Pi 上装 ultralytics/torch/opencv:那是 `stereo_yolo` 检测链的依赖,
+  Pi 4B 跑不动实时 YOLO,检测应留在机载算力更强的平台或吊舱内部。
+- 若确实要装 opencv,用 `opencv-python-headless`(无 GUI 依赖),
+  并 `sudo apt install libopenblas-dev libatlas-base-dev`。
+
+## 3. MAVSDK 在 ARM 上的坑
+
+- pip 的 `mavsdk` 包内嵌 `mavsdk_server` 二进制;**aarch64 有官方 wheel**
+  (PyPI 核对:mavsdk 4.0.0 提供 `cp38-abi3 manylinux2014_aarch64`,
+  abi3 向前兼容 Python 3.12,要求 glibc≥2.17,Ubuntu 24.04 为 2.39 ✅)。
+  32 位 armv7l 支持不稳定——Ubuntu 24.04 Pi 镜像只有 64 位,无此问题。
+- 安装后别只验证 import,要让内嵌二进制真跑一次:
+  `find ~/venv-pod -name mavsdk_server` 找到路径后直接执行(无参数即起服务,
+  Ctrl+C 退出),能打印启动信息说明架构匹配;`Exec format error` 则装错了架构。
+- 连接串写法(`pod_bridge.json` 的 `px4.connection_string`):
+  - Pi 串口直连飞控 TELEM:`serial:///dev/ttyUSB0:57600`(波特率按飞控侧设置);
+  - 经数传/仿真:`udp://:14540`。
+- PX4 侧需确认 TELEM 口已配 MAVLink 且 offboard 心跳未被占用。
+
+## 4. 串口硬件(吊舱侧,最容易翻车)
+
+- 协议为 **115200 8N1 无校验**(规格 §2.1)。先确认吊舱串口电平:
+  - **RS232 → 必须加电平转换或直接用 USB 转串口模块**(FTDI/CH340,推荐,
+    免折腾 GPIO UART,设备号 `/dev/ttyUSB0`);
+  - TTL 3.3V → 可用 Pi GPIO14/15(`/dev/serial0`)。
+- 用 GPIO UART 时(**Ubuntu 没有 raspi-config**,手工配置):
+  - `/boot/firmware/config.txt` 确认有 `enable_uart=1`;若控制台占着串口,
+    关掉:`sudo systemctl disable --now serial-getty@ttyAMA0.service`;
+  - Pi 4B 上 GPIO UART 默认是 mini-UART(ttyS0,波特率随主频漂移),115200 下
+    不稳——同一文件加 `dtoverlay=disable-bt` 把 PL011(ttyAMA0)换回来
+    (设备树 overlay 由固件加载,与发行版无关,Ubuntu 同样生效);
+  - **Ubuntu 没有 Raspberry Pi OS 的 `/dev/serial0` 软链**,设备名直接写
+    `/dev/ttyAMA0`(或 ttyS0);
+  - **Pi 是 3.3V 电平,绝不直连 5V/RS232 电平**,会烧 GPIO。
+- 接线三线:TX/RX **交叉**、**必须共地(GND)**,不共地是通讯乱码的第一大原因。
+- 用户权限:`sudo usermod -aG dialout $USER`,重登录生效,否则 open 串口 Permission denied。
+
+## 5. 时钟同步(规格表3 依赖)
+
+- `bridge._build_attitude` 用**宿主机 UTC 墙钟**填表3 的年月日时分秒;
+  Pi 4B **无 RTC**,断电后时间是错的,吊舱会拿到错误时间戳。
+- 上机后确认 `timedatectl` 显示 `System clock synchronized: yes`;
+  无网环境考虑给 Pi 加 RTC 模块,或由飞控 GPS 授时(需改代码,见 §8 已知问题)。
+
+## 6. 配置文件(`utils/config/pod_bridge.json`)上机必改两处
+
+| 字段 | 改成 |
+|---|---|
+| `pod_serial.port` | 实际设备号,如 `/dev/ttyUSB0`(留空会 fail-fast 报错,这是刻意的) |
+| `px4.connection_string` | 实际飞控连接,如 `serial:///dev/ttyUSB1:57600` |
+
+- 引导参数(`guidance.*`)全部是**占位值,未经标定不得用于飞行决策**;
+  首次实机只验证通讯收发,速度上限先调小。
+- 配置加载拒绝未知键——改 JSON 时注意拼写,多写的键会直接报错(防呆设计)。
+
+## 7. 上机验证顺序(每步过了再下一步)
+
+1. **纯软件自测**(无需任何接线):
+   `cd utils && ~/venv-pod/bin/python -m pytest tests/pod_bridge -q` → 应 92 全绿。
+2. **串口回环**:USB 转串口模块 TX/RX 短接,跑一个 feed/echo 小脚本验证字节收发。
+3. **只接吊舱**:观察能否解析出 表2-2(60Hz EE 16)帧;用 `send_control(归中 0x2B)`
+   验证下行。
+4. **只接飞控**:`Px4Gateway.connect` 后确认 `own_state().position_valid` 为 True。
+5. **全链路地面联试**:电机不装桨,观察 offboard 心跳与引导输出。
+6. 最后才考虑带动力测试。
+
+## 8. 已知问题处置状态(2026-09-23 更新)
+
+代码审查(2026-09-22)发现 4 项,其中 3 项安全相关缺陷**已于 2026-09-23 修复**,
+新增回归测试 8 条(pod_bridge 测试 84 → 92,全量 348 条全绿):
+
+1. ~~`Px4Gateway` 四路遥测共用 `OwnState.time_ms`~~ → **已修**:`OwnState` 拆出
+   `position_time_ms`(仅 position 流刷新),引导的位置超龄判定改用它;
+2. ~~表2-2 锁定状态(bit2)未参与引导门控~~ → **已修**:`PodBridge._pod_locked`
+   要求遥测在 `link.telemetry_timeout_ms` 内且 bit2=锁定,表4 坐标才视为有效
+   (该配置项由此从死配置变为生效配置);
+3. ~~`PodBridge.run` 主循环无异常护栏~~ → **已修**:单周期异常按「第 1 次 +
+   每满 100 次」告警并补发零速刹车保 offboard 心跳,任务不终止;
+4. 表3 时间戳用宿主机墙钟而非 GPS 时间(见 §5)——**仍未修**,属可接受风险,
+   如需精确授时按 §5 对策处理。
+
+## 9. 常驻运行(systemd 示例)
+
+```ini
+# /etc/systemd/system/pod-bridge.service
+[Unit]
+Description=Pod-PX4 Bridge
+After=network.target
+
+[Service]
+ExecStart=/home/<user>/venv-pod/bin/python -m pod_bridge_app
+WorkingDirectory=/home/<user>/code_drone-test/utils
+Restart=always
+RestartSec=3
+StandardOutput=journal
+StandardError=journal
+
+[Install]
+WantedBy=multi-user.target
+```
+
+- 热路径不打日志是设计使然(告警走 WarnThrottle),现场排查用
+  `journalctl -u pod-bridge -f`。
+- 代码热路径按 10Hz 引导 + 20Hz 姿态下发,Pi 4B 单核占用可忽略,无需实时内核。

requirements-pod.txt

@@ -0,0 +1,6 @@
+# requirements-pod.txt —— 树莓派 4B(Ubuntu 24.04)吊舱桥接最小依赖
+# 只装这几个,切勿在 Pi 上装全量 requirements.txt(torch/cv2 无意义且数百 MB)
+# 详见 docs/树莓派4B部署注意事项.md §2
+mavsdk      # PX4 遥测订阅 + offboard 指令(aarch64 官方 wheel)
+pyserial    # 吊舱串口(115200 8N1)
+pytest      # 仅上机自测(tests/pod_bridge 92 条)需要,部署运行可省

requirements.txt

@@ -0,0 +1,21 @@
+# code_drone-test Python 依赖
+# 扫描自全仓库 .py 文件的实际 import(2026-09-22)
+
+# ---- 运行时核心 ----
+numpy                   # target_tracker 卡尔曼滤波 / stereo_yolo sbs 拆分
+opencv-python           # camera.py / camera_test.py / video_test.py / demo_stereo_yolo.py (cv2)
+
+# ---- 检测推理(stereo_yolo,真实推理才加载,torch 为其依赖) ----
+ultralytics
+
+# ---- 飞控通信(MAVSDK offboard -> PX4) ----
+mavsdk                  # test.py / demo_track_control.py / pod_bridge.px4_gateway
+
+# ---- 吊舱串口(pod_bridge.pod_serial,惰性导入) ----
+pyserial
+
+# ---- 可视化(test.py 轨迹绘图) ----
+matplotlib
+
+# ---- 测试 ----
+pytest                  # utils/tests 全量单测(256 条)

utils/config/pod_bridge.json

@@ -0,0 +1,29 @@
+{
+  "pod_serial": {
+    "port": "",
+    "baudrate": 115200
+  },
+  "px4": {
+    "connection_string": "udp://:14540"
+  },
+  "link": {
+    "attitude_downlink_hz": 20.0,
+    "telemetry_timeout_ms": 500
+  },
+  "guidance": {
+    "frequency_hz": 10.0,
+    "target_stale_ms": 1000,
+    "position_stale_ms": 1000,
+    "stop_radius_m": 5.0,
+    "max_speed_mps": 5.0,
+    "speed_kp": 0.5,
+    "speed_ki": 0.0,
+    "speed_kd": 0.0,
+    "speed_integral_limit": 0.0,
+    "speed_derivative_filter_coef": 0.0,
+    "speed_slew_mps2": 2.0,
+    "max_yaw_rate_dps": 30.0,
+    "yaw_kp": 1.0,
+    "yaw_slew_dps2": 60.0
+  }
+}

utils/pod_bridge/__init__.py

@@ -0,0 +1,108 @@
+"""pod_bridge —— 光电吊舱 ↔ PX4 飞控 协议桥接（规格：docs/光电吊舱控制协议G版-PB.docx）。
+
+吊舱自带 CPU 完成目标解算；本包只做双向搬运 + 格式转换：
+  PX4 遥测(姿态/经纬度/时间) → 表3(EB 91) 串口下发吊舱
+  吊舱 表4(EE 18) 目标经纬度 + 表2-2(EE 16) 遥测 → offboard 速度设定点驱动 PX4 朝目标飞行
+
+当前含纯协议编解码层 protocol（四帧 encode/decode + FrameParser）、地理位置
+引导律 guidance（载机位置 + 目标经纬度 → offboard 速度/航向）、配置 config
+（BridgeConfig + load_from_json）、飞控侧网关 px4_gateway（读 PX4 遥测 +
+复用 MavsdkCommandGateway 发 offboard 指令）、吊舱侧串口 pod_serial（PodSerial
++ PodLink 接口）、闭环主循环 bridge（PodBridge）；均可 mock 单测，无硬件依赖。
+"""
+
+from .bridge import PodBridge
+from .config import (
+    BridgeConfig,
+    LinkConfig,
+    PodSerialConfig,
+    Px4Config,
+    load_from_json,
+)
+from .guidance import (
+    GuidanceConfig,
+    GuidanceInput,
+    GuidanceLaw,
+    haversine_distance_m,
+    initial_bearing_deg,
+)
+from .pod_serial import PodLink, PodSerial
+from .protocol import (
+    ALT_SCALE,
+    ANGLE_SCALE,
+    ATTITUDE_LEN,
+    CONTROL_LEN,
+    CarrierAttitude,
+    ControlCode,
+    ControlCommand,
+    FrameParser,
+    LATLON_SCALE,
+    RANGE_SCALE,
+    RATE_SCALE,
+    SPEED_SCALE,
+    SYNC_ATTITUDE,
+    SYNC_CONTROL,
+    SYNC_TARGET,
+    SYNC_TELEMETRY,
+    TARGET_LEN,
+    TELEMETRY_LEN,
+    Telemetry,
+    TargetGeo,
+    checksum,
+    decode_attitude,
+    decode_control,
+    decode_target,
+    decode_telemetry,
+    encode_attitude,
+    encode_control,
+    encode_target,
+    encode_telemetry,
+)
+from .px4_gateway import OwnState, Px4Gateway
+
+__all__ = [
+    "ALT_SCALE",
+    "ANGLE_SCALE",
+    "ATTITUDE_LEN",
+    "BridgeConfig",
+    "CONTROL_LEN",
+    "CarrierAttitude",
+    "ControlCode",
+    "ControlCommand",
+    "FrameParser",
+    "GuidanceConfig",
+    "GuidanceInput",
+    "GuidanceLaw",
+    "LATLON_SCALE",
+    "LinkConfig",
+    "OwnState",
+    "PodBridge",
+    "PodLink",
+    "PodSerial",
+    "PodSerialConfig",
+    "Px4Config",
+    "Px4Gateway",
+    "RANGE_SCALE",
+    "RATE_SCALE",
+    "SPEED_SCALE",
+    "SYNC_ATTITUDE",
+    "SYNC_CONTROL",
+    "SYNC_TARGET",
+    "SYNC_TELEMETRY",
+    "TARGET_LEN",
+    "TELEMETRY_LEN",
+    "Telemetry",
+    "TargetGeo",
+    "checksum",
+    "decode_attitude",
+    "decode_control",
+    "decode_target",
+    "decode_telemetry",
+    "encode_attitude",
+    "encode_control",
+    "encode_target",
+    "encode_telemetry",
+    "haversine_distance_m",
+    "initial_bearing_deg",
+    "load_from_json",
+]

utils/pod_bridge/bridge.py

@@ -0,0 +1,207 @@
+"""bridge.py —— 吊舱 ↔ PX4 桥接主循环（asyncio 胶水层）。
+
+闭环（每控制周期，guidance.frequency_hz）：
+  ① poll 吊舱串口 → 缓存最新 表4 目标经纬度 / 表2-2 遥测（带单调时间戳）
+  ② 按 link.attitude_downlink_hz 节流，读 PX4 遥测 → 打包 表3 载机姿态下发吊舱
+     （吊舱据此解算目标经纬度，文档要求 1~60Hz）
+  ③ 目标门控：表2-2 状态字节1 bit2=锁定 且遥测在 link.telemetry_timeout_ms 内，
+     表4 坐标才视为有效（规格：表4 周期发送，搜索状态下坐标无意义；激光无效时
+     吊舱不解算、表4 自然中断，由 guidance.target_stale_ms 兜底）
+  ④ 自身状态 + 最新目标 → GuidanceInput → GuidanceLaw.update → ControlOutput
+  ⑤ 按模式分发：VELOCITY_HEADING → offboard 速度；BRAKE_HOVER → 零速刹车
+     （引导律对一切不安全情形输出 BRAKE_HOVER，故 offboard 始终有心跳，无失联悬停）
+
+run() 主循环带异常护栏：单周期异常（如串口拔线）按节流节奏告警并补发零速刹车
+保 offboard 心跳，任务不终止。
+
+时钟一致性：Px4Gateway 与 PodBridge 必须共用同一单调毫秒时钟（OwnState.time_ms
+与 step(now_ms) 同源），否则 position/target 超龄判定失效。connect() 工厂已统一。
+"""
+
+import asyncio
+import logging
+import math
+import time
+from datetime import datetime, timezone
+from typing import Optional
+
+from visual_track_control.staleness_checker import is_stale
+from visual_track_control.types import ControlMode
+
+from .config import BridgeConfig
+from .guidance import GuidanceInput, GuidanceLaw
+from .pod_serial import PodLink, PodSerial
+from .protocol import (CarrierAttitude, ControlCommand, TargetGeo, Telemetry,
+                       encode_attitude, encode_control)
+from .px4_gateway import Px4Gateway
+
+logger = logging.getLogger(__name__)
+
+
+def _monotonic_ms() -> int:
+    return int(time.monotonic() * 1000)
+
+
+class PodBridge:
+    """px4：Px4Gateway（或等价 mock）；pod：PodLink（PodSerial 或内存 FakePod）。"""
+
+    def __init__(self, cfg: BridgeConfig, px4: Px4Gateway, pod: PodLink,
+                 now_ms_fn=_monotonic_ms) -> None:
+        self._cfg = cfg
+        self._px4 = px4
+        self._pod = pod
+        self._now_ms = now_ms_fn
+        self._guidance = GuidanceLaw(cfg.guidance)
+        self._attitude_period_ms = max(
+            1, int(round(1000.0 / cfg.link.attitude_downlink_hz)))
+        self._last_attitude_ms: Optional[int] = None
+        self._latest_target = None      # (TargetGeo, time_ms)
+        self._latest_telemetry = None   # (Telemetry, time_ms)
+        self._step_failures = 0         # run() 连续异常计数（告警节流用）
+
+    # --- 生命周期 -----------------------------------------------------------
+    async def start(self) -> bool:
+        """启动 PX4 遥测订阅与 offboard；offboard 失败返回 False（不应继续闭环）。"""
+        await self._px4.start_telemetry()
+        if not await self._px4.start_offboard():
+            logger.error("[Bridge] offboard 启动失败，桥接未运行")
+            return False
+        logger.info("[Bridge] 启动：引导 %.1fHz，姿态下发 %.1fHz",
+                    self._cfg.guidance.frequency_hz,
+                    self._cfg.link.attitude_downlink_hz)
+        return True
+
+    async def stop(self) -> None:
+        """安全停机：先零速刹车，再停 offboard / 遥测 / 串口。"""
+        try:
+            await self._px4.send_brake_hover(self._now_ms())
+        finally:
+            await self._px4.stop_offboard()
+            await self._px4.stop_telemetry()
+            self._pod.close()
+            self._guidance.reset()
+            logger.info("[Bridge] 已停止")
+
+    # --- 主循环 -------------------------------------------------------------
+    async def run(self, stop_event: asyncio.Event) -> None:
+        """按引导频率循环 step()，直到 stop_event 置位。
+
+        异常护栏：单周期抛错（典型：吊舱串口拔线 SerialException/OSError）不终止
+        任务，按「第 1 次 + 每满 100 次」节奏告警（对齐 WarnThrottle 语义），并补发
+        零速刹车保住 offboard 心跳——否则 PX4 数秒内触发 offboard 失联保护。
+        CancelledError 继承 BaseException，不在此拦截。
+        """
+        period_s = 1.0 / self._cfg.guidance.frequency_hz
+        while not stop_event.is_set():
+            try:
+                await self.step(self._now_ms())
+                self._step_failures = 0
+            except Exception as exc:
+                self._step_failures += 1
+                if self._step_failures == 1 or self._step_failures % 100 == 0:
+                    logger.warning("[Bridge] 控制周期异常(第 %d 次): %s",
+                                   self._step_failures, exc)
+                try:
+                    await self._px4.send_brake_hover(self._now_ms())
+                except Exception:
+                    pass  # 补发失败不掩盖原异常，下周期重试
+            await asyncio.sleep(period_s)
+
+    async def step(self, now_ms: int):
+        """单控制周期（显式传入时钟，便于确定性单测）。返回 ControlOutput。"""
+        self._ingest_pod_frames(now_ms)
+
+        own = self._px4.own_state()
+        self._downlink_attitude(own, now_ms)
+
+        out = self._guidance.update(self._build_guidance_input(own, now_ms), now_ms)
+        await self._dispatch(out, now_ms)
+        return out
+
+    # --- 吊舱侧 -------------------------------------------------------------
+    def _ingest_pod_frames(self, now_ms: int) -> None:
+        for frame in self._pod.poll():
+            if isinstance(frame, TargetGeo):
+                self._latest_target = (frame, now_ms)
+            elif isinstance(frame, Telemetry):
+                self._latest_telemetry = (frame, now_ms)
+
+    def _downlink_attitude(self, own, now_ms: int) -> None:
+        if (self._last_attitude_ms is not None
+                and now_ms - self._last_attitude_ms < self._attitude_period_ms):
+            return
+        self._pod.send(encode_attitude(self._build_attitude(own)))
+        self._last_attitude_ms = now_ms
+
+    def _build_attitude(self, own) -> CarrierAttitude:
+        t = datetime.now(timezone.utc)
+        return CarrierAttitude(
+            roll_deg=math.degrees(own.roll_rad),
+            pitch_deg=math.degrees(own.pitch_rad),
+            yaw_deg=math.degrees(own.yaw_rad),
+            latitude_deg=own.latitude_deg,
+            longitude_deg=own.longitude_deg,
+            abs_alt_m=own.absolute_altitude_m,
+            rel_alt_m=own.relative_altitude_m,
+            year=t.year, month=t.month, day=t.day,
+            hour=t.hour, minute=t.minute, second=t.second,
+            centisecond=t.microsecond // 10000,
+            airspeed_mps=0.0,                      # 无空速管，置 0（文档允许）
+            ground_speed_mps=own.ground_speed_mps,
+        )
+
+    def send_control(self, cmd: ControlCommand) -> None:
+        """触发式下发 表2-1 控制指令（如开目标识别 / 框选跟踪）。"""
+        self._pod.send(encode_control(cmd))
+
+    # --- 飞控侧 -------------------------------------------------------------
+    def _build_guidance_input(self, own, now_ms: int) -> GuidanceInput:
+        tgt = self._latest_target
+        locked = self._pod_locked(now_ms)
+        return GuidanceInput(
+            own_lat_deg=own.latitude_deg,
+            own_lon_deg=own.longitude_deg,
+            own_yaw_deg=math.degrees(own.yaw_rad),
+            own_position_valid=own.position_valid,
+            own_position_time_ms=own.position_time_ms,  # position 流专用时间戳
+            target_lat_deg=tgt[0].latitude_deg if tgt else 0.0,
+            target_lon_deg=tgt[0].longitude_deg if tgt else 0.0,
+            target_valid=tgt is not None and locked,     # 未锁定=目标无效
+            target_time_ms=tgt[1] if tgt else 0,
+        )
+
+    def _pod_locked(self, now_ms: int) -> bool:
+        """吊舱锁定门控：表2-2 遥测在 link.telemetry_timeout_ms 内且状态字节1
+        bit2=锁定 才算锁定。从未收到遥测 / 遥测超龄 / 搜索状态一律视为未锁定
+        （宁可刹车，不朝搜索状态下的无意义坐标飞）。"""
+        tel = self._latest_telemetry
+        if tel is None:
+            return False
+        frame, t_ms = tel
+        if is_stale(t_ms, now_ms, self._cfg.link.telemetry_timeout_ms):
+            return False
+        return frame.is_target_locked
+
+    async def _dispatch(self, out, now_ms: int) -> None:
+        if out.mode is ControlMode.VELOCITY_HEADING and out.valid:
+            await self._px4.send_velocity_heading(out, now_ms)
+        elif out.mode is ControlMode.BRAKE_HOVER and out.valid:
+            await self._px4.send_brake_hover(now_ms)
+
+    # --- 诊断访问 -----------------------------------------------------------
+    @property
+    def latest_target(self):
+        return self._latest_target
+
+    @property
+    def latest_telemetry(self):
+        return self._latest_telemetry
+
+    # --- 实机工厂 -----------------------------------------------------------
+    @classmethod
+    async def connect(cls, cfg: BridgeConfig) -> "PodBridge":
+        """按配置连接 PX4(MAVSDK) 与吊舱(串口)，共用单一单调时钟。"""
+        px4 = await Px4Gateway.connect(cfg.px4.connection_string,
+                                       now_ms_fn=_monotonic_ms)
+        pod = PodSerial(cfg.pod_serial.port, cfg.pod_serial.baudrate)
+        return cls(cfg, px4, pod, now_ms_fn=_monotonic_ms)

utils/pod_bridge/config.py

@@ -0,0 +1,234 @@
+"""config.py —— 吊舱桥接配置（BridgeConfig + load_from_json）。
+
+集中管理：吊舱串口、PX4(MAVSDK) 连接串、链路频率/超时、引导律参数
+（复用 guidance.GuidanceConfig）。风格对齐 visual_track_control/config.py：
+启动期一次性解析，错误携带完整字段名（如 "guidance.max_speed_mps"）抛 ValueError，
+解析结果与 JSON 脱钩，热路径不热更新。
+
+与 visual_track_control/config.py 的差异：本配置多处取值仍待定（串口设备号、
+PX4 连接串等），故采用「缺字段回退 dataclass 默认值」而非全字段强制；为保证
+安全，仍对出现的字段做类型/范围校验，并**拒绝各分组内的未知键**（防拼写错误
+导致的静默回退）。
+"""
+
+import dataclasses
+import json
+from dataclasses import dataclass, field
+from typing import Any
+
+from .guidance import GuidanceConfig
+
+
+@dataclass
+class PodSerialConfig:
+    """吊舱侧串口（文档：115200、无校验、8N1）。port 待定，留空为占位。"""
+
+    port: str = ""
+    baudrate: int = 115200
+
+
+@dataclass
+class Px4Config:
+    """飞控侧 MAVSDK 连接串。TELEM 串口形如 serial:///dev/ttyS1:57600；
+    SITL/仿真可用 udp://:14540。"""
+
+    connection_string: str = "udp://:14540"
+
+
+@dataclass
+class LinkConfig:
+    """链路频率与超时。"""
+
+    attitude_downlink_hz: float = 20.0   # 表3 载机姿态下发频率（文档：1~60Hz）
+    telemetry_timeout_ms: int = 500       # 吊舱遥测/目标超龄判定（链路级）
+
+
+@dataclass
+class BridgeConfig:
+    pod_serial: PodSerialConfig = field(default_factory=PodSerialConfig)
+    px4: Px4Config = field(default_factory=Px4Config)
+    link: LinkConfig = field(default_factory=LinkConfig)
+    guidance: GuidanceConfig = field(default_factory=GuidanceConfig)
+
+
+# ---------------------------------------------------------------------------
+# 校验原语（错误消息携带完整字段名）
+# ---------------------------------------------------------------------------
+def _is_number(value: Any) -> bool:
+    return isinstance(value, (int, float)) and not isinstance(value, bool)
+
+
+def _is_integer(value: Any) -> bool:
+    return isinstance(value, int) and not isinstance(value, bool)
+
+
+def _require_section(root: dict, section: str) -> dict:
+    value = root.get(section)
+    if value is None:
+        return {}  # 缺省分组：全部回退默认值
+    if not isinstance(value, dict):
+        raise ValueError(f"配置缺失分组或类型错误: {section}")
+    return value
+
+
+def _reject_unknown(section: dict, allowed: set, section_name: str) -> None:
+    unknown = set(section.keys()) - allowed
+    if unknown:
+        names = ", ".join(f"{section_name}.{k}" for k in sorted(unknown))
+        raise ValueError(f"配置存在未知字段: {names}")
+
+
+def _get_number(section: dict, section_name: str, key: str, default: float) -> float:
+    if key not in section:
+        return float(default)
+    value = section[key]
+    if not _is_number(value):
+        raise ValueError(f"配置字段类型错误(需数值): {section_name}.{key}")
+    return float(value)
+
+
+def _get_integer(section: dict, section_name: str, key: str, default: int) -> int:
+    if key not in section:
+        return int(default)
+    value = section[key]
+    if not _is_integer(value):
+        raise ValueError(f"配置字段类型错误(需整数): {section_name}.{key}")
+    return int(value)
+
+
+def _get_string(section: dict, section_name: str, key: str, default: str) -> str:
+    if key not in section:
+        return str(default)
+    value = section[key]
+    if not isinstance(value, str):
+        raise ValueError(f"配置字段类型错误(需字符串): {section_name}.{key}")
+    return value
+
+
+def _positive(value: float, field_name: str) -> float:
+    if not (value > 0.0):
+        raise ValueError(f"配置非法(必须为正): {field_name}")
+    return value
+
+
+def _non_negative(value: float, field_name: str) -> float:
+    if value < 0.0:
+        raise ValueError(f"配置非法(不得为负): {field_name}")
+    return value
+
+
+def _in_range(value: float, lo: float, hi: float, field_name: str,
+              half_open: bool = False) -> float:
+    """范围校验：默认闭区间 [lo, hi]；half_open=True 为左闭右开 [lo, hi)。"""
+    ok = (lo <= value < hi) if half_open else (lo <= value <= hi)
+    if not ok:
+        raise ValueError(f"配置非法(超出范围): {field_name}")
+    return value
+
+
+# ---------------------------------------------------------------------------
+# 各分组加载
+# ---------------------------------------------------------------------------
+def _load_pod_serial(root: dict) -> PodSerialConfig:
+    s = _require_section(root, "pod_serial")
+    _reject_unknown(s, {"port", "baudrate"}, "pod_serial")
+    c = PodSerialConfig()
+    c.port = _get_string(s, "pod_serial", "port", c.port)
+    c.baudrate = int(_positive(
+        _get_integer(s, "pod_serial", "baudrate", c.baudrate), "pod_serial.baudrate"))
+    return c
+
+
+def _load_px4(root: dict) -> Px4Config:
+    s = _require_section(root, "px4")
+    _reject_unknown(s, {"connection_string"}, "px4")
+    c = Px4Config()
+    c.connection_string = _get_string(
+        s, "px4", "connection_string", c.connection_string)
+    if not c.connection_string:
+        raise ValueError("配置非法(不得为空): px4.connection_string")
+    return c
+
+
+def _load_link(root: dict) -> LinkConfig:
+    s = _require_section(root, "link")
+    _reject_unknown(s, {"attitude_downlink_hz", "telemetry_timeout_ms"}, "link")
+    c = LinkConfig()
+    c.attitude_downlink_hz = _in_range(
+        _positive(_get_number(s, "link", "attitude_downlink_hz",
+                              c.attitude_downlink_hz), "link.attitude_downlink_hz"),
+        1.0, 60.0, "link.attitude_downlink_hz")
+    c.telemetry_timeout_ms = int(_positive(
+        _get_integer(s, "link", "telemetry_timeout_ms", c.telemetry_timeout_ms),
+        "link.telemetry_timeout_ms"))
+    return c
+
+
+def _load_guidance(root: dict) -> GuidanceConfig:
+    s = _require_section(root, "guidance")
+    allowed = {f.name for f in dataclasses.fields(GuidanceConfig)}
+    _reject_unknown(s, allowed, "guidance")
+    d = GuidanceConfig()
+    g = GuidanceConfig()
+
+    g.frequency_hz = _positive(
+        _get_number(s, "guidance", "frequency_hz", d.frequency_hz),
+        "guidance.frequency_hz")
+    g.target_stale_ms = int(_positive(
+        _get_integer(s, "guidance", "target_stale_ms", d.target_stale_ms),
+        "guidance.target_stale_ms"))
+    g.position_stale_ms = int(_positive(
+        _get_integer(s, "guidance", "position_stale_ms", d.position_stale_ms),
+        "guidance.position_stale_ms"))
+    g.stop_radius_m = _non_negative(
+        _get_number(s, "guidance", "stop_radius_m", d.stop_radius_m),
+        "guidance.stop_radius_m")
+    g.max_speed_mps = _positive(
+        _get_number(s, "guidance", "max_speed_mps", d.max_speed_mps),
+        "guidance.max_speed_mps")
+    g.speed_kp = _non_negative(
+        _get_number(s, "guidance", "speed_kp", d.speed_kp), "guidance.speed_kp")
+    g.speed_ki = _non_negative(
+        _get_number(s, "guidance", "speed_ki", d.speed_ki), "guidance.speed_ki")
+    g.speed_kd = _non_negative(
+        _get_number(s, "guidance", "speed_kd", d.speed_kd), "guidance.speed_kd")
+    g.speed_integral_limit = _non_negative(
+        _get_number(s, "guidance", "speed_integral_limit", d.speed_integral_limit),
+        "guidance.speed_integral_limit")
+    g.speed_derivative_filter_coef = _in_range(
+        _get_number(s, "guidance", "speed_derivative_filter_coef",
+                    d.speed_derivative_filter_coef),
+        0.0, 1.0, "guidance.speed_derivative_filter_coef", half_open=True)
+    g.speed_slew_mps2 = _positive(
+        _get_number(s, "guidance", "speed_slew_mps2", d.speed_slew_mps2),
+        "guidance.speed_slew_mps2")
+    g.max_yaw_rate_dps = _positive(
+        _get_number(s, "guidance", "max_yaw_rate_dps", d.max_yaw_rate_dps),
+        "guidance.max_yaw_rate_dps")
+    g.yaw_kp = _non_negative(
+        _get_number(s, "guidance", "yaw_kp", d.yaw_kp), "guidance.yaw_kp")
+    g.yaw_slew_dps2 = _positive(
+        _get_number(s, "guidance", "yaw_slew_dps2", d.yaw_slew_dps2),
+        "guidance.yaw_slew_dps2")
+    return g
+
+
+def load_from_json(path: str) -> BridgeConfig:
+    """从 JSON 文件一次性加载桥接配置（启动期调用，调参重启生效）。"""
+    try:
+        with open(path, "r", encoding="utf-8") as fp:
+            root = json.load(fp)
+    except OSError:
+        raise ValueError(f"配置文件打开失败: {path}") from None
+    except json.JSONDecodeError as exc:
+        raise ValueError(f"配置文件解析失败: {path} ({exc})") from None
+
+    if not isinstance(root, dict):
+        raise ValueError(f"配置文件解析失败: {path} (根节点必须为对象)")
+
+    cfg = BridgeConfig()
+    cfg.pod_serial = _load_pod_serial(root)
+    cfg.px4 = _load_px4(root)
+    cfg.link = _load_link(root)
+    cfg.guidance = _load_guidance(root)
+    return cfg

utils/pod_bridge/guidance.py

@@ -0,0 +1,204 @@
+"""guidance.py —— 地理位置引导律：载机位置 + 目标经纬度 → offboard 速度/航向。
+
+吊舱（表4）给出目标经纬度，本律计算到目标的方位角/距离，生成"朝目标飞"的
+控制意图，输出复用 visual_track_control.types.ControlOutput，可直接喂现有
+MavsdkCommandGateway（速率式分支：vx_body_mps 前向 + yaw_rate_dps 偏航）。
+
+与 visual_track_control.TrackingControlLaw 的区别：后者由像素视线角驱动（视觉
+伺服），本律由绝对地理坐标驱动（航位推算）。复用其纯算法构件（PID / 限幅 /
+超龄判定），故 pod_bridge 依赖 visual_track_control 作为控制原语库。
+
+控制策略（速率式，避开"绝对航向为正北 0° 被网关误判为无航向指令"的歧义）：
+  * 方位误差 err = wrap180(目标方位 − 当前航向) → 偏航角速度（比例 + 变化率限幅）
+  * 距离误差 = 地面距离 − 停止半径 → 前向基准速度（PID + 限幅）
+  * 对准门控：前向 = 基准速度 · max(0, cos(err))，未对准时先原地转向再前进
+  * 垂直：高度保持（vz = 0）；高度逼近策略待后续 config 化
+  * 到达停止半径 / 目标或载机位置不可用 / 超龄 → 零速刹车（BRAKE_HOVER）
+"""
+
+import logging
+import math
+from dataclasses import dataclass
+
+from visual_track_control.pid_controller import PidController, PidParams
+from visual_track_control.rate_limiter import SlewRateLimiter, clamp
+from visual_track_control.staleness_checker import is_stale
+from visual_track_control.types import ControlMode, ControlOutput
+
+logger = logging.getLogger(__name__)
+
+_K_DEG_TO_RAD = math.pi / 180.0
+_K_RAD_TO_DEG = 180.0 / math.pi
+_K_EARTH_RADIUS_M = 6371008.8  # WGS84 平均半径
+_K_MAX_DT_S = 0.5              # 单步 dt 上限（秒）：异常节拍保护
+
+
+# --- 大地测量（纯函数）-----------------------------------------------------
+def haversine_distance_m(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
+    """两点地面距离（米，haversine 大圆距离）。"""
+    phi1 = lat1 * _K_DEG_TO_RAD
+    phi2 = lat2 * _K_DEG_TO_RAD
+    dphi = (lat2 - lat1) * _K_DEG_TO_RAD
+    dlmb = (lon2 - lon1) * _K_DEG_TO_RAD
+    a = (math.sin(dphi / 2.0) ** 2
+         + math.cos(phi1) * math.cos(phi2) * math.sin(dlmb / 2.0) ** 2)
+    return 2.0 * _K_EARTH_RADIUS_M * math.asin(min(1.0, math.sqrt(a)))
+
+
+def initial_bearing_deg(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
+    """从点 1 指向点 2 的初始方位角（度，正北为 0，顺时针 [0, 360)）。"""
+    phi1 = lat1 * _K_DEG_TO_RAD
+    phi2 = lat2 * _K_DEG_TO_RAD
+    dlmb = (lon2 - lon1) * _K_DEG_TO_RAD
+    x = math.sin(dlmb) * math.cos(phi2)
+    y = (math.cos(phi1) * math.sin(phi2)
+         - math.sin(phi1) * math.cos(phi2) * math.cos(dlmb))
+    return (math.atan2(x, y) * _K_RAD_TO_DEG) % 360.0
+
+
+def _wrap_deg_180(deg: float) -> float:
+    """角度回绕到 [-180, 180)。"""
+    d = math.fmod(deg + 180.0, 360.0)
+    if d < 0.0:
+        d += 360.0
+    return d - 180.0
+
+
+# --- 配置 / 输入 ------------------------------------------------------------
+@dataclass
+class GuidanceConfig:
+    """引导律参数（全部可调，后续 config.py 从 JSON 加载并校验）。"""
+
+    frequency_hz: float = 10.0
+    target_stale_ms: int = 1000      # 目标经纬度超龄门限
+    position_stale_ms: int = 1000    # 载机位置超龄门限
+    stop_radius_m: float = 5.0       # 到达此半径内悬停（不再前进）
+    max_speed_mps: float = 5.0       # 前向速度上限
+    speed_kp: float = 0.5            # 距离比例增益（mps / m）
+    speed_ki: float = 0.0
+    speed_kd: float = 0.0
+    speed_integral_limit: float = 0.0
+    speed_derivative_filter_coef: float = 0.0
+    speed_slew_mps2: float = 2.0     # 前向加速度限幅
+    max_yaw_rate_dps: float = 30.0   # 偏航角速度上限
+    yaw_kp: float = 1.0              # 方位误差比例增益（dps / deg）
+    yaw_slew_dps2: float = 60.0      # 偏航角速度变化率限幅
+
+
+@dataclass
+class GuidanceInput:
+    """单周期引导输入：载机状态（PX4 遥测）+ 目标（吊舱表4），各带取样时间戳。"""
+
+    own_lat_deg: float = 0.0
+    own_lon_deg: float = 0.0
+    own_yaw_deg: float = 0.0          # 当前航向（NED，度，正北 0 顺时针）
+    own_position_valid: bool = False  # GPS 定位/位置是否可用
+    own_position_time_ms: int = 0
+
+    target_lat_deg: float = 0.0
+    target_lon_deg: float = 0.0
+    target_valid: bool = False        # 表4 目标经纬度是否有效
+    target_time_ms: int = 0
+
+
+# --- 引导律 -----------------------------------------------------------------
+class GuidanceLaw:
+    """地理位置引导主控律。update() 单周期返回 ControlOutput。
+
+    诊断量（last_distance_m / last_bearing_deg / last_heading_err_deg）每周期
+    更新，供桥接层日志使用；刹车/降级时为最近一次有效计算值或 NaN。
+    """
+
+    def __init__(self, cfg: GuidanceConfig) -> None:
+        self._cfg = cfg
+        self._speed_pid = PidController(PidParams(
+            kp=cfg.speed_kp, ki=cfg.speed_ki, kd=cfg.speed_kd,
+            integral_limit=cfg.speed_integral_limit,
+            derivative_filter_coef=cfg.speed_derivative_filter_coef,
+            output_limit=cfg.max_speed_mps,
+        ))
+        self._speed_limiter = SlewRateLimiter(cfg.speed_slew_mps2)
+        self._yaw_limiter = SlewRateLimiter(cfg.yaw_slew_dps2)
+        self._last_now_ms = 0
+        self._last_mode = ControlMode.BRAKE_HOVER
+        self.last_distance_m = float("nan")
+        self.last_bearing_deg = float("nan")
+        self.last_heading_err_deg = float("nan")
+
+    def update(self, inp: GuidanceInput, now_ms: int) -> ControlOutput:
+        dt_s = self._advance_dt(now_ms)
+        out = ControlOutput()
+
+        # ---- 可用性 / 超龄判定 ----
+        position_ok = (inp.own_position_valid
+                       and not is_stale(inp.own_position_time_ms, now_ms,
+                                        self._cfg.position_stale_ms))
+        target_ok = (inp.target_valid
+                     and not is_stale(inp.target_time_ms, now_ms,
+                                      self._cfg.target_stale_ms))
+        if not position_ok or not target_ok:
+            return self._brake(out, "载机位置或目标不可用/超龄，零速刹车")
+
+        distance = haversine_distance_m(inp.own_lat_deg, inp.own_lon_deg,
+                                        inp.target_lat_deg, inp.target_lon_deg)
+        bearing = initial_bearing_deg(inp.own_lat_deg, inp.own_lon_deg,
+                                      inp.target_lat_deg, inp.target_lon_deg)
+        heading_err = _wrap_deg_180(bearing - inp.own_yaw_deg)
+        self.last_distance_m = distance
+        self.last_bearing_deg = bearing
+        self.last_heading_err_deg = heading_err
+
+        # ---- 到达停止半径 → 悬停 ----
+        if distance <= self._cfg.stop_radius_m:
+            return self._brake(out, "已到达停止半径，悬停")
+
+        # ---- 偏航角速度（比例 + 变化率限幅 + 上限）----
+        target_rate = clamp(self._cfg.yaw_kp * heading_err,
+                            -self._cfg.max_yaw_rate_dps, self._cfg.max_yaw_rate_dps)
+        out.yaw_rate_dps = self._yaw_limiter.limit(target_rate, dt_s)
+
+        # ---- 前向速度（距离 PID + 对准门控 + 加速度限幅 + 上限）----
+        base_speed = clamp(self._speed_pid.update(distance - self._cfg.stop_radius_m,
+                                                  dt_s),
+                           0.0, self._cfg.max_speed_mps)
+        alignment = max(0.0, math.cos(heading_err * _K_DEG_TO_RAD))
+        out.vx_body_mps = self._speed_limiter.limit(base_speed * alignment, dt_s)
+
+        out.vz_mps = 0.0  # 高度保持
+        out.mode = ControlMode.VELOCITY_HEADING
+        out.valid = True
+        return self._finish_and_log(out, "朝目标引导")
+
+    def reset(self) -> None:
+        """停止闭环时清全部内部状态。"""
+        self._speed_pid.reset()
+        self._speed_limiter.reset()
+        self._yaw_limiter.reset()
+        self._last_now_ms = 0
+        self._last_mode = ControlMode.BRAKE_HOVER
+
+    def _advance_dt(self, now_ms: int) -> float:
+        """节拍推算：异常节拍/时钟回拨用配置频率兜底，单步上限 0.5s。"""
+        dt_s = 1.0 / self._cfg.frequency_hz
+        if self._last_now_ms > 0 and now_ms > self._last_now_ms:
+            dt_s = min((now_ms - self._last_now_ms) / 1000.0, _K_MAX_DT_S)
+        self._last_now_ms = now_ms
+        return dt_s
+
+    def _brake(self, out: ControlOutput, reason: str) -> ControlOutput:
+        """零速刹车：进入时清积分与限幅器历史，恢复引导后指令从零渐变。"""
+        if self._last_mode is not ControlMode.BRAKE_HOVER:
+            self._speed_pid.reset()
+            self._speed_limiter.reset()
+            self._yaw_limiter.reset()
+        out.mode = ControlMode.BRAKE_HOVER
+        out.valid = True  # 速度分量保持默认全零
+        return self._finish_and_log(out, reason)
+
+    def _finish_and_log(self, out: ControlOutput, reason: str) -> ControlOutput:
+        """仅模式变化时打印状态切换日志（热路径零日志）。"""
+        if out.mode is not self._last_mode:
+            logger.info("[Guidance] 状态切换: %s -> %s (%s)",
+                        self._last_mode.name, out.mode.name, reason)
+            self._last_mode = out.mode
+        return out

utils/pod_bridge/pod_serial.py

@@ -0,0 +1,68 @@
+"""pod_serial.py —— 吊舱串口链路（pyserial 8N1 + FrameParser 分帧）。
+
+文档：波特率 115200、无校验、8N1。本模块只做字节搬运与分帧，不理解帧语义：
+  * poll() —— 非阻塞读取已到达字节（timeout=0 + in_waiting），喂 FrameParser，
+    返回本次解析出的帧列表（粘包/半帧由 FrameParser 处理）；
+  * send() —— 写入一帧已编码字节；
+  * close() —— 关闭串口。
+
+非阻塞轮询设计便于与 asyncio 桥接主循环单线程协作（无需读线程）。PodLink 为
+duck-typed 接口，桥接层据此工作，测试可注入内存 FakePod，无需硬件。
+"""
+
+import logging
+from typing import List, Protocol, runtime_checkable
+
+from .protocol import FrameParser
+
+logger = logging.getLogger(__name__)
+
+
+@runtime_checkable
+class PodLink(Protocol):
+    """吊舱链路接口（PodSerial 实机实现 / 测试 FakePod 均满足）。"""
+
+    def poll(self) -> List[object]:
+        """返回本次解析出的帧列表（Telemetry / TargetGeo 等）。"""
+        ...
+
+    def send(self, data: bytes) -> None:
+        """写入一帧已编码字节。"""
+        ...
+
+    def close(self) -> None:
+        ...
+
+
+class PodSerial:
+    """实机串口实现。pyserial 惰性导入：未装 pyserial 也能 import 本模块并用 mock。"""
+
+    def __init__(self, port: str, baudrate: int = 115200,
+                 parser: FrameParser | None = None) -> None:
+        if not port:
+            raise ValueError("吊舱串口未配置: pod_serial.port 为空")
+        import serial  # 惰性导入
+        self._ser = serial.Serial(
+            port=port, baudrate=baudrate,
+            bytesize=serial.EIGHTBITS, parity=serial.PARITY_NONE,
+            stopbits=serial.STOPBITS_ONE,
+            timeout=0, write_timeout=1.0,
+        )
+        self._parser = parser or FrameParser()
+        logger.info("[PodSerial] 打开 %s @%d 8N1", port, baudrate)
+
+    def poll(self) -> List[object]:
+        waiting = self._ser.in_waiting
+        if waiting <= 0:
+            return []
+        return self._parser.feed(self._ser.read(waiting))
+
+    def send(self, data: bytes) -> None:
+        self._ser.write(data)
+
+    def close(self) -> None:
+        try:
+            self._ser.close()
+            logger.info("[PodSerial] 串口已关闭")
+        except Exception as exc:
+            logger.warning("[PodSerial] 关闭异常: %s", exc)

utils/pod_bridge/protocol.py

@@ -0,0 +1,419 @@
+"""protocol.py —— 光电吊舱控制协议 G 版编解码（纯函数 + 数据类，无硬件依赖）。
+
+规格：docs/光电吊舱控制协议G版-PB.docx。
+
+四种定长帧，统一规则：
+  * 多字节小端（低字节在前，高字节在后）；
+  * 末尾校验和 = 从第 0 字节累加取低 8 位（sum(body) & 0xFF）；
+  * 文档样例 eb 90 2b 00..00 a6 已锁定校验算法。
+
+  表2-1 控制指令   同步字 EB 90  16B  控制器 → 吊舱  触发式
+  表2-2 周期遥测   同步字 EE 16  32B  吊舱 → 控制器  60Hz
+  表3   载机姿态   同步字 EB 91  32B  控制器 → 吊舱  1~60Hz
+  表4   目标经纬度 同步字 EE 18  32B  吊舱 → 控制器  1~60Hz
+
+控制器（本桥接）发送 表2-1/表3、接收 表2-2/表4；为便于回环测试与后续吊舱
+模拟器，四种帧均提供对称的 encode/decode。
+"""
+
+from dataclasses import dataclass, field
+from enum import IntEnum
+import struct
+
+# --- 同步字与帧长 -----------------------------------------------------------
+SYNC_CONTROL = (0xEB, 0x90)    # 表2-1
+SYNC_TELEMETRY = (0xEE, 0x16)  # 表2-2
+SYNC_ATTITUDE = (0xEB, 0x91)   # 表3
+SYNC_TARGET = (0xEE, 0x18)     # 表4
+
+CONTROL_LEN = 16
+TELEMETRY_LEN = 32
+ATTITUDE_LEN = 32
+TARGET_LEN = 32
+
+# --- 量纲（每 LSB 物理量）---------------------------------------------------
+ANGLE_SCALE = 0.01    # 角度 / 偏移角，°
+RATE_SCALE = 0.01     # 角速度，°/s
+RANGE_SCALE = 0.1     # 激光测距，m
+ALT_SCALE = 0.2       # 高度，m
+LATLON_SCALE = 1e-7   # 经纬度，°
+SPEED_SCALE = 0.5     # 空速 / 地速，m/s
+
+_S16_MIN, _S16_MAX = -32768, 32767
+_U16_MAX = 65535
+_S32_MIN, _S32_MAX = -(2 ** 31), 2 ** 31 - 1
+
+
+def checksum(body: bytes) -> int:
+    """从第 0 字节累加取低 8 位（body 不含校验和自身）。"""
+    return sum(body) & 0xFF
+
+
+def _to_s16(value: float, scale: float) -> int:
+    return max(_S16_MIN, min(_S16_MAX, int(round(value / scale))))
+
+
+def _to_u16(value: float, scale: float) -> int:
+    return max(0, min(_U16_MAX, int(round(value / scale))))
+
+
+def _to_s32(value: float, scale: float) -> int:
+    return max(_S32_MIN, min(_S32_MAX, int(round(value / scale))))
+
+
+class ControlCode(IntEnum):
+    """表2-1 字节 2「控制信息」取值。"""
+
+    NONE = 0x00                 # 空指令
+    VISIBLE_1 = 0x01            # 可见光1
+    VISIBLE_2 = 0x02            # 可见光2
+    IR_1 = 0x03                 # 红外1
+    IR_2 = 0x04                 # 红外2
+    TARGET_DETECT_ON = 0x07     # 目标识别开（参数X：0=人/车，1=无人机）
+    TARGET_DETECT_OFF = 0x08    # 目标识别关（默认）
+    VIDEO_RECORD_ON = 0x09      # 视频录制开
+    VIDEO_RECORD_OFF = 0x0A     # 视频录制关（默认）
+    BOX_TRACK = 0x0C            # 框选跟踪（左上 X/Y + 右下 X2/Y2）
+    POINT_TRACK = 0x0D          # 指点跟踪（X/Y）
+    STOP_TRACK = 0x0E           # 停止跟踪
+    AUTO_LOCK_ON_FOUND = 0x0F   # 发现目标自动锁定
+    DETECT_TO_TRACK = 0x10      # 识别转跟踪
+    IR_WHITE_HOT = 0x11         # 红外白热
+    IR_BLACK_HOT = 0x12         # 红外黑热
+    ANTI_OCCLUSION_OFF = 0x17   # 关抗遮挡
+    ANTI_OCCLUSION_ON = 0x18    # 开抗遮挡
+    GIMBAL_SEARCH = 0x24        # 云台搜索（X=方位转速0.1°/s，Y=俯仰转速0.1°/s）
+    OPTICAL_ZOOM = 0x25         # 光学变焦（X=倍数0.1倍）
+    SET_FRAME_ANGLE = 0x26      # 指定框架角度（X=方位0.01°，Y=俯仰0.01°）
+    MOTOR_ON = 0x27             # 开电机
+    MOTOR_OFF = 0x28            # 关电机
+    FOLLOW_OFF = 0x29           # 关跟随
+    FOLLOW_ON = 0x2A            # 跟随
+    CENTER = 0x2B               # 归中
+    SUPPRESS_GYRO_DRIFT = 0x2C  # 抑制陀螺仪漂移
+    ELECTRIC_LOCK = 0x30        # 电锁模式
+    ELECTRIC_UNLOCK = 0x31      # 解除电锁
+    PIP_ON = 0x40               # 开画中画
+    PIP_OFF = 0x41              # 关画中画
+    OSD_ON = 0x51               # OSD 显示开
+    OSD_OFF = 0x52              # OSD 显示关
+    SET_ZOOM = 0x5A             # 指定变焦倍数（X=倍数0.1倍）
+    TAKE_PHOTO = 0x5B           # 拍照指令
+    EIS_ON = 0x5C               # 开电子稳像
+    EIS_OFF = 0x5D              # 关电子稳像
+    SERVO_STAB_ON = 0x60        # 开伺服稳像
+    SERVO_STAB_OFF = 0x61       # 关伺服稳像
+    VERSION_QUERY = 0x62        # 版本查询
+    SET_BITRATE = 0x64          # 设置码率（X=kbps）
+
+
+# --- 表2-1 控制指令 ---------------------------------------------------------
+@dataclass
+class ControlCommand:
+    """控制器 → 吊舱 控制帧。param* 语义随 code 变化，见 ControlCode 注释。"""
+
+    code: int = ControlCode.NONE
+    param_x: int = 0          # S16
+    param_y: int = 0          # S16
+    param3: int = 0           # U8（画中画小画面等）
+    file_save_mode: int = 0   # S8（0=时间戳，1=前缀+序号）
+    param_x2: int = 0         # S16（框选跟踪右下角横向）
+    param_y2: int = 0         # S16（框选跟踪右下角纵向）
+
+
+def encode_control(cmd: ControlCommand) -> bytes:
+    body = struct.pack(
+        "<BBBhhBbhh2s",
+        SYNC_CONTROL[0], SYNC_CONTROL[1],
+        cmd.code & 0xFF,
+        _to_s16(cmd.param_x, 1), _to_s16(cmd.param_y, 1),
+        cmd.param3 & 0xFF,
+        max(-128, min(127, int(cmd.file_save_mode))),
+        _to_s16(cmd.param_x2, 1), _to_s16(cmd.param_y2, 1),
+        b"\x00\x00",
+    )
+    return body + bytes([checksum(body)])
+
+
+def decode_control(data: bytes) -> ControlCommand:
+    _check(data, CONTROL_LEN, SYNC_CONTROL)
+    code, x, y, param3, save, x2, y2 = struct.unpack("<BhhBbhh", data[2:13])
+    return ControlCommand(code=code, param_x=x, param_y=y, param3=param3,
+                          file_save_mode=save, param_x2=x2, param_y2=y2)
+
+
+# --- 表2-2 周期遥测 ---------------------------------------------------------
+@dataclass
+class Telemetry:
+    """吊舱 → 控制器 周期遥测帧（60Hz）。角度单位 °，角速度 °/s，测距 m。"""
+
+    status1: int = 0
+    status2: int = 0
+    status3: int = 0
+    off_target_x_deg: float = 0.0   # 目标脱靶量 X 轴偏移角
+    off_target_y_deg: float = 0.0   # 目标脱靶量 Y 轴偏移角
+    frame_roll_deg: float = 0.0     # 横滚框架角
+    frame_pitch_deg: float = 0.0    # 俯仰框架角
+    frame_yaw_deg: float = 0.0      # 方位框架角
+    att_roll_deg: float = 0.0       # 横滚姿态角
+    att_pitch_deg: float = 0.0      # 俯仰姿态角
+    roll_rate_dps: float = 0.0
+    pitch_rate_dps: float = 0.0
+    yaw_rate_dps: float = 0.0
+    laser_range_m: float | None = None  # None 表示无效（原始 0）
+    self_check: int = 0
+
+    @property
+    def track_source(self) -> int:
+        """Bit7-6 跟踪视频源：0可见1 1可见2 2红外1 3红外2。"""
+        return (self.status1 >> 6) & 0x3
+
+    @property
+    def is_target_locked(self) -> bool:
+        """Bit2 目标跟踪状态：True=锁定，False=搜索。"""
+        return bool((self.status1 >> 2) & 0x1)
+
+    @property
+    def video_storing(self) -> bool:
+        return bool((self.status2 >> 5) & 0x1)
+
+    @property
+    def motor_on(self) -> bool:
+        return bool((self.status2 >> 3) & 0x1)
+
+    @property
+    def follow_on(self) -> bool:
+        return bool((self.status2 >> 2) & 0x1)
+
+    @property
+    def electric_lock_on(self) -> bool:
+        return bool((self.status2 >> 1) & 0x1)
+
+    @property
+    def big_picture(self) -> int:
+        return (self.status3 >> 6) & 0x3
+
+    @property
+    def small_picture(self) -> int:
+        return (self.status3 >> 4) & 0x3
+
+
+def encode_telemetry(t: Telemetry) -> bytes:
+    laser = 0 if t.laser_range_m is None else _to_u16(t.laser_range_m, RANGE_SCALE)
+    body = struct.pack(
+        "<BBBBBBhhhhhhhhhhHB2s",
+        SYNC_TELEMETRY[0], SYNC_TELEMETRY[1],
+        t.status1 & 0xFF, t.status2 & 0xFF, 0, t.status3 & 0xFF,
+        _to_s16(t.off_target_x_deg, ANGLE_SCALE),
+        _to_s16(t.off_target_y_deg, ANGLE_SCALE),
+        _to_s16(t.frame_roll_deg, ANGLE_SCALE),
+        _to_s16(t.frame_pitch_deg, ANGLE_SCALE),
+        _to_s16(t.frame_yaw_deg, ANGLE_SCALE),
+        _to_s16(t.att_roll_deg, ANGLE_SCALE),
+        _to_s16(t.att_pitch_deg, ANGLE_SCALE),
+        _to_s16(t.roll_rate_dps, RATE_SCALE),
+        _to_s16(t.pitch_rate_dps, RATE_SCALE),
+        _to_s16(t.yaw_rate_dps, RATE_SCALE),
+        laser,
+        t.self_check & 0xFF,
+        b"\x00\x00",
+    )
+    return body + bytes([checksum(body)])
+
+
+def decode_telemetry(data: bytes) -> Telemetry:
+    _check(data, TELEMETRY_LEN, SYNC_TELEMETRY)
+    (s1, s2, _rsv, s3, ox, oy, froll, fpitch, fyaw, aroll, apitch,
+     rrate, prate, yrate, laser, selfcheck, _rsv2) = struct.unpack(
+        "<BBBBhhhhhhhhhhHB2s", data[2:31])
+    return Telemetry(
+        status1=s1, status2=s2, status3=s3,
+        off_target_x_deg=ox * ANGLE_SCALE,
+        off_target_y_deg=oy * ANGLE_SCALE,
+        frame_roll_deg=froll * ANGLE_SCALE,
+        frame_pitch_deg=fpitch * ANGLE_SCALE,
+        frame_yaw_deg=fyaw * ANGLE_SCALE,
+        att_roll_deg=aroll * ANGLE_SCALE,
+        att_pitch_deg=apitch * ANGLE_SCALE,
+        roll_rate_dps=rrate * RATE_SCALE,
+        pitch_rate_dps=prate * RATE_SCALE,
+        yaw_rate_dps=yrate * RATE_SCALE,
+        laser_range_m=(laser * RANGE_SCALE) if laser != 0 else None,
+        self_check=selfcheck,
+    )
+
+
+# --- 表3 载机姿态和经纬度 ---------------------------------------------------
+@dataclass
+class CarrierAttitude:
+    """控制器 → 吊舱 载机姿态/位置/时间帧（供吊舱解算目标经纬度）。"""
+
+    roll_deg: float = 0.0
+    pitch_deg: float = 0.0
+    yaw_deg: float = 0.0
+    latitude_deg: float = 0.0
+    longitude_deg: float = 0.0
+    abs_alt_m: float = 0.0
+    rel_alt_m: float = 0.0
+    year: int = 2000        # 完整年份（编码时存 year-2000）
+    month: int = 1
+    day: int = 1
+    hour: int = 0
+    minute: int = 0
+    second: int = 0
+    centisecond: int = 0    # 百分秒，10ms 单位，0~99
+    airspeed_mps: float = 0.0
+    ground_speed_mps: float = 0.0
+
+
+def encode_attitude(a: CarrierAttitude) -> bytes:
+    body = struct.pack(
+        "<BBhhhiihhBBBBBBBHH",
+        SYNC_ATTITUDE[0], SYNC_ATTITUDE[1],
+        _to_s16(a.roll_deg, ANGLE_SCALE),
+        _to_s16(a.pitch_deg, ANGLE_SCALE),
+        _to_s16(a.yaw_deg, ANGLE_SCALE),
+        _to_s32(a.latitude_deg, LATLON_SCALE),
+        _to_s32(a.longitude_deg, LATLON_SCALE),
+        _to_s16(a.abs_alt_m, ALT_SCALE),
+        _to_s16(a.rel_alt_m, ALT_SCALE),
+        (a.year - 2000) & 0xFF, a.month & 0xFF, a.day & 0xFF,
+        a.hour & 0xFF, a.minute & 0xFF, a.second & 0xFF, a.centisecond & 0xFF,
+        _to_u16(a.airspeed_mps, SPEED_SCALE),
+        _to_u16(a.ground_speed_mps, SPEED_SCALE),
+    )
+    return body + bytes([checksum(body)])
+
+
+def decode_attitude(data: bytes) -> CarrierAttitude:
+    _check(data, ATTITUDE_LEN, SYNC_ATTITUDE)
+    (roll, pitch, yaw, lat, lon, absalt, relalt, yr, mo, day, hour, mi, sec,
+     cs, air, gs) = struct.unpack("<hhhiihhBBBBBBBHH", data[2:31])
+    return CarrierAttitude(
+        roll_deg=roll * ANGLE_SCALE,
+        pitch_deg=pitch * ANGLE_SCALE,
+        yaw_deg=yaw * ANGLE_SCALE,
+        latitude_deg=lat * LATLON_SCALE,
+        longitude_deg=lon * LATLON_SCALE,
+        abs_alt_m=absalt * ALT_SCALE,
+        rel_alt_m=relalt * ALT_SCALE,
+        year=2000 + yr, month=mo, day=day, hour=hour, minute=mi,
+        second=sec, centisecond=cs,
+        airspeed_mps=air * SPEED_SCALE,
+        ground_speed_mps=gs * SPEED_SCALE,
+    )
+
+
+# --- 表4 目标经纬度 ---------------------------------------------------------
+@dataclass
+class TargetGeo:
+    """吊舱 → 控制器 目标经纬度帧（吊舱解算结果）。"""
+
+    latitude_deg: float = 0.0
+    longitude_deg: float = 0.0
+    abs_alt_m: float = 0.0
+    rel_alt_m: float = 0.0
+    year: int = 2000
+    month: int = 1
+    day: int = 1
+    hour: int = 0
+    minute: int = 0
+    second: int = 0
+    centisecond: int = 0
+
+
+def encode_target(g: TargetGeo) -> bytes:
+    body = struct.pack(
+        "<BBiihhBBBBBBB10s",
+        SYNC_TARGET[0], SYNC_TARGET[1],
+        _to_s32(g.latitude_deg, LATLON_SCALE),
+        _to_s32(g.longitude_deg, LATLON_SCALE),
+        _to_s16(g.abs_alt_m, ALT_SCALE),
+        _to_s16(g.rel_alt_m, ALT_SCALE),
+        (g.year - 2000) & 0xFF, g.month & 0xFF, g.day & 0xFF,
+        g.hour & 0xFF, g.minute & 0xFF, g.second & 0xFF, g.centisecond & 0xFF,
+        b"\x00" * 10,
+    )
+    return body + bytes([checksum(body)])
+
+
+def decode_target(data: bytes) -> TargetGeo:
+    _check(data, TARGET_LEN, SYNC_TARGET)
+    (lat, lon, absalt, relalt, yr, mo, day, hour, mi, sec, cs,
+     _rsv) = struct.unpack("<iihhBBBBBBB10s", data[2:31])
+    return TargetGeo(
+        latitude_deg=lat * LATLON_SCALE,
+        longitude_deg=lon * LATLON_SCALE,
+        abs_alt_m=absalt * ALT_SCALE,
+        rel_alt_m=relalt * ALT_SCALE,
+        year=2000 + yr, month=mo, day=day, hour=hour, minute=mi,
+        second=sec, centisecond=cs,
+    )
+
+
+def _check(data: bytes, length: int, sync: tuple) -> None:
+    if len(data) != length:
+        raise ValueError(f"帧长错误: 期望 {length} 字节, 实际 {len(data)}")
+    if data[0] != sync[0] or data[1] != sync[1]:
+        raise ValueError(
+            f"同步字错误: 期望 {sync[0]:02X} {sync[1]:02X}, "
+            f"实际 {data[0]:02X} {data[1]:02X}")
+    if checksum(data[:length - 1]) != data[length - 1]:
+        raise ValueError("校验和错误")
+
+
+# --- 字节流分帧器 -----------------------------------------------------------
+_FRAME_SPECS = {
+    SYNC_CONTROL: (CONTROL_LEN, decode_control),
+    SYNC_TELEMETRY: (TELEMETRY_LEN, decode_telemetry),
+    SYNC_ATTITUDE: (ATTITUDE_LEN, decode_attitude),
+    SYNC_TARGET: (TARGET_LEN, decode_target),
+}
+
+
+class FrameParser:
+    """串口字节流分帧：按同步字滑窗取定长帧，校验通过则解码输出。
+
+    默认只接收吊舱 → 控制器的两种帧（遥测 EE16 / 目标 EE18）；可通过 kinds
+    指定要识别的同步字集合（如吊舱模拟器侧识别 EB90/EB91）。粘包、半帧、坏校验
+    均安全处理：坏校验丢弃该同步字首字节后重扫，半帧留待下次 feed。
+    """
+
+    def __init__(self, kinds: tuple = (SYNC_TELEMETRY, SYNC_TARGET)) -> None:
+        for k in kinds:
+            if k not in _FRAME_SPECS:
+                raise ValueError(f"未知帧类型: {k}")
+        self._specs = {k: _FRAME_SPECS[k] for k in kinds}
+        self._buf = bytearray()
+
+    def feed(self, data: bytes):
+        """喂入任意长度字节，返回本次可解析出的帧列表（按到达顺序）。"""
+        self._buf.extend(data)
+        frames = []
+        while True:
+            idx, sync = self._find_sync()
+            if idx is None:
+                # 无同步字：仅保留最后 1 字节（可能是下一帧同步字首字节）
+                if len(self._buf) > 1:
+                    del self._buf[:-1]
+                break
+            if idx > 0:
+                del self._buf[:idx]
+            length, decoder = self._specs[sync]
+            if len(self._buf) < length:
+                break  # 半帧，等待更多数据
+            frame = bytes(self._buf[:length])
+            if checksum(frame[:length - 1]) == frame[length - 1]:
+                frames.append(decoder(frame))
+                del self._buf[:length]
+            else:
+                del self._buf[:1]  # 坏校验：跳过该同步字首字节重扫
+        return frames
+
+    def _find_sync(self):
+        earliest_idx, earliest_sync = None, None
+        for sync in self._specs:
+            i = self._buf.find(bytes(sync))
+            if i >= 0 and (earliest_idx is None or i < earliest_idx):
+                earliest_idx, earliest_sync = i, sync
+        return earliest_idx, earliest_sync

utils/pod_bridge/px4_gateway.py

@@ -0,0 +1,169 @@
+"""px4_gateway.py —— PX4(MAVSDK) 侧网关：读飞控遥测 + 发 offboard 速度指令。
+
+职责（桥接的飞控侧）：
+  * 订阅 PX4 遥测（position / attitude_euler / gps_info / velocity_body），缓存为
+    OwnState 快照，供桥接层构造 表3(载机姿态下发) 与 GuidanceInput(引导输入)；
+  * 复用 visual_track_control.MavsdkCommandGateway 发送 offboard 速度设定点
+    （不重造三分支映射，直接喂 GuidanceLaw 的 ControlOutput）。
+
+设计：遥测拆为「纯缓存更新方法 update_*（无 IO，可单测）」+「薄异步订阅胶水
+start_telemetry/_consume」，便于无硬件 mock 测试。指令发送整体委托给被组合的
+MavsdkCommandGateway。
+"""
+
+import asyncio
+import logging
+import math
+import time
+from dataclasses import dataclass, replace
+from typing import Any, Callable, Optional
+
+from visual_track_control.mavsdk_gateway import MavsdkCommandGateway
+
+logger = logging.getLogger(__name__)
+
+_GPS_FIX_3D = 3  # mavsdk GpsFixType.THREE_D_FIX 起视为定位有效
+
+
+def _enum_int(value: Any) -> int:
+    """MAVSDK 枚举（Enum/IntEnum）或裸整数统一转 int。"""
+    if hasattr(value, "value"):
+        try:
+            return int(value.value)
+        except (TypeError, ValueError):
+            return 0
+    try:
+        return int(value)
+    except (TypeError, ValueError):
+        return 0
+
+
+@dataclass
+class OwnState:
+    """PX4 遥测缓存快照（角度弧度 NED；位置度；高度/速度米制）。"""
+
+    latitude_deg: float = 0.0
+    longitude_deg: float = 0.0
+    absolute_altitude_m: float = 0.0
+    relative_altitude_m: float = 0.0
+    roll_rad: float = 0.0
+    pitch_rad: float = 0.0
+    yaw_rad: float = 0.0
+    ground_speed_mps: float = 0.0
+    fix_type: int = 0
+    num_satellites: int = 0
+    has_position: bool = False
+    has_attitude: bool = False
+    position_valid: bool = False  # has_position 且 GPS 3D 定位
+    time_ms: int = 0              # 任意遥测流最近一次更新（毫秒），仅供诊断
+    position_time_ms: int = 0     # position 流最近一次更新（毫秒），位置超龄判定专用
+
+
+class Px4Gateway:
+    """drone：mavsdk System（duck-typed，便于注入 mock）。"""
+
+    def __init__(self, drone: Any,
+                 now_ms_fn: Optional[Callable[[], int]] = None) -> None:
+        self._drone = drone
+        self._cmd = MavsdkCommandGateway(drone)
+        self._now_ms = now_ms_fn or (lambda: int(time.monotonic() * 1000))
+        self._state = OwnState()
+        self._tasks: list = []
+
+    # --- 连接（实机）-------------------------------------------------------
+    @classmethod
+    async def connect(cls, connection_string: str,
+                      now_ms_fn: Optional[Callable[[], int]] = None) -> "Px4Gateway":
+        """创建 MAVSDK System、连接并等待联通后返回网关。"""
+        from mavsdk import System
+        drone = System()
+        logger.info("[Px4Gateway] 连接飞控 %s ...", connection_string)
+        await drone.connect(system_address=connection_string)
+        async for state in drone.core.connection_state():
+            if state.is_connected:
+                logger.info("[Px4Gateway] 飞控已连接")
+                break
+        return cls(drone, now_ms_fn=now_ms_fn)
+
+    # --- 遥测缓存更新（纯方法，可单测）------------------------------------
+    def update_position(self, p: Any) -> None:
+        self._state.latitude_deg = float(p.latitude_deg)
+        self._state.longitude_deg = float(p.longitude_deg)
+        self._state.absolute_altitude_m = float(p.absolute_altitude_m)
+        self._state.relative_altitude_m = float(p.relative_altitude_m)
+        self._state.has_position = True
+        self._recompute_valid()
+        now = self._now_ms()
+        self._state.time_ms = now
+        self._state.position_time_ms = now  # 仅此流刷新，姿态高频流不得代刷
+
+    def update_attitude(self, e: Any) -> None:
+        self._state.roll_rad = float(e.roll_rad)
+        self._state.pitch_rad = float(e.pitch_rad)
+        self._state.yaw_rad = float(e.yaw_rad)
+        self._state.has_attitude = True
+        self._state.time_ms = self._now_ms()
+
+    def update_gps(self, g: Any) -> None:
+        self._state.fix_type = _enum_int(getattr(g, "fix_type", 0))
+        self._state.num_satellites = _enum_int(getattr(g, "num_satellites", 0))
+        self._recompute_valid()
+        self._state.time_ms = self._now_ms()
+
+    def update_velocity_body(self, v: Any) -> None:
+        self._state.ground_speed_mps = math.hypot(
+            float(v.forward_m_s), float(v.right_m_s))
+        self._state.time_ms = self._now_ms()
+
+    def own_state(self) -> OwnState:
+        """返回当前缓存的快照副本（调用方修改不影响内部状态）。"""
+        return replace(self._state)
+
+    def _recompute_valid(self) -> None:
+        self._state.position_valid = (self._state.has_position
+                                      and self._state.fix_type >= _GPS_FIX_3D)
+
+    # --- 遥测订阅（异步胶水）----------------------------------------------
+    async def start_telemetry(self) -> None:
+        """订阅四路遥测流，后台任务持续刷新缓存。"""
+        t = self._drone.telemetry
+        self._tasks = [
+            asyncio.ensure_future(self._consume(t.position(), self.update_position)),
+            asyncio.ensure_future(
+                self._consume(t.attitude_euler(), self.update_attitude)),
+            asyncio.ensure_future(self._consume(t.gps_info(), self.update_gps)),
+            asyncio.ensure_future(
+                self._consume(t.velocity_body(), self.update_velocity_body)),
+        ]
+        logger.info("[Px4Gateway] 遥测订阅已启动")
+
+    async def _consume(self, stream: Any, handler: Callable[[Any], None]) -> None:
+        try:
+            async for msg in stream:
+                handler(msg)
+        except asyncio.CancelledError:
+            raise
+        except Exception as exc:
+            logger.warning("[Px4Gateway] 遥测流中断: %s", exc)
+
+    async def stop_telemetry(self) -> None:
+        for task in self._tasks:
+            task.cancel()
+        if self._tasks:
+            await asyncio.gather(*self._tasks, return_exceptions=True)
+        self._tasks = []
+        logger.info("[Px4Gateway] 遥测订阅已停止")
+
+    # --- offboard 指令（委托 MavsdkCommandGateway）-------------------------
+    async def start_offboard(self) -> bool:
+        return await self._cmd.start()
+
+    async def send_velocity_heading(self, output: Any,
+                                    time_boot_ms: Optional[int] = None) -> bool:
+        return await self._cmd.send_velocity_heading(output, time_boot_ms)
+
+    async def send_brake_hover(self, time_boot_ms: Optional[int] = None) -> bool:
+        return await self._cmd.send_brake_hover(time_boot_ms)
+
+    async def stop_offboard(self) -> None:
+        await self._cmd.stop()

utils/tests/pod_bridge/bridge_test.py

@@ -0,0 +1,358 @@
+"""bridge_test.py —— PodBridge 闭环主循环单元测试（FakePx4 + FakePod，无硬件）。
+
+覆盖：单周期 step（有/无目标、位置无效、位置时间戳超龄）、姿态下发节流与帧内容、
+吊舱帧入站缓存、锁定门控（表2-2 bit2 + 遥测超龄）、触发式控制指令编码、
+start/stop 生命周期与停机刹车、run() 主循环异常护栏与故障恢复。
+"""
+
+import asyncio
+import time
+from dataclasses import replace
+
+from pod_bridge.bridge import PodBridge
+from pod_bridge.config import BridgeConfig
+from pod_bridge.protocol import (ControlCode, ControlCommand, TargetGeo,
+                                 Telemetry, decode_attitude, decode_control)
+from pod_bridge.px4_gateway import OwnState
+
+
+class FakePx4:
+    def __init__(self, state=None):
+        self._state = state or OwnState()
+        self.telemetry_started = False
+        self.telemetry_stopped = False
+        self.offboard_started = False
+        self.offboard_stopped = False
+        self.offboard_start_ok = True
+        self.velocity_calls = []
+        self.brake_calls = []
+
+    def own_state(self):
+        return replace(self._state)
+
+    async def start_telemetry(self):
+        self.telemetry_started = True
+
+    async def stop_telemetry(self):
+        self.telemetry_stopped = True
+
+    async def start_offboard(self):
+        self.offboard_started = True
+        return self.offboard_start_ok
+
+    async def stop_offboard(self):
+        self.offboard_stopped = True
+
+    async def send_velocity_heading(self, out, time_boot_ms=None):
+        self.velocity_calls.append((out, time_boot_ms))
+        return True
+
+    async def send_brake_hover(self, time_boot_ms=None):
+        self.brake_calls.append(time_boot_ms)
+        return True
+
+
+class FakePod:
+    def __init__(self):
+        self.inbox = []
+        self.sent = []
+        self.closed = False
+
+    def queue(self, *frames):
+        self.inbox.extend(frames)
+
+    def poll(self):
+        out, self.inbox = self.inbox, []
+        return out
+
+    def send(self, data):
+        self.sent.append(data)
+
+    def close(self):
+        self.closed = True
+
+
+def _valid_state(now_ms, lat=0.0, lon=0.0, yaw_rad=0.0, gs=0.0):
+    return OwnState(latitude_deg=lat, longitude_deg=lon, yaw_rad=yaw_rad,
+                    ground_speed_mps=gs, fix_type=3, num_satellites=12,
+                    has_position=True, has_attitude=True, position_valid=True,
+                    time_ms=now_ms, position_time_ms=now_ms)
+
+
+def _locked_telemetry():
+    """状态字节1 bit2=1(锁定)的遥测帧。"""
+    return Telemetry(status1=0b00000100)
+
+
+def _bridge(px4, pod, cfg=None):
+    return PodBridge(cfg or BridgeConfig(), px4, pod, now_ms_fn=lambda: 0)
+
+
+# --- step：有目标 → 速度指令 ------------------------------------------------
+def test_step_with_target_commands_velocity():
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    pod.queue(TargetGeo(latitude_deg=0.001, longitude_deg=0.0),  # 正北 ~111m
+              _locked_telemetry())
+    b = _bridge(px4, pod)
+
+    out = asyncio.run(b.step(1000))
+    assert out.mode.name == "VELOCITY_HEADING"
+    assert len(px4.velocity_calls) == 1
+    assert px4.brake_calls == []
+
+
+def test_step_without_target_brakes():
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    b = _bridge(px4, pod)
+
+    out = asyncio.run(b.step(1000))
+    assert out.mode.name == "BRAKE_HOVER"
+    assert len(px4.brake_calls) == 1
+    assert px4.velocity_calls == []
+
+
+def test_step_invalid_position_brakes_even_with_target():
+    state = replace(_valid_state(1000), position_valid=False, fix_type=0)
+    px4 = FakePx4(state)
+    pod = FakePod()
+    pod.queue(TargetGeo(latitude_deg=0.001))
+    b = _bridge(px4, pod)
+
+    out = asyncio.run(b.step(1000))
+    assert out.mode.name == "BRAKE_HOVER"
+
+
+# --- 姿态下发：首拍即发 + 节流 + 帧内容 ------------------------------------
+def test_attitude_downlinked_on_first_step_with_own_state():
+    px4 = FakePx4(_valid_state(1000, lat=31.5, lon=121.5, yaw_rad=1.57, gs=4.0))
+    pod = FakePod()
+    b = _bridge(px4, pod)
+
+    asyncio.run(b.step(1000))
+    assert len(pod.sent) == 1
+    att = decode_attitude(pod.sent[0])
+    assert abs(att.latitude_deg - 31.5) < 1e-6
+    assert abs(att.longitude_deg - 121.5) < 1e-6
+    assert abs(att.yaw_deg - 89.95) < 0.02      # 1.57 rad ≈ 89.95°
+    assert att.ground_speed_mps == 4.0
+    assert att.year >= 2026 and 1 <= att.month <= 12
+
+
+def test_attitude_downlink_throttled():
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    cfg = BridgeConfig()
+    cfg.link.attitude_downlink_hz = 20.0  # 周期 50ms
+    b = _bridge(px4, pod, cfg)
+
+    async def scenario():
+        await b.step(1000)  # 发（首拍）
+        await b.step(1010)  # 10ms < 50ms，跳过
+        await b.step(1040)  # 40ms < 50ms，跳过
+        await b.step(1060)  # 60ms ≥ 50ms，发
+    asyncio.run(scenario())
+    assert len(pod.sent) == 2
+
+
+# --- 吊舱帧入站缓存 --------------------------------------------------------
+def test_target_frame_updates_latest_and_flips_to_velocity():
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    b = _bridge(px4, pod)
+
+    asyncio.run(b.step(1000))                 # 无目标 → 刹车
+    assert b.latest_target is None
+    pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry())
+    asyncio.run(b.step(1100))                 # 收到目标且锁定 → 速度
+    assert b.latest_target is not None
+    assert len(px4.velocity_calls) == 1
+
+
+def test_telemetry_frame_cached():
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    pod.queue(Telemetry(laser_range_m=88.8))
+    b = _bridge(px4, pod)
+    asyncio.run(b.step(1000))
+    assert b.latest_telemetry is not None
+    assert b.latest_telemetry[0].laser_range_m == 88.8
+
+
+# --- 触发式控制指令 --------------------------------------------------------
+def test_send_control_encodes_frame():
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    b = _bridge(px4, pod)
+    b.send_control(ControlCommand(code=ControlCode.CENTER))
+    assert len(pod.sent) == 1
+    assert decode_control(pod.sent[0]).code == ControlCode.CENTER
+
+
+# --- 生命周期 --------------------------------------------------------------
+def test_start_wires_telemetry_and_offboard():
+    px4 = FakePx4(_valid_state(0))
+    b = _bridge(px4, FakePod())
+    assert asyncio.run(b.start()) is True
+    assert px4.telemetry_started and px4.offboard_started
+
+
+def test_start_returns_false_when_offboard_fails():
+    px4 = FakePx4(_valid_state(0))
+    px4.offboard_start_ok = False
+    b = _bridge(px4, FakePod())
+    assert asyncio.run(b.start()) is False
+
+
+def test_stop_brakes_and_tears_down():
+    px4 = FakePx4(_valid_state(0))
+    pod = FakePod()
+    b = _bridge(px4, pod)
+
+    async def scenario():
+        await b.start()
+        await b.stop()
+    asyncio.run(scenario())
+    assert len(px4.brake_calls) >= 1     # 停机前刹车
+    assert px4.offboard_stopped and px4.telemetry_stopped
+    assert pod.closed
+
+
+
+# --- 缺陷①回归:position 时间戳超龄 -------------------------------------------
+def test_step_brakes_when_position_timestamp_stale():
+    """time_ms 被姿态流刷得很新,但 position_time_ms 超龄 → 必须刹车。"""
+    state = replace(_valid_state(1000), time_ms=100000, position_time_ms=1000)
+    px4 = FakePx4(state)
+    pod = FakePod()
+    pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry())
+    b = _bridge(px4, pod)
+
+    out = asyncio.run(b.step(100000))  # position 已 99s 未更新
+    assert out.mode.name == "BRAKE_HOVER"
+    assert px4.velocity_calls == []
+
+
+# --- 缺陷②:锁定门控 -----------------------------------------------------------
+def test_target_without_lock_bit_brakes():
+    """吊舱在搜索状态(bit2=0),即使收到表4 坐标也必须刹车。"""
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    pod.queue(TargetGeo(latitude_deg=0.001), Telemetry(status1=0))  # 未锁定
+    b = _bridge(px4, pod)
+
+    out = asyncio.run(b.step(1000))
+    assert out.mode.name == "BRAKE_HOVER"
+    assert px4.velocity_calls == []
+
+
+def test_target_without_any_telemetry_brakes():
+    """只收到表4、从未收到表2-2 → 锁定状态未知,按未锁定处理。"""
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    pod.queue(TargetGeo(latitude_deg=0.001))
+    b = _bridge(px4, pod)
+
+    out = asyncio.run(b.step(1000))
+    assert out.mode.name == "BRAKE_HOVER"
+
+
+def test_target_with_stale_telemetry_brakes():
+    """锁定遥测超龄(> link.telemetry_timeout_ms)→ 目标视为无效。"""
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    cfg = BridgeConfig()
+    cfg.link.telemetry_timeout_ms = 500
+    b = PodBridge(cfg, px4, pod, now_ms_fn=lambda: 0)
+
+    async def scenario():
+        pod.queue(_locked_telemetry(), TargetGeo(latitude_deg=0.001))
+        await b.step(1000)                    # 遥测新鲜,但此刻若无目标则刹车
+        pod.queue(TargetGeo(latitude_deg=0.001))
+        out1 = await b.step(1200)             # 遥测 200ms 前,仍新鲜 → 速度
+        out2 = await b.step(1600)             # 遥测 600ms 前,超龄 → 刹车
+        return out1, out2
+
+    out1, out2 = asyncio.run(scenario())
+    assert out1.mode.name == "VELOCITY_HEADING"
+    assert out2.mode.name == "BRAKE_HOVER"
+
+
+def test_lock_lost_midflight_falls_back_to_brake():
+    """先锁定引导,随后遥测变为搜索状态 → 立即回落刹车。"""
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    b = _bridge(px4, pod)
+
+    async def scenario():
+        pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry())
+        out1 = await b.step(1000)
+        pod.queue(Telemetry(status1=0))       # 丢失锁定
+        out2 = await b.step(1100)
+        return out1, out2
+
+    out1, out2 = asyncio.run(scenario())
+    assert out1.mode.name == "VELOCITY_HEADING"
+    assert out2.mode.name == "BRAKE_HOVER"
+
+
+# --- 缺陷③:run() 异常护栏 -----------------------------------------------------
+class FlakyPod(FakePod):
+    """前 fail_times 次 poll 抛串口异常,之后恢复正常。"""
+
+    def __init__(self, fail_times):
+        super().__init__()
+        self.fail_times = fail_times
+
+    def poll(self):
+        if self.fail_times > 0:
+            self.fail_times -= 1
+            raise OSError("serial device disconnected")
+        return super().poll()
+
+
+def _run_bridge(px4, pod, hz=200.0):
+    cfg = BridgeConfig()
+    cfg.guidance.frequency_hz = hz  # 加快节拍,缩短测试时间
+    return PodBridge(cfg, px4, pod)  # 用真实单调时钟
+
+
+def test_run_survives_poll_exceptions_and_keeps_heartbeat():
+    """串口持续异常:任务不死、每周期补发刹车保心跳、stop_event 正常退出。"""
+    now = int(time.monotonic() * 1000)
+    px4 = FakePx4(_valid_state(now))
+    pod = FlakyPod(fail_times=10**9)  # 永远失败
+    b = _run_bridge(px4, pod)
+
+    async def scenario():
+        stop = asyncio.Event()
+        task = asyncio.ensure_future(b.run(stop))
+        await asyncio.sleep(0.2)
+        stop.set()
+        await asyncio.wait_for(task, timeout=2.0)
+
+    asyncio.run(scenario())
+    assert len(px4.brake_calls) > 0       # 异常期间持续补发刹车
+    assert pod.fail_times > 0             # 循环确实在反复尝试(未提前退出)
+
+
+def test_run_recovers_after_transient_failure():
+    """串口短暂异常后恢复:异常期刹车,恢复后照常引导。"""
+    now = int(time.monotonic() * 1000)
+    px4 = FakePx4(_valid_state(now))
+    pod = FlakyPod(fail_times=3)
+    pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry())
+    b = _run_bridge(px4, pod)
+
+    async def scenario():
+        stop = asyncio.Event()
+        task = asyncio.ensure_future(b.run(stop))
+        await asyncio.sleep(0.3)
+        stop.set()
+        await asyncio.wait_for(task, timeout=2.0)
+
+    asyncio.run(scenario())
+    assert len(px4.brake_calls) >= 3      # 3 次异常各补一发刹车
+    assert len(px4.velocity_calls) >= 1   # 恢复后进入正常引导
+    assert b._step_failures == 0          # 计数已复位

utils/tests/pod_bridge/config_test.py

@@ -0,0 +1,115 @@
+"""config_test.py —— 桥接配置加载与校验单元测试。"""
+
+import json
+from pathlib import Path
+
+import pytest
+
+from pod_bridge.config import (BridgeConfig, GuidanceConfig, PodSerialConfig,
+                               load_from_json)
+
+_DEFAULT_CONFIG = Path(__file__).resolve().parents[2] / "config" / "pod_bridge.json"
+
+
+def _write(tmp_path, obj) -> str:
+    p = tmp_path / "cfg.json"
+    p.write_text(json.dumps(obj), encoding="utf-8")
+    return str(p)
+
+
+def test_shipped_default_config_loads():
+    cfg = load_from_json(str(_DEFAULT_CONFIG))
+    assert cfg.pod_serial.baudrate == 115200
+    assert cfg.px4.connection_string == "udp://:14540"
+    assert cfg.link.attitude_downlink_hz == pytest.approx(20.0)
+    assert cfg.guidance.max_speed_mps == pytest.approx(5.0)
+    assert cfg.guidance.stop_radius_m == pytest.approx(5.0)
+
+
+def test_empty_json_uses_all_defaults(tmp_path):
+    cfg = load_from_json(_write(tmp_path, {}))
+    default = BridgeConfig()
+    assert cfg.pod_serial == default.pod_serial
+    assert cfg.px4 == default.px4
+    assert cfg.link == default.link
+    assert cfg.guidance == default.guidance
+
+
+def test_partial_override_keeps_other_defaults(tmp_path):
+    cfg = load_from_json(_write(tmp_path, {
+        "pod_serial": {"port": "COM3"},
+        "guidance": {"max_speed_mps": 8.0},
+    }))
+    assert cfg.pod_serial.port == "COM3"
+    assert cfg.pod_serial.baudrate == 115200          # 默认回退
+    assert cfg.guidance.max_speed_mps == 8.0
+    assert cfg.guidance.stop_radius_m == GuidanceConfig().stop_radius_m  # 默认回退
+
+
+def test_full_override_roundtrip(tmp_path):
+    obj = {
+        "pod_serial": {"port": "/dev/ttyUSB0", "baudrate": 115200},
+        "px4": {"connection_string": "serial:///dev/ttyS1:57600"},
+        "link": {"attitude_downlink_hz": 30.0, "telemetry_timeout_ms": 800},
+        "guidance": {"frequency_hz": 20.0, "stop_radius_m": 3.0,
+                     "max_speed_mps": 7.5, "speed_kp": 0.8, "yaw_kp": 1.5},
+    }
+    cfg = load_from_json(_write(tmp_path, obj))
+    assert cfg.px4.connection_string == "serial:///dev/ttyS1:57600"
+    assert cfg.link.attitude_downlink_hz == 30.0
+    assert cfg.link.telemetry_timeout_ms == 800
+    assert cfg.guidance.frequency_hz == 20.0
+    assert cfg.guidance.yaw_kp == 1.5
+
+
+# --- 校验错误 ---------------------------------------------------------------
+def test_missing_file_raises():
+    with pytest.raises(ValueError, match="打开失败"):
+        load_from_json(str(_DEFAULT_CONFIG.parent / "no_such_file.json"))
+
+
+def test_non_object_root_raises(tmp_path):
+    p = tmp_path / "bad.json"
+    p.write_text("[1, 2, 3]", encoding="utf-8")
+    with pytest.raises(ValueError, match="根节点必须为对象"):
+        load_from_json(str(p))
+
+
+def test_malformed_json_raises(tmp_path):
+    p = tmp_path / "bad.json"
+    p.write_text("{not json", encoding="utf-8")
+    with pytest.raises(ValueError, match="解析失败"):
+        load_from_json(str(p))
+
+
+def test_wrong_type_raises_with_field_name(tmp_path):
+    with pytest.raises(ValueError, match="guidance.max_speed_mps"):
+        load_from_json(_write(tmp_path, {"guidance": {"max_speed_mps": "fast"}}))
+
+
+def test_unknown_key_rejected(tmp_path):
+    with pytest.raises(ValueError, match="未知字段"):
+        load_from_json(_write(tmp_path, {"guidance": {"max_speed": 5.0}}))
+
+
+def test_empty_connection_string_raises(tmp_path):
+    with pytest.raises(ValueError, match="px4.connection_string"):
+        load_from_json(_write(tmp_path, {"px4": {"connection_string": ""}}))
+
+
+@pytest.mark.parametrize("obj,field", [
+    ({"link": {"attitude_downlink_hz": 100.0}}, "link.attitude_downlink_hz"),  # >60
+    ({"link": {"attitude_downlink_hz": 0.5}}, "link.attitude_downlink_hz"),    # <1
+    ({"guidance": {"max_speed_mps": -1.0}}, "guidance.max_speed_mps"),         # 非正
+    ({"guidance": {"stop_radius_m": -2.0}}, "guidance.stop_radius_m"),         # 负
+    ({"guidance": {"speed_derivative_filter_coef": 1.0}},                      # 上界开
+     "guidance.speed_derivative_filter_coef"),
+    ({"pod_serial": {"baudrate": 0}}, "pod_serial.baudrate"),                  # 非正
+])
+def test_out_of_range_raises(tmp_path, obj, field):
+    with pytest.raises(ValueError, match=field.replace(".", r"\.")):
+        load_from_json(_write(tmp_path, obj))
+
+
+def test_pod_serial_default_port_is_placeholder():
+    assert PodSerialConfig().port == ""

utils/tests/pod_bridge/guidance_test.py

@@ -0,0 +1,152 @@
+"""guidance_test.py —— 地理位置引导律单元测试。
+
+覆盖：大地测量（方位/距离）、对准/未对准/背向目标的速率式输出、到达悬停、
+目标/位置超龄与不可用刹车、速度上限与加速度限幅渐变。
+"""
+
+import math
+
+import pytest
+
+from pod_bridge.guidance import (GuidanceConfig, GuidanceInput, GuidanceLaw,
+                                 haversine_distance_m, initial_bearing_deg)
+from visual_track_control.types import ControlMode
+
+_DEG_LAT_M = 111194.93  # 1° 纬度 ≈ 此米数（R=6371008.8）
+
+
+def _inp(own_lat=0.0, own_lon=0.0, own_yaw=0.0, tgt_lat=0.0, tgt_lon=0.0,
+         pos_valid=True, tgt_valid=True, pos_ms=1000, tgt_ms=1000):
+    return GuidanceInput(
+        own_lat_deg=own_lat, own_lon_deg=own_lon, own_yaw_deg=own_yaw,
+        own_position_valid=pos_valid, own_position_time_ms=pos_ms,
+        target_lat_deg=tgt_lat, target_lon_deg=tgt_lon,
+        target_valid=tgt_valid, target_time_ms=tgt_ms,
+    )
+
+
+# --- 大地测量 ---------------------------------------------------------------
+@pytest.mark.parametrize("tgt,expected", [
+    ((0.001, 0.0), 0.0),      # 正北
+    ((0.0, 0.001), 90.0),     # 正东（赤道）
+    ((-0.001, 0.0), 180.0),   # 正南
+    ((0.0, -0.001), 270.0),   # 正西
+])
+def test_initial_bearing_cardinals(tgt, expected):
+    b = initial_bearing_deg(0.0, 0.0, tgt[0], tgt[1])
+    # 0 与 360 等价，用回绕差判定
+    diff = abs((b - expected + 180.0) % 360.0 - 180.0)
+    assert diff < 0.5
+
+
+def test_haversine_distance_one_millidegree():
+    d = haversine_distance_m(0.0, 0.0, 0.001, 0.0)
+    assert d == pytest.approx(_DEG_LAT_M / 1000.0, abs=1.0)
+
+
+def test_haversine_zero_distance():
+    assert haversine_distance_m(31.2, 121.4, 31.2, 121.4) == pytest.approx(0.0, abs=1e-6)
+
+
+# --- 对准目标：前进、不偏航 -------------------------------------------------
+def test_aligned_target_moves_forward_no_yaw():
+    law = GuidanceLaw(GuidanceConfig())
+    out = law.update(_inp(tgt_lat=0.001, own_yaw=0.0), now_ms=1000)  # 正北 111m，机头朝北
+    assert out.mode is ControlMode.VELOCITY_HEADING
+    assert out.valid is True
+    assert out.vx_body_mps > 0.0
+    assert out.yaw_rate_dps == pytest.approx(0.0, abs=1e-6)
+    assert out.vz_mps == 0.0
+    assert law.last_bearing_deg == pytest.approx(0.0, abs=0.5)
+
+
+def test_target_to_east_yaws_right_no_forward():
+    law = GuidanceLaw(GuidanceConfig())
+    # 目标正东（方位 90），机头朝北（0）→ 误差 +90：右转为正、未对准时前向门控为 0
+    out = law.update(_inp(tgt_lon=0.001, own_yaw=0.0), now_ms=1000)
+    assert out.yaw_rate_dps > 0.0
+    assert out.vx_body_mps == pytest.approx(0.0, abs=1e-6)
+
+
+def test_facing_target_moves_forward():
+    law = GuidanceLaw(GuidanceConfig())
+    # 目标正东、机头也朝东（yaw=90）→ 已对准，前进不偏航
+    out = law.update(_inp(tgt_lon=0.001, own_yaw=90.0), now_ms=1000)
+    assert out.vx_body_mps > 0.0
+    assert out.yaw_rate_dps == pytest.approx(0.0, abs=1e-6)
+
+
+def test_target_behind_no_forward_and_turns():
+    law = GuidanceLaw(GuidanceConfig())
+    out = law.update(_inp(tgt_lat=-0.001, own_yaw=0.0), now_ms=1000)  # 正南，机头朝北
+    assert out.vx_body_mps == pytest.approx(0.0, abs=1e-6)  # cos(180°)<0 门控为 0
+    assert abs(out.yaw_rate_dps) > 0.0
+
+
+# --- 到达 / 不可用 / 超龄 → 刹车 -------------------------------------------
+def test_arrived_within_stop_radius_brakes():
+    law = GuidanceLaw(GuidanceConfig(stop_radius_m=5.0))
+    near = 2.0 / _DEG_LAT_M  # ≈2m 纬度偏移
+    out = law.update(_inp(tgt_lat=near), now_ms=1000)
+    assert out.mode is ControlMode.BRAKE_HOVER
+    assert out.vx_body_mps == 0.0
+
+
+def test_stale_target_brakes():
+    law = GuidanceLaw(GuidanceConfig(target_stale_ms=1000))
+    out = law.update(_inp(tgt_lat=0.001, tgt_ms=1000), now_ms=5000)
+    assert out.mode is ControlMode.BRAKE_HOVER
+
+
+def test_invalid_position_brakes():
+    law = GuidanceLaw(GuidanceConfig())
+    out = law.update(_inp(tgt_lat=0.001, pos_valid=False), now_ms=1000)
+    assert out.mode is ControlMode.BRAKE_HOVER
+
+
+def test_invalid_target_brakes():
+    law = GuidanceLaw(GuidanceConfig())
+    out = law.update(_inp(tgt_lat=0.001, tgt_valid=False), now_ms=1000)
+    assert out.mode is ControlMode.BRAKE_HOVER
+
+
+# --- 速度上限与加速度限幅渐变 ----------------------------------------------
+def test_speed_ramps_up_and_clamps_to_max():
+    cfg = GuidanceConfig(frequency_hz=10.0, max_speed_mps=5.0,
+                         speed_slew_mps2=2.0, stop_radius_m=5.0)
+    law = GuidanceLaw(cfg)
+    forward = 0.0
+    now = 1000
+    for _ in range(60):  # 6s，足够加速度限幅爬到上限
+        out = law.update(_inp(tgt_lat=0.05, own_yaw=0.0, pos_ms=now, tgt_ms=now),
+                         now_ms=now)
+        forward = out.vx_body_mps
+        assert forward <= cfg.max_speed_mps + 1e-9
+        now += 100
+    assert forward == pytest.approx(cfg.max_speed_mps, abs=0.1)
+
+
+def test_first_command_is_slew_limited_not_full_speed():
+    law = GuidanceLaw(GuidanceConfig(frequency_hz=10.0, max_speed_mps=5.0,
+                                     speed_slew_mps2=2.0))
+    out = law.update(_inp(tgt_lat=0.05, own_yaw=0.0), now_ms=1000)
+    # 首拍从零渐变：dt=0.1s，加速度上限 2 → 前向 ≤ 0.2，远小于 5
+    assert 0.0 < out.vx_body_mps <= 0.2 + 1e-9
+
+
+def test_brake_then_recover_ramps_from_zero():
+    law = GuidanceLaw(GuidanceConfig(frequency_hz=10.0, speed_slew_mps2=2.0))
+    law.update(_inp(tgt_lat=0.05, own_yaw=0.0), now_ms=1000)   # 前进
+    law.update(_inp(tgt_lat=0.05, tgt_valid=False), now_ms=1100)  # 刹车，清状态
+    out = law.update(_inp(tgt_lat=0.05, own_yaw=0.0), now_ms=1200)  # 恢复
+    assert out.mode is ControlMode.VELOCITY_HEADING
+    assert out.vx_body_mps <= 0.2 + 1e-9  # 从零重新渐变
+
+
+def test_reset_clears_state():
+    law = GuidanceLaw(GuidanceConfig())
+    law.update(_inp(tgt_lat=0.05, own_yaw=0.0), now_ms=1000)
+    law.reset()
+    out = law.update(_inp(tgt_lat=0.05, own_yaw=0.0, pos_ms=2000, tgt_ms=2000),
+                     now_ms=2000)
+    assert out.vx_body_mps <= 0.2 + 1e-9  # 复位后首条仍从零渐变

utils/tests/pod_bridge/pod_serial_test.py

@@ -0,0 +1,89 @@
+"""pod_serial_test.py —— PodSerial 单元测试（注入 fake serial 模块，无硬件）。"""
+
+import sys
+import types
+
+import pytest
+
+from pod_bridge.pod_serial import PodLink, PodSerial
+from pod_bridge.protocol import TargetGeo, encode_target
+
+
+class _FakeSerialInstance:
+    def __init__(self, **kwargs):
+        self.kwargs = kwargs
+        self._buf = b""
+        self.written = []
+        self.closed = False
+
+    @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(data)
+
+    def close(self):
+        self.closed = True
+
+
+@pytest.fixture
+def fake_serial(monkeypatch):
+    mod = types.ModuleType("serial")
+    mod.EIGHTBITS = 8
+    mod.PARITY_NONE = "N"
+    mod.STOPBITS_ONE = 1
+    mod.Serial = _FakeSerialInstance
+    monkeypatch.setitem(sys.modules, "serial", mod)
+    return mod
+
+
+def test_empty_port_raises():
+    with pytest.raises(ValueError, match="port 为空"):
+        PodSerial("")
+
+
+def test_serial_opened_with_8n1(fake_serial):
+    ps = PodSerial("COM3", baudrate=115200)
+    kw = ps._ser.kwargs
+    assert kw["port"] == "COM3"
+    assert kw["baudrate"] == 115200
+    assert kw["bytesize"] == 8
+    assert kw["parity"] == "N"
+    assert kw["stopbits"] == 1
+    assert kw["timeout"] == 0  # 非阻塞轮询
+
+
+def test_poll_parses_buffered_frames(fake_serial):
+    ps = PodSerial("COM3")
+    ps._ser._buf = encode_target(TargetGeo(latitude_deg=31.0, longitude_deg=121.0))
+    frames = ps.poll()
+    assert len(frames) == 1
+    assert isinstance(frames[0], TargetGeo)
+    assert frames[0].latitude_deg == pytest.approx(31.0, abs=1e-7)
+
+
+def test_poll_empty_returns_empty_list(fake_serial):
+    ps = PodSerial("COM3")
+    assert ps.poll() == []
+
+
+def test_send_writes_bytes(fake_serial):
+    ps = PodSerial("COM3")
+    ps.send(b"\xeb\x90\x01")
+    assert ps._ser.written == [b"\xeb\x90\x01"]
+
+
+def test_close_closes_serial(fake_serial):
+    ps = PodSerial("COM3")
+    ps.close()
+    assert ps._ser.closed is True
+
+
+def test_satisfies_podlink_protocol(fake_serial):
+    ps = PodSerial("COM3")
+    assert isinstance(ps, PodLink)

utils/tests/pod_bridge/protocol_test.py

@@ -0,0 +1,222 @@
+"""protocol_test.py —— 光电吊舱协议编解码单元测试（规格：docs/光电吊舱控制协议G版-PB.docx）。
+
+覆盖：文档样例校验和、四帧 encode/decode 回环、量纲换算、遥测位域属性、
+分帧器（粘包/半帧/噪声前缀/坏校验恢复）、异常帧拒绝。
+"""
+
+import pytest
+
+from pod_bridge.protocol import (
+    CarrierAttitude,
+    ControlCode,
+    ControlCommand,
+    FrameParser,
+    Telemetry,
+    TargetGeo,
+    checksum,
+    decode_attitude,
+    decode_control,
+    decode_target,
+    decode_telemetry,
+    encode_attitude,
+    encode_control,
+    encode_target,
+    encode_telemetry,
+)
+
+
+# --- 校验和：文档第 589 行样例 ---------------------------------------------
+def test_checksum_matches_doc_example():
+    # 文档样例 eb 90 2b 00..00（15 字节 body）累加取低 8 位应为 0xa6
+    body = bytes.fromhex("eb902b000000000000000000000000")
+    assert len(body) == 15
+    assert checksum(body) == 0xA6
+
+
+def test_encode_control_center_matches_doc_bytes():
+    frame = encode_control(ControlCommand(code=ControlCode.CENTER))
+    assert frame == bytes.fromhex("eb902b000000000000000000000000a6")
+    assert len(frame) == 16
+
+
+# --- 表2-1 控制指令回环 -----------------------------------------------------
+def test_control_roundtrip_box_track():
+    cmd = ControlCommand(code=ControlCode.BOX_TRACK, param_x=-120, param_y=340,
+                         param3=2, file_save_mode=1, param_x2=500, param_y2=-600)
+    decoded = decode_control(encode_control(cmd))
+    assert decoded == cmd
+
+
+def test_control_signed_params_preserved():
+    cmd = ControlCommand(code=ControlCode.POINT_TRACK, param_x=-32768, param_y=32767)
+    decoded = decode_control(encode_control(cmd))
+    assert decoded.param_x == -32768
+    assert decoded.param_y == 32767
+
+
+# --- 表3 载机姿态回环（含量纲换算）----------------------------------------
+def test_attitude_roundtrip():
+    att = CarrierAttitude(
+        roll_deg=1.23, pitch_deg=-4.56, yaw_deg=179.99,
+        latitude_deg=31.2345678, longitude_deg=121.4737012,
+        abs_alt_m=123.4, rel_alt_m=56.8,
+        year=2026, month=9, day=22, hour=14, minute=30, second=15, centisecond=42,
+        airspeed_mps=15.5, ground_speed_mps=12.0,
+    )
+    d = decode_attitude(encode_attitude(att))
+    assert d.roll_deg == pytest.approx(1.23, abs=0.01)
+    assert d.pitch_deg == pytest.approx(-4.56, abs=0.01)
+    assert d.yaw_deg == pytest.approx(179.99, abs=0.01)
+    assert d.latitude_deg == pytest.approx(31.2345678, abs=1e-7)
+    assert d.longitude_deg == pytest.approx(121.4737012, abs=1e-7)
+    assert d.abs_alt_m == pytest.approx(123.4, abs=0.2)
+    assert d.rel_alt_m == pytest.approx(56.8, abs=0.2)
+    assert (d.year, d.month, d.day) == (2026, 9, 22)
+    assert (d.hour, d.minute, d.second, d.centisecond) == (14, 30, 15, 42)
+    assert d.airspeed_mps == pytest.approx(15.5, abs=0.5)
+    assert d.ground_speed_mps == pytest.approx(12.0, abs=0.5)
+
+
+def test_attitude_sync_and_length():
+    frame = encode_attitude(CarrierAttitude())
+    assert len(frame) == 32
+    assert frame[0] == 0xEB and frame[1] == 0x91
+
+
+# --- 表4 目标经纬度回环 -----------------------------------------------------
+def test_target_roundtrip():
+    tgt = TargetGeo(latitude_deg=31.2345678, longitude_deg=121.4737012,
+                    abs_alt_m=100.2, rel_alt_m=33.4,
+                    year=2026, month=9, day=22, hour=8, minute=5, second=30,
+                    centisecond=99)
+    d = decode_target(encode_target(tgt))
+    assert d.latitude_deg == pytest.approx(31.2345678, abs=1e-7)
+    assert d.longitude_deg == pytest.approx(121.4737012, abs=1e-7)
+    assert d.abs_alt_m == pytest.approx(100.2, abs=0.2)
+    assert d.rel_alt_m == pytest.approx(33.4, abs=0.2)
+    assert d.year == 2026 and d.centisecond == 99
+
+
+def test_target_negative_longitude():
+    tgt = TargetGeo(latitude_deg=-33.8688, longitude_deg=-70.6693)
+    d = decode_target(encode_target(tgt))
+    assert d.longitude_deg == pytest.approx(-70.6693, abs=1e-7)
+    assert d.latitude_deg == pytest.approx(-33.8688, abs=1e-7)
+
+
+# --- 表2-2 遥测：物理量 + 位域 ---------------------------------------------
+def test_telemetry_roundtrip_physical_values():
+    t = Telemetry(status1=0b10000100, status2=0b00101010, status3=0b01000000,
+                  off_target_x_deg=1.23, off_target_y_deg=-4.56,
+                  frame_roll_deg=10.0, frame_pitch_deg=-20.0, frame_yaw_deg=90.5,
+                  att_roll_deg=2.0, att_pitch_deg=-3.0,
+                  roll_rate_dps=1.5, pitch_rate_dps=-2.5, yaw_rate_dps=30.0,
+                  laser_range_m=123.4, self_check=0)
+    d = decode_telemetry(encode_telemetry(t))
+    assert d.off_target_x_deg == pytest.approx(1.23, abs=0.01)
+    assert d.off_target_y_deg == pytest.approx(-4.56, abs=0.01)
+    assert d.frame_yaw_deg == pytest.approx(90.5, abs=0.01)
+    assert d.yaw_rate_dps == pytest.approx(30.0, abs=0.01)
+    assert d.laser_range_m == pytest.approx(123.4, abs=0.1)
+
+
+def test_telemetry_laser_zero_is_none():
+    d = decode_telemetry(encode_telemetry(Telemetry(laser_range_m=None)))
+    assert d.laser_range_m is None
+
+
+def test_telemetry_bitfield_properties():
+    t = Telemetry(status1=0b11000100, status2=0b00101110, status3=0b01100000)
+    d = decode_telemetry(encode_telemetry(t))
+    assert d.track_source == 0b11          # 红外2
+    assert d.is_target_locked is True       # status1 Bit2 = 1
+    assert d.motor_on is True               # status2 Bit3 = 1
+    assert d.follow_on is True              # status2 Bit2 = 1
+    assert d.big_picture == 0b01
+    assert d.small_picture == 0b10
+
+
+# --- 异常帧拒绝 ------------------------------------------------------------
+def test_decode_rejects_bad_sync():
+    frame = bytearray(encode_control(ControlCommand(code=ControlCode.CENTER)))
+    frame[1] = 0x91  # 破坏同步字
+    with pytest.raises(ValueError, match="同步字"):
+        decode_control(bytes(frame))
+
+
+def test_decode_rejects_bad_checksum():
+    frame = bytearray(encode_target(TargetGeo(latitude_deg=10.0)))
+    frame[31] ^= 0xFF
+    with pytest.raises(ValueError, match="校验和"):
+        decode_target(bytes(frame))
+
+
+def test_decode_rejects_wrong_length():
+    with pytest.raises(ValueError, match="帧长"):
+        decode_telemetry(b"\xee\x16" + b"\x00" * 10)
+
+
+# --- 分帧器 ----------------------------------------------------------------
+def test_parser_single_frame():
+    parser = FrameParser()
+    frames = parser.feed(encode_target(TargetGeo(latitude_deg=31.0, longitude_deg=121.0)))
+    assert len(frames) == 1
+    assert isinstance(frames[0], TargetGeo)
+    assert frames[0].latitude_deg == pytest.approx(31.0, abs=1e-7)
+
+
+def test_parser_byte_by_byte_half_frame():
+    parser = FrameParser()
+    raw = encode_telemetry(Telemetry(laser_range_m=50.0))
+    collected = []
+    for i in range(len(raw)):
+        collected.extend(parser.feed(raw[i:i + 1]))
+    assert len(collected) == 1
+    assert collected[0].laser_range_m == pytest.approx(50.0, abs=0.1)
+
+
+def test_parser_concatenated_and_mixed_frames():
+    parser = FrameParser()
+    blob = (encode_telemetry(Telemetry(laser_range_m=10.0))
+            + encode_target(TargetGeo(latitude_deg=1.0))
+            + encode_telemetry(Telemetry(laser_range_m=20.0)))
+    frames = parser.feed(blob)
+    assert [type(f) for f in frames] == [Telemetry, TargetGeo, Telemetry]
+    assert frames[0].laser_range_m == pytest.approx(10.0, abs=0.1)
+    assert frames[2].laser_range_m == pytest.approx(20.0, abs=0.1)
+
+
+def test_parser_skips_noise_prefix():
+    parser = FrameParser()
+    blob = b"\x00\xff\x12\x34" + encode_target(TargetGeo(latitude_deg=5.0))
+    frames = parser.feed(blob)
+    assert len(frames) == 1
+    assert frames[0].latitude_deg == pytest.approx(5.0, abs=1e-7)
+
+
+def test_parser_recovers_from_bad_checksum():
+    parser = FrameParser()
+    bad = bytearray(encode_telemetry(Telemetry(laser_range_m=1.0)))
+    bad[31] ^= 0xFF  # 坏校验
+    good = encode_target(TargetGeo(latitude_deg=7.0))
+    frames = parser.feed(bytes(bad) + good)
+    # 坏帧被丢弃，紧随的好帧仍能解析
+    assert len(frames) == 1
+    assert isinstance(frames[0], TargetGeo)
+
+
+def test_parser_split_across_feeds():
+    parser = FrameParser()
+    raw = encode_target(TargetGeo(latitude_deg=42.0))
+    assert parser.feed(raw[:5]) == []      # 半帧，未输出
+    frames = parser.feed(raw[5:])
+    assert len(frames) == 1
+    assert frames[0].latitude_deg == pytest.approx(42.0, abs=1e-7)
+
+
+def test_parser_accepts_configured_kinds():
+    parser = FrameParser(kinds=((0xEB, 0x90),))  # 只识别控制帧（模拟器侧）
+    frames = parser.feed(encode_control(ControlCommand(code=ControlCode.CENTER)))
+    assert len(frames) == 1
+    assert isinstance(frames[0], ControlCommand)
+    assert frames[0].code == ControlCode.CENTER

utils/tests/pod_bridge/px4_gateway_test.py

@@ -0,0 +1,214 @@
+"""px4_gateway_test.py —— Px4Gateway 单元测试（mock 遥测流 + mock offboard）。
+
+覆盖：遥测缓存更新（position/attitude/gps/velocity）、position_valid 判据、
+快照副本隔离、时间戳注入、枚举转换、offboard 指令委托、异步订阅启停。
+"""
+
+import asyncio
+from enum import Enum
+from types import SimpleNamespace
+
+import pytest
+
+from pod_bridge.px4_gateway import OwnState, Px4Gateway, _enum_int
+
+
+class FakeOffboard:
+    def __init__(self):
+        self.body = []
+        self.ned = []
+        self.start_count = 0
+        self.stop_count = 0
+
+    async def set_velocity_body(self, sp):
+        self.body.append(sp)
+
+    async def set_velocity_ned(self, sp):
+        self.ned.append(sp)
+
+    async def start(self):
+        self.start_count += 1
+
+    async def stop(self):
+        self.stop_count += 1
+
+
+class FakeTelemetry:
+    """各遥测流：先吐预设消息，再挂起（由 stop_telemetry 取消）。"""
+
+    def __init__(self, position=None, attitude=None, gps=None, velocity=None):
+        self._position = position or []
+        self._attitude = attitude or []
+        self._gps = gps or []
+        self._velocity = velocity or []
+
+    async def _stream(self, msgs):
+        for m in msgs:
+            yield m
+        await asyncio.sleep(3600)
+
+    def position(self):
+        return self._stream(self._position)
+
+    def attitude_euler(self):
+        return self._stream(self._attitude)
+
+    def gps_info(self):
+        return self._stream(self._gps)
+
+    def velocity_body(self):
+        return self._stream(self._velocity)
+
+
+class FakeDrone:
+    def __init__(self, telemetry=None):
+        self.offboard = FakeOffboard()
+        self.telemetry = telemetry or FakeTelemetry()
+
+
+def _pos(lat=31.0, lon=121.0, aalt=100.0, ralt=50.0):
+    return SimpleNamespace(latitude_deg=lat, longitude_deg=lon,
+                           absolute_altitude_m=aalt, relative_altitude_m=ralt)
+
+
+def _att(roll=0.1, pitch=0.2, yaw=1.57):
+    return SimpleNamespace(roll_rad=roll, pitch_rad=pitch, yaw_rad=yaw)
+
+
+# --- 枚举转换 ---------------------------------------------------------------
+def test_enum_int_handles_enum_and_raw():
+    class Fix(Enum):
+        NO_FIX = 0
+        THREE_D = 3
+
+    assert _enum_int(Fix.THREE_D) == 3
+    assert _enum_int(3) == 3
+    assert _enum_int(None) == 0
+
+
+# --- 缓存更新 ---------------------------------------------------------------
+def test_update_position_sets_fields_but_not_valid_without_gps():
+    gw = Px4Gateway(FakeDrone())
+    gw.update_position(_pos())
+    s = gw.own_state()
+    assert s.latitude_deg == 31.0 and s.longitude_deg == 121.0
+    assert s.absolute_altitude_m == 100.0 and s.relative_altitude_m == 50.0
+    assert s.has_position is True
+    assert s.position_valid is False  # 尚无 GPS 定位
+
+
+def test_position_valid_requires_3d_fix():
+    gw = Px4Gateway(FakeDrone())
+    gw.update_position(_pos())
+    gw.update_gps(SimpleNamespace(fix_type=3, num_satellites=12))
+    s = gw.own_state()
+    assert s.position_valid is True
+    assert s.num_satellites == 12
+    # 定位降级 → 立即失效
+    gw.update_gps(SimpleNamespace(fix_type=2, num_satellites=6))
+    assert gw.own_state().position_valid is False
+
+
+def test_update_attitude_sets_radians():
+    gw = Px4Gateway(FakeDrone())
+    gw.update_attitude(_att())
+    s = gw.own_state()
+    assert s.has_attitude is True
+    assert s.yaw_rad == pytest.approx(1.57)
+
+
+def test_update_velocity_body_ground_speed_is_horizontal_magnitude():
+    gw = Px4Gateway(FakeDrone())
+    gw.update_velocity_body(SimpleNamespace(forward_m_s=3.0, right_m_s=4.0,
+                                            down_m_s=-9.9))
+    assert gw.own_state().ground_speed_mps == pytest.approx(5.0)  # 不含 down
+
+
+def test_timestamp_uses_injected_clock():
+    clock = [1000]
+    gw = Px4Gateway(FakeDrone(), now_ms_fn=lambda: clock[0])
+    gw.update_position(_pos())
+    assert gw.own_state().time_ms == 1000
+    assert gw.own_state().position_time_ms == 1000
+    clock[0] = 1500
+    gw.update_attitude(_att())
+    assert gw.own_state().time_ms == 1500
+    assert gw.own_state().position_time_ms == 1000  # 姿态流不得代刷位置时间戳
+
+
+def test_own_state_returns_isolated_copy():
+    gw = Px4Gateway(FakeDrone())
+    gw.update_position(_pos(lat=10.0))
+    snap = gw.own_state()
+    snap.latitude_deg = 999.0
+    assert gw.own_state().latitude_deg == 10.0  # 内部状态未被外部副本修改
+
+
+# --- offboard 指令委托 ------------------------------------------------------
+def test_command_delegation_reaches_offboard():
+    drone = FakeDrone()
+    gw = Px4Gateway(drone)
+
+    async def scenario():
+        assert await gw.start_offboard()
+        out = SimpleNamespace(yaw_rate_dps=5.0, vx_body_mps=1.5, vz_mps=0.0,
+                              vx_mps=0.0, vy_mps=0.0, yaw_deg=0.0)
+        assert await gw.send_velocity_heading(out, 1)
+        assert await gw.send_brake_hover(2)
+        await gw.stop_offboard()
+
+    asyncio.run(scenario())
+    assert drone.offboard.start_count == 1
+    assert len(drone.offboard.body) >= 2
+    assert drone.offboard.stop_count == 1
+
+
+# --- 异步订阅启停 -----------------------------------------------------------
+def test_start_telemetry_populates_cache_then_stop():
+    telemetry = FakeTelemetry(
+        position=[_pos(lat=22.0, lon=114.0)],
+        attitude=[_att(yaw=0.5)],
+        gps=[SimpleNamespace(fix_type=3, num_satellites=10)],
+        velocity=[SimpleNamespace(forward_m_s=2.0, right_m_s=0.0, down_m_s=0.0)],
+    )
+    gw = Px4Gateway(FakeDrone(telemetry))
+
+    async def scenario():
+        await gw.start_telemetry()
+        await asyncio.sleep(0.05)  # 让后台任务消费预设消息
+        s = gw.own_state()
+        assert s.latitude_deg == 22.0 and s.longitude_deg == 114.0
+        assert s.yaw_rad == pytest.approx(0.5)
+        assert s.position_valid is True
+        assert s.ground_speed_mps == pytest.approx(2.0)
+        await gw.stop_telemetry()
+        assert gw._tasks == []
+
+    asyncio.run(scenario())
+
+
+def test_default_own_state_is_invalid():
+    s = OwnState()
+    assert s.has_position is False and s.position_valid is False
+    assert s.time_ms == 0 and s.position_time_ms == 0
+
+
+
+def test_position_timestamp_not_refreshed_by_other_streams():
+    """缺陷①回归:position 流断更后,attitude/gps/velocity 高频流继续刷新
+    time_ms 时,position_time_ms 必须保持原值(位置超龄判定才有效)。"""
+    clock = [1000]
+    gw = Px4Gateway(FakeDrone(), now_ms_fn=lambda: clock[0])
+    gw.update_position(_pos())
+    assert gw.own_state().position_time_ms == 1000
+    clock[0] = 2000
+    gw.update_attitude(_att())
+    gw.update_gps(SimpleNamespace(fix_type=3, num_satellites=9))
+    gw.update_velocity_body(SimpleNamespace(forward_m_s=1.0, right_m_s=0.0,
+                                            down_m_s=0.0))
+    s = gw.own_state()
+    assert s.time_ms == 2000            # 诊断时间戳照常刷新
+    assert s.position_time_ms == 1000   # 位置时间戳停留 → 超龄判定可触发
+    clock[0] = 2500
+    gw.update_position(_pos(lat=32.0))  # position 流恢复 → 时间戳跟进
+    assert gw.own_state().position_time_ms == 2500