first commit

2026-09-02 15:13:13 +08:00
commit cb00322a50
107 changed files with 10918 additions and 0 deletions
@@ -0,0 +1,176 @@
+"""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