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

- pod_bridge: 协议G版编解码/地理位置引导律/PX4网关/串口链路/闭环主循环 - 含三项安全修复: position_time_ms拆分、表2-2锁定门控、run()异常护栏 - 测试92条全绿; 附树莓派4B(Ubuntu 24.04)部署注意事项与最小依赖清单
2026-09-23 14:38:23 +08:00
parent 3a8e2c7f4b
commit 477ad5f5de
17 changed files with 2758 additions and 0 deletions
@@ -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          # 计数已复位