keita
df91b1110e
归档 legacy/：stereo_yolo/stereo_ranger/target_tracker 三包 + 175 条测试 + demo/配置/权重/三份规格；运行链路不再依赖，默认 pytest 446 条，归档测试按需运行。 目标有效性第二判据改为 LOS 运动学一致性（pod_bridge/los_consistency.py），删除 require_vision；pod_locked() 叠加该判据，状态机随之后退重搜。 自定义返航（task_runner/return_home.py）：保持转场高度回起飞点 → 恢复初始航向 → 垂直降落，不再爬 RTL_RETURN_ALT；未起飞/无起飞点/超时/异常仍退回 PX4 原生 RTL。 任务链安全兜底：主循环异常→紧急返航、stop 幂等、landing_stop 保留遥测、in_air 流复用+超时、解锁/起飞与吊舱识别仅执行一次、rtl_timeout 上限。 入口共享件拆到 utils/sitl_support.py；pod_sitl_test.py→l1_sitl_test.py；删除旧架构 test.py；相机脚本移入 tools/。 新增 .gitignore 并剔除历史误跟踪的 59 个 .pyc；requirements 按实际依赖重写；文档全面对齐，新增 legacy/README.md 与 tools/README.md。
259 lines
8.6 KiB
Python
259 lines
8.6 KiB
Python
"""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,
|
||
status=None):
|
||
self._position = position or []
|
||
self._attitude = attitude or []
|
||
self._gps = gps or []
|
||
self._velocity = velocity or []
|
||
self._status = status 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)
|
||
|
||
def status_text(self):
|
||
return self._stream(self._status)
|
||
|
||
|
||
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())
|
||
|
||
|
||
# --- 飞控状态文本（STATUSTEXT）----------------------------------------------
|
||
class Sev(Enum):
|
||
INFO = 1
|
||
WARNING = 2
|
||
|
||
|
||
def test_update_fc_status_and_take_messages():
|
||
gw = Px4Gateway(FakeDrone())
|
||
gw.update_fc_status(SimpleNamespace(type=Sev.WARNING, text="Low battery"))
|
||
gw.update_fc_status(SimpleNamespace(type="INFO", text="Takeoff detected"))
|
||
assert gw.take_fc_messages() == [("WARNING", "Low battery"),
|
||
("INFO", "Takeoff detected")]
|
||
assert gw.take_fc_messages() == [] # 取后清空
|
||
|
||
|
||
def test_status_text_subscription_captures_messages():
|
||
telemetry = FakeTelemetry(status=[
|
||
SimpleNamespace(type=Sev.WARNING, text="EKF variance")])
|
||
gw = Px4Gateway(FakeDrone(telemetry))
|
||
|
||
async def scenario():
|
||
await gw.start_telemetry()
|
||
await asyncio.sleep(0.05)
|
||
assert gw.take_fc_messages() == [("WARNING", "EKF variance")]
|
||
await gw.stop_telemetry()
|
||
|
||
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
|