"""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 # 计数已复位