first commit

2026-09-02 15:13:13 +08:00
commit cb00322a50
107 changed files with 10918 additions and 0 deletions
@@ -0,0 +1,98 @@
+"""MAVSDK offboard 网关 —— 控制意图 → MAVSDK 设定点（语义映射）。
+
+对应 C++ 参考实现 reference-c++/visual_track_control/src/mavlink_command_gateway.cpp
+（C++ 版手工编码 MAVLink ID 84 字节流；Python 版改用 MAVSDK offboard API，
+按 A2 语义映射三分支）：
+
+  速率式（yaw_rate_dps ≠ 0）→ set_velocity_body(机体前向, 0, vz, yaw_rate)
+  位置式（yaw_deg ≠ 0 且角速率为零）→ set_velocity_ned(NED 速度 + 绝对 yaw)
+  两者均零 → set_velocity_body 配零角速率（保持当前航向，A2 回归语义）
+  刹车 → set_velocity_body(0, 0, 0, 0)（MAVSDK 无刹车掩码的行为等效适配）
+
+降级保持（DEGRADED_HOLD）不发送任何指令，由调用方（胶水层）判断模式。
+MAVSDK Python API 为 asyncio 异步接口，本网关方法均为协程；传输异常
+捕获后返回 False（与 C++ 传输失败传播一致），不向外抛异常。
+"""
+
+import logging
+from typing import Any, Optional
+
+from .types import ControlOutput
+
+logger = logging.getLogger(__name__)
+
+
+class MavsdkCommandGateway:
+    """drone：mavsdk System（仅取 .offboard 属性，duck-typed 便于注入 mock）。"""
+
+    def __init__(self, drone: Any) -> None:
+        self._offboard = drone.offboard
+        self._started = False
+
+    async def start(self) -> bool:
+        """进入 offboard：PX4 要求先流式发送首条设定点再请求启动。"""
+        try:
+            await self._offboard.set_velocity_body(
+                self._velocity_body(0.0, 0.0, 0.0, 0.0))
+            await self._offboard.start()
+        except Exception as exc:  # OffboardError / 连接异常统一视为启动失败
+            logger.warning("[MavsdkGateway] offboard 启动失败: %s", exc)
+            return False
+        self._started = True
+        logger.info("[MavsdkGateway] offboard 已启动")
+        return True
+
+    async def send_velocity_heading(self, output: ControlOutput,
+                                    time_boot_ms: Optional[int] = None) -> bool:
+        try:
+            if output.yaw_rate_dps != 0.0:
+                # 速率式：机体系速度 + 偏航角速度（NED 分量不再使用）
+                await self._offboard.set_velocity_body(
+                    self._velocity_body(output.vx_body_mps, 0.0, output.vz_mps,
+                                        output.yaw_rate_dps))
+            elif output.yaw_deg != 0.0:
+                # 位置式：NED 速度 + 绝对航向（A2：绝对航向合成在控制律完成）
+                await self._offboard.set_velocity_ned(
+                    self._velocity_ned(output.vx_mps, output.vy_mps,
+                                       output.vz_mps, output.yaw_deg))
+            else:
+                # 两者均零：机体系速度 + 零角速率 = 保持当前航向（A2 回归语义）
+                await self._offboard.set_velocity_body(
+                    self._velocity_body(output.vx_body_mps, 0.0, output.vz_mps,
+                                        0.0))
+        except Exception as exc:
+            logger.warning("[MavsdkGateway] 速度航向指令发送失败: %s", exc)
+            return False
+        return True
+
+    async def send_brake_hover(self, time_boot_ms: Optional[int] = None) -> bool:
+        try:
+            await self._offboard.set_velocity_body(
+                self._velocity_body(0.0, 0.0, 0.0, 0.0))
+        except Exception as exc:
+            logger.warning("[MavsdkGateway] 刹车指令发送失败: %s", exc)
+            return False
+        return True
+
+    async def stop(self) -> None:
+        try:
+            await self._offboard.stop()
+        except Exception as exc:
+            logger.warning("[MavsdkGateway] offboard 停止失败: %s", exc)
+        self._started = False
+        logger.info("[MavsdkGateway] offboard 已停止")
+
+    @staticmethod
+    def _velocity_body(forward: float, right: float, down: float,
+                       yawspeed: float) -> Any:
+        from mavsdk import offboard as _mavsdk_offboard
+        return _mavsdk_offboard.VelocityBodyYawspeed(
+            forward_m_s=forward, right_m_s=right, down_m_s=down,
+            yawspeed_deg_s=yawspeed)
+
+    @staticmethod
+    def _velocity_ned(north: float, east: float, down: float,
+                      yaw: float) -> Any:
+        from mavsdk import offboard as _mavsdk_offboard
+        return _mavsdk_offboard.VelocityNedYaw(
+            north_m_s=north, east_m_s=east, down_m_s=down, yaw_deg=yaw)