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。
122 lines
5.5 KiB
Python
122 lines
5.5 KiB
Python
"""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 要求先流式发送首条设定点再请求启动。
|
||
|
||
幂等：已启动（或重入）时**不重发** offboard.start()，避免任务状态机重入
|
||
动作（丢目标后回 SEARCH/参考点）反复重启 offboard 造成模式抖动。
|
||
"""
|
||
if self._started:
|
||
return True
|
||
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_yaw_rate(self, yaw_rate_dps: float,
|
||
forward_mps: float = 0.0,
|
||
down_mps: float = 0.0) -> bool:
|
||
"""原地偏航扫视：零前向/侧向速度 + 指定偏航角速度（SEARCH 态扫视用）。
|
||
|
||
与 send_brake_hover 同类，但带角速度，故机头缓慢旋转、位置保持——
|
||
吊舱相机刚性固连机体，转弯即改变观察方向，以此覆盖参考点周边。
|
||
异常一律捕获返回 False（与其它指令一致，不向外抛）。
|
||
"""
|
||
try:
|
||
await self._offboard.set_velocity_body(
|
||
self._velocity_body(forward_mps, 0.0, down_mps, yaw_rate_dps))
|
||
except Exception as exc:
|
||
logger.warning("[MavsdkGateway] 扫视指令发送失败: %s", exc)
|
||
return False
|
||
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)
|