code-test-drone/utils/tests/pod_bridge/px4_gateway_test.py

"""px4_gateway_test.py —— Px4Gateway 单元测试（mock 遥测流 + mock offboard）。

覆盖：遥测缓存更新（position/attitude/gps/velocity）、position_valid 判据、
快照副本隔离、时间戳注入、枚举转换、offboard 指令委托、异步订阅启停。
"""

import asyncio
import math
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_ned(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_deg=5.0, pitch_deg=10.0, yaw_deg=90.0):
    """MAVSDK EulerAngle 字段是度（roll_deg/pitch_deg/yaw_deg），网关转弧度。"""
    return SimpleNamespace(roll_deg=roll_deg, pitch_deg=pitch_deg, yaw_deg=yaw_deg)


# --- 枚举转换 ---------------------------------------------------------------
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.roll_rad == pytest.approx(math.radians(5.0))
    assert s.pitch_rad == pytest.approx(math.radians(10.0))
    assert s.yaw_rad == pytest.approx(math.radians(90.0))


def test_update_velocity_ned_ground_speed_is_horizontal_magnitude():
    gw = Px4Gateway(FakeDrone())
    gw.update_velocity_ned(SimpleNamespace(north_m_s=3.0, east_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_deg=30.0)],
        gps=[SimpleNamespace(fix_type=3, num_satellites=10)],
        velocity=[SimpleNamespace(north_m_s=2.0, east_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(math.radians(30.0))
        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_ned(SimpleNamespace(north_m_s=1.0, east_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