177 lines
5.7 KiB
Python
177 lines
5.7 KiB
Python
"""mavsdk_gateway_test.py —— MavsdkCommandGateway 单元测试（mock offboard）。
|
||
|
||
移植自 reference-c++/visual_track_control/tests/mavlink_command_gateway_test.cpp。
|
||
C++ 版校验 MAVLink ID 84 线格式字节流；Python 版采用 MAVSDK 语义映射，
|
||
改为以 FakeOffboard 捕获设定点对象，校验三分支映射（物理追踪思路 §2.2、
|
||
A2 掩码三分支的语义等价）：
|
||
|
||
角速率非零 → 机体速度设定点 + 非零 yawspeed；
|
||
航向角非零（角速率为零）→ NED 速度设定点 + 绝对 yaw；
|
||
两者均零 → 机体速度设定点 + 零 yawspeed（保持航向，A2 回归语义）；
|
||
刹车 → 全零机体速度设定点。
|
||
传输失败（异常）→ 返回 False 且不向外抛。
|
||
"""
|
||
|
||
import asyncio
|
||
|
||
import pytest
|
||
|
||
from visual_track_control.mavsdk_gateway import MavsdkCommandGateway
|
||
from visual_track_control.types import ControlOutput
|
||
|
||
|
||
class FakeOffboard:
|
||
"""捕获式 offboard mock：记录每次调用的设定点，可注入失败。"""
|
||
|
||
def __init__(self):
|
||
self.body_setpoints = []
|
||
self.ned_setpoints = []
|
||
self.attempts = 0
|
||
self.start_count = 0
|
||
self.stop_count = 0
|
||
self.succeed = True
|
||
|
||
async def set_velocity_body(self, setpoint):
|
||
self.attempts += 1 # 与 C++ CaptureTransport 一致：失败也计入发送次数
|
||
if not self.succeed:
|
||
raise RuntimeError("mock transport failure")
|
||
self.body_setpoints.append(setpoint)
|
||
|
||
async def set_velocity_ned(self, setpoint):
|
||
self.attempts += 1
|
||
if not self.succeed:
|
||
raise RuntimeError("mock transport failure")
|
||
self.ned_setpoints.append(setpoint)
|
||
|
||
async def start(self):
|
||
if not self.succeed:
|
||
raise RuntimeError("mock start failure")
|
||
self.start_count += 1
|
||
|
||
async def stop(self):
|
||
self.stop_count += 1
|
||
|
||
@property
|
||
def send_count(self):
|
||
return self.attempts
|
||
|
||
|
||
class FakeDrone:
|
||
def __init__(self):
|
||
self.offboard = FakeOffboard()
|
||
|
||
|
||
def run(coro):
|
||
return asyncio.run(coro)
|
||
|
||
|
||
def make_gateway():
|
||
drone = FakeDrone()
|
||
return MavsdkCommandGateway(drone), drone.offboard
|
||
|
||
|
||
def test_start_sends_initial_setpoint_then_starts():
|
||
gateway, offboard = make_gateway()
|
||
assert run(gateway.start())
|
||
assert offboard.start_count == 1
|
||
# PX4 要求先流式发送首条设定点（全零）再请求启动
|
||
assert len(offboard.body_setpoints) == 1
|
||
sp = offboard.body_setpoints[0]
|
||
assert sp.forward_m_s == 0.0
|
||
assert sp.right_m_s == 0.0
|
||
assert sp.down_m_s == 0.0
|
||
assert sp.yawspeed_deg_s == 0.0
|
||
|
||
|
||
def test_start_failure_returns_false():
|
||
gateway, offboard = make_gateway()
|
||
offboard.succeed = False
|
||
assert not run(gateway.start())
|
||
|
||
|
||
def _make_velocity_heading_output():
|
||
from visual_track_control.types import ControlMode
|
||
out = ControlOutput()
|
||
out.mode = ControlMode.VELOCITY_HEADING
|
||
return out
|
||
|
||
|
||
def test_yaw_rate_branch_uses_body_frame_with_yawspeed():
|
||
from visual_track_control.types import ControlMode
|
||
gateway, offboard = make_gateway()
|
||
out = _make_velocity_heading_output()
|
||
out.vx_body_mps = 1.5
|
||
out.vz_mps = 0.25
|
||
out.yaw_rate_dps = 5.0
|
||
assert run(gateway.send_velocity_heading(out, 12345))
|
||
assert offboard.send_count == 1
|
||
assert len(offboard.body_setpoints) == 1
|
||
sp = offboard.body_setpoints[0]
|
||
assert sp.forward_m_s == pytest.approx(1.5)
|
||
assert sp.right_m_s == 0.0
|
||
assert sp.down_m_s == pytest.approx(0.25)
|
||
assert sp.yawspeed_deg_s == pytest.approx(5.0)
|
||
assert offboard.ned_setpoints == []
|
||
|
||
|
||
def test_yaw_only_branch_uses_ned_frame_with_absolute_yaw():
|
||
# 航向角非零且角速率为零 → 航向掩码语义：NED 速度 + 绝对航向
|
||
gateway, offboard = make_gateway()
|
||
out = _make_velocity_heading_output()
|
||
out.vx_mps = 2.0
|
||
out.vy_mps = -1.0
|
||
out.vz_mps = 0.5
|
||
out.yaw_deg = 10.0
|
||
out.yaw_rate_dps = 0.0
|
||
assert run(gateway.send_velocity_heading(out, 1))
|
||
assert offboard.send_count == 1
|
||
assert len(offboard.ned_setpoints) == 1
|
||
sp = offboard.ned_setpoints[0]
|
||
assert sp.north_m_s == pytest.approx(2.0)
|
||
assert sp.east_m_s == pytest.approx(-1.0)
|
||
assert sp.down_m_s == pytest.approx(0.5)
|
||
assert sp.yaw_deg == pytest.approx(10.0)
|
||
assert offboard.body_setpoints == []
|
||
|
||
|
||
def test_both_zero_keeps_heading_via_body_zero_yawspeed():
|
||
# A2 回归：两者均零时机体速度设定点配零角速率 = 保持当前航向，
|
||
# 不得误发绝对航向 0（正北）。
|
||
gateway, offboard = make_gateway()
|
||
out = _make_velocity_heading_output()
|
||
out.vx_body_mps = 1.0
|
||
out.yaw_rate_dps = 0.0
|
||
out.yaw_deg = 0.0
|
||
assert run(gateway.send_velocity_heading(out, 1))
|
||
assert len(offboard.body_setpoints) == 1
|
||
sp = offboard.body_setpoints[0]
|
||
assert sp.forward_m_s == pytest.approx(1.0)
|
||
assert sp.yawspeed_deg_s == 0.0
|
||
assert offboard.ned_setpoints == [] # 不得走绝对航向支路
|
||
|
||
|
||
def test_brake_hover_sends_zero_velocity_body():
|
||
gateway, offboard = make_gateway()
|
||
assert run(gateway.send_brake_hover(777))
|
||
assert offboard.send_count == 1
|
||
sp = offboard.body_setpoints[0]
|
||
assert sp.forward_m_s == 0.0
|
||
assert sp.right_m_s == 0.0
|
||
assert sp.down_m_s == 0.0
|
||
assert sp.yawspeed_deg_s == 0.0
|
||
|
||
|
||
def test_transport_failure_propagates_as_false():
|
||
gateway, offboard = make_gateway()
|
||
offboard.succeed = False
|
||
out = _make_velocity_heading_output()
|
||
assert not run(gateway.send_velocity_heading(out, 1))
|
||
assert not run(gateway.send_brake_hover(1))
|
||
assert offboard.send_count == 2
|
||
|
||
|
||
def test_stop_calls_offboard_stop():
|
||
gateway, offboard = make_gateway()
|
||
run(gateway.stop())
|
||
assert offboard.stop_count == 1
|