"""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 @pytest.fixture(autouse=True) def _stub_mavsdk(fake_mavsdk): """本模块全部用例与真实 libcmavsdk.so 脱钩(树莓派原生库符号缺失实测)。""" return fake_mavsdk 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