"""bridge_test.py —— PodBridge 闭环主循环单元测试（FakePx4 + FakePod，无硬件）。 覆盖：单周期 step（有/无目标、位置无效、位置时间戳超龄）、姿态下发节流与帧内容、 吊舱帧入站缓存、锁定门控（表2-2 bit2 + 遥测超龄）、触发式控制指令编码、 start/stop 生命周期与停机刹车、run() 主循环异常护栏与故障恢复。 """ import asyncio import time from dataclasses import replace import pytest from pod_bridge.bridge import PodBridge from pod_bridge.config import BridgeConfig, load_from_json from pod_bridge.ground_link import GroundTarget 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(laser=50.0): """状态字节1 bit2=1(锁定)的遥测帧;默认带有效激光(默认闸门要求激光有效)。""" return Telemetry(status1=0b00000100, laser_range_m=laser) 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" # --- 激光有效性门控(锁定位置位粘滞时的兜底)----------------------------------- def test_laser_invalid_blocks_guidance_when_required(): """默认 require_laser_valid=True:锁定位置位但激光无效 → 目标无效 → 刹车。""" px4 = FakePx4(_valid_state(1000)) pod = FakePod() pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry(laser=None)) b = _bridge(px4, pod) out = asyncio.run(b.step(1000)) assert out.mode.name == "BRAKE_HOVER" assert px4.velocity_calls == [] def test_laser_invalid_allowed_when_gate_disabled(): """本设备无激光(厂商确认)→ require_laser_valid=False 时照常引导。""" px4 = FakePx4(_valid_state(1000)) pod = FakePod() pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry(laser=None)) cfg = BridgeConfig() cfg.link.require_laser_valid = False b = _bridge(px4, pod, cfg) out = asyncio.run(b.step(1000)) assert out.mode.name == "VELOCITY_HEADING" def test_deployed_config_disables_laser_gate(): """部署配置回归:本机无激光,pod_bridge.json 必须关掉激光闸门。""" import os path = os.path.join(os.path.dirname(os.path.dirname(os.path.dirname( os.path.abspath(__file__)))), "config", "pod_bridge.json") cfg = load_from_json(path) assert cfg.link.require_laser_valid is False # --- 缺陷③: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 # 计数已复位 # --- L1 航向伺服（方案 §2，heading_servo.enabled=True）----------------------- def _l1_config(**hs_overrides): """L1 配置：控制律限幅近似透明，前向置 0，隔离验证 β→yaw_rate / α→vz 数学。""" cfg = BridgeConfig() cfg.heading_servo.enabled = True for k, v in hs_overrides.items(): setattr(cfg.heading_servo, k, v) t = cfg.tracking t.heading.gain = 1.0 t.heading.yaw_rate_limit_dps = 30.0 t.heading.yaw_slew_limit_dps2 = 100000.0 t.vertical.vz_gain_mps_per_deg = 0.1 t.vertical.vz_limit_mps = 2.0 t.accel_limit.ax_mps2 = 1000.0 t.accel_limit.ay_mps2 = 1000.0 t.accel_limit.az_mps2 = 1000.0 t.distance.no_distance_approach_limit_mps = 0.0 # L1 前向隔离为 0 return cfg def _locked_off_target(x=0.0, y=0.0, frame_yaw=0.0, frame_pitch=0.0, laser=50.0): """bit2=锁定 + 有效激光，带指定脱靶量/框架角的遥测帧。""" return Telemetry(status1=0b00000100, laser_range_m=laser, off_target_x_deg=x, off_target_y_deg=y, frame_yaw_deg=frame_yaw, frame_pitch_deg=frame_pitch) def test_l1_off_target_beta_drives_yaw_rate(): px4 = FakePx4(_valid_state(1000)) pod = FakePod() pod.queue(_locked_off_target(x=10.0)) # 目标偏右 10° → 右转正 b = _bridge(px4, pod, _l1_config()) out = asyncio.run(b.step(1000)) assert out.mode.name == "VELOCITY_HEADING" assert out.yaw_rate_dps == pytest.approx(10.0, abs=1e-9) assert len(px4.velocity_calls) == 1 def test_l1_beta_sign_flips_direction(): px4 = FakePx4(_valid_state(1000)) pod = FakePod() pod.queue(_locked_off_target(x=10.0)) b = _bridge(px4, pod, _l1_config(beta_sign=-1)) # 台架标定若接反，配置翻符号 out = asyncio.run(b.step(1000)) assert out.yaw_rate_dps == pytest.approx(-10.0, abs=1e-9) def test_l1_frame_angle_ignored_when_disabled(): # 无电机：beta_from_frame=False → frame_yaw 不贡献，仅脱靶量生效 px4 = FakePx4(_valid_state(1000)) pod = FakePod() pod.queue(_locked_off_target(x=5.0, frame_yaw=99.0)) b = _bridge(px4, pod, _l1_config()) out = asyncio.run(b.step(1000)) assert out.yaw_rate_dps == pytest.approx(5.0, abs=1e-9) def test_l1_frame_angle_added_when_enabled(): # 装电机后：beta_from_frame=True → β = frame_yaw + off_target_x px4 = FakePx4(_valid_state(1000)) pod = FakePod() pod.queue(_locked_off_target(x=5.0, frame_yaw=3.0)) b = _bridge(px4, pod, _l1_config(beta_from_frame=True)) out = asyncio.run(b.step(1000)) assert out.yaw_rate_dps == pytest.approx(8.0, abs=1e-9) def test_l1_vertical_enabled_drives_vz(): px4 = FakePx4(_valid_state(1000)) pod = FakePod() pod.queue(_locked_off_target(y=20.0)) # α=20° × gain 0.1 = 2.0（上限 2.0） b = _bridge(px4, pod, _l1_config()) out = asyncio.run(b.step(1000)) assert out.vz_mps == pytest.approx(2.0, abs=1e-9) def test_l1_vertical_disabled_zeroes_vz(): px4 = FakePx4(_valid_state(1000)) pod = FakePod() pod.queue(_locked_off_target(y=20.0)) b = _bridge(px4, pod, _l1_config(vertical_enabled=False)) out = asyncio.run(b.step(1000)) assert out.vz_mps == 0.0 def test_l1_not_locked_brakes(): px4 = FakePx4(_valid_state(1000)) pod = FakePod() pod.queue(Telemetry(status1=0, off_target_x_deg=10.0)) # 搜索态 b = _bridge(px4, pod, _l1_config()) out = asyncio.run(b.step(1000)) assert out.mode.name == "BRAKE_HOVER" assert px4.velocity_calls == [] assert len(px4.brake_calls) == 1 def test_l1_no_telemetry_degrades_and_keeps_heartbeat(): # 无表2-2 → track_time_ms=0 → 控制律判视觉超龄 → DEGRADED_HOLD；仍补发刹车保心跳 px4 = FakePx4(_valid_state(1000)) pod = FakePod() b = _bridge(px4, pod, _l1_config()) out = asyncio.run(b.step(1000)) assert out.mode.name == "DEGRADED_HOLD" assert px4.velocity_calls == [] assert len(px4.brake_calls) == 1 def test_l1_require_vision_blocks_without_source(): # §2.3 第二判据：require_vision=True 但未接视觉源 → fail-safe 视为无目标 → 刹车 px4 = FakePx4(_valid_state(1000)) pod = FakePod() pod.queue(_locked_off_target(x=10.0)) b = _bridge(px4, pod, _l1_config(require_vision=True)) out = asyncio.run(b.step(1000)) assert out.mode.name == "BRAKE_HOVER" assert px4.velocity_calls == [] def test_l1_require_vision_passes_with_source(): px4 = FakePx4(_valid_state(1000)) pod = FakePod() pod.queue(_locked_off_target(x=10.0)) b = PodBridge(_l1_config(require_vision=True), px4, pod, now_ms_fn=lambda: 0, vision_target_fn=lambda: True) out = asyncio.run(b.step(1000)) assert out.mode.name == "VELOCITY_HEADING" assert out.yaw_rate_dps == pytest.approx(10.0, abs=1e-9) # --- 地面坐标注入（A2）------------------------------------------------------- def _inject_config(timeout_ms=5000): cfg = BridgeConfig() cfg.ground_inject.enabled = True cfg.ground_inject.timeout_ms = timeout_ms return cfg def test_ground_inject_priority_over_table4(): """注入坐标优先于表4：吊舱无表4/无锁定（本设备常态），注入新鲜坐标照常引导。""" px4 = FakePx4(_valid_state(1000)) pod = FakePod() injected = GroundTarget(latitude_deg=0.001, longitude_deg=0.0, time_ms=1000) b = PodBridge(_inject_config(), px4, pod, now_ms_fn=lambda: 0, ground_target_fn=lambda: injected) out = asyncio.run(b.step(1000)) assert out.mode.name == "VELOCITY_HEADING" assert len(px4.velocity_calls) == 1 def test_ground_inject_stale_falls_back_to_brake(): """注入坐标超龄 → 视为无目标 → 刹车（不得朝过期坐标飞）。""" px4 = FakePx4(_valid_state(2000)) pod = FakePod() injected = GroundTarget(latitude_deg=0.001, longitude_deg=0.0, time_ms=1000) # 1000ms 前注入 cfg = _inject_config(timeout_ms=500) b = PodBridge(cfg, px4, pod, now_ms_fn=lambda: 0, ground_target_fn=lambda: injected) out = asyncio.run(b.step(2000)) # 差值 1000ms > 500ms → 超龄 assert out.mode.name == "BRAKE_HOVER" assert px4.velocity_calls == [] def test_ground_inject_disabled_ignores_injection(): """未启用地面注入 → 忽略注入坐标，回退表4 原路径（无表4 → 刹车）。""" px4 = FakePx4(_valid_state(1000)) pod = FakePod() injected = GroundTarget(latitude_deg=0.001, longitude_deg=0.0, time_ms=1000) cfg = BridgeConfig() # ground_inject.enabled=False b = PodBridge(cfg, px4, pod, now_ms_fn=lambda: 0, ground_target_fn=lambda: injected) out = asyncio.run(b.step(1000)) assert out.mode.name == "BRAKE_HOVER" # --- 运行时切换控制律（A3：GOTO(L2) ↔ TRACK(L1)）----------------------------- def test_set_heading_servo_enabled_runtime_switch(): """运行时切换控制律：初始 L1 → 切 L2（刹车）→ 切回 L1（速度）。""" px4 = FakePx4(_valid_state(1000)) pod = FakePod() b = _bridge(px4, pod, _l1_config()) # enabled=True → 初始 L1 # 初始 L1：锁定 + 脱靶量 → 速度 pod.queue(_locked_off_target(x=10.0)) out = asyncio.run(b.step(1000)) assert out.mode.name == "VELOCITY_HEADING" assert out.yaw_rate_dps == pytest.approx(10.0, abs=1e-9) # 切 L2：无表4/无锁定 → 刹车 b.set_heading_servo_enabled(False) out = asyncio.run(b.step(1100)) assert out.mode.name == "BRAKE_HOVER" # 切回 L1：重新喂锁定脱靶量 → 速度 b.set_heading_servo_enabled(True) pod.queue(_locked_off_target(x=10.0)) out = asyncio.run(b.step(1200)) assert out.mode.name == "VELOCITY_HEADING" assert out.yaw_rate_dps == pytest.approx(10.0, abs=1e-9)