keita
477ad5f5de
- pod_bridge: 协议G版编解码/地理位置引导律/PX4网关/串口链路/闭环主循环 - 含三项安全修复: position_time_ms拆分、表2-2锁定门控、run()异常护栏 - 测试92条全绿; 附树莓派4B(Ubuntu 24.04)部署注意事项与最小依赖清单
170 lines
6.8 KiB
Python
170 lines
6.8 KiB
Python
"""px4_gateway.py —— PX4(MAVSDK) 侧网关：读飞控遥测 + 发 offboard 速度指令。
|
||
|
||
职责（桥接的飞控侧）：
|
||
* 订阅 PX4 遥测（position / attitude_euler / gps_info / velocity_body），缓存为
|
||
OwnState 快照，供桥接层构造 表3(载机姿态下发) 与 GuidanceInput(引导输入)；
|
||
* 复用 visual_track_control.MavsdkCommandGateway 发送 offboard 速度设定点
|
||
（不重造三分支映射，直接喂 GuidanceLaw 的 ControlOutput）。
|
||
|
||
设计：遥测拆为「纯缓存更新方法 update_*（无 IO，可单测）」+「薄异步订阅胶水
|
||
start_telemetry/_consume」，便于无硬件 mock 测试。指令发送整体委托给被组合的
|
||
MavsdkCommandGateway。
|
||
"""
|
||
|
||
import asyncio
|
||
import logging
|
||
import math
|
||
import time
|
||
from dataclasses import dataclass, replace
|
||
from typing import Any, Callable, Optional
|
||
|
||
from visual_track_control.mavsdk_gateway import MavsdkCommandGateway
|
||
|
||
logger = logging.getLogger(__name__)
|
||
|
||
_GPS_FIX_3D = 3 # mavsdk GpsFixType.THREE_D_FIX 起视为定位有效
|
||
|
||
|
||
def _enum_int(value: Any) -> int:
|
||
"""MAVSDK 枚举（Enum/IntEnum）或裸整数统一转 int。"""
|
||
if hasattr(value, "value"):
|
||
try:
|
||
return int(value.value)
|
||
except (TypeError, ValueError):
|
||
return 0
|
||
try:
|
||
return int(value)
|
||
except (TypeError, ValueError):
|
||
return 0
|
||
|
||
|
||
@dataclass
|
||
class OwnState:
|
||
"""PX4 遥测缓存快照（角度弧度 NED；位置度；高度/速度米制）。"""
|
||
|
||
latitude_deg: float = 0.0
|
||
longitude_deg: float = 0.0
|
||
absolute_altitude_m: float = 0.0
|
||
relative_altitude_m: float = 0.0
|
||
roll_rad: float = 0.0
|
||
pitch_rad: float = 0.0
|
||
yaw_rad: float = 0.0
|
||
ground_speed_mps: float = 0.0
|
||
fix_type: int = 0
|
||
num_satellites: int = 0
|
||
has_position: bool = False
|
||
has_attitude: bool = False
|
||
position_valid: bool = False # has_position 且 GPS 3D 定位
|
||
time_ms: int = 0 # 任意遥测流最近一次更新（毫秒），仅供诊断
|
||
position_time_ms: int = 0 # position 流最近一次更新（毫秒），位置超龄判定专用
|
||
|
||
|
||
class Px4Gateway:
|
||
"""drone：mavsdk System（duck-typed，便于注入 mock）。"""
|
||
|
||
def __init__(self, drone: Any,
|
||
now_ms_fn: Optional[Callable[[], int]] = None) -> None:
|
||
self._drone = drone
|
||
self._cmd = MavsdkCommandGateway(drone)
|
||
self._now_ms = now_ms_fn or (lambda: int(time.monotonic() * 1000))
|
||
self._state = OwnState()
|
||
self._tasks: list = []
|
||
|
||
# --- 连接（实机）-------------------------------------------------------
|
||
@classmethod
|
||
async def connect(cls, connection_string: str,
|
||
now_ms_fn: Optional[Callable[[], int]] = None) -> "Px4Gateway":
|
||
"""创建 MAVSDK System、连接并等待联通后返回网关。"""
|
||
from mavsdk import System
|
||
drone = System()
|
||
logger.info("[Px4Gateway] 连接飞控 %s ...", connection_string)
|
||
await drone.connect(system_address=connection_string)
|
||
async for state in drone.core.connection_state():
|
||
if state.is_connected:
|
||
logger.info("[Px4Gateway] 飞控已连接")
|
||
break
|
||
return cls(drone, now_ms_fn=now_ms_fn)
|
||
|
||
# --- 遥测缓存更新（纯方法，可单测）------------------------------------
|
||
def update_position(self, p: Any) -> None:
|
||
self._state.latitude_deg = float(p.latitude_deg)
|
||
self._state.longitude_deg = float(p.longitude_deg)
|
||
self._state.absolute_altitude_m = float(p.absolute_altitude_m)
|
||
self._state.relative_altitude_m = float(p.relative_altitude_m)
|
||
self._state.has_position = True
|
||
self._recompute_valid()
|
||
now = self._now_ms()
|
||
self._state.time_ms = now
|
||
self._state.position_time_ms = now # 仅此流刷新，姿态高频流不得代刷
|
||
|
||
def update_attitude(self, e: Any) -> None:
|
||
self._state.roll_rad = float(e.roll_rad)
|
||
self._state.pitch_rad = float(e.pitch_rad)
|
||
self._state.yaw_rad = float(e.yaw_rad)
|
||
self._state.has_attitude = True
|
||
self._state.time_ms = self._now_ms()
|
||
|
||
def update_gps(self, g: Any) -> None:
|
||
self._state.fix_type = _enum_int(getattr(g, "fix_type", 0))
|
||
self._state.num_satellites = _enum_int(getattr(g, "num_satellites", 0))
|
||
self._recompute_valid()
|
||
self._state.time_ms = self._now_ms()
|
||
|
||
def update_velocity_body(self, v: Any) -> None:
|
||
self._state.ground_speed_mps = math.hypot(
|
||
float(v.forward_m_s), float(v.right_m_s))
|
||
self._state.time_ms = self._now_ms()
|
||
|
||
def own_state(self) -> OwnState:
|
||
"""返回当前缓存的快照副本（调用方修改不影响内部状态）。"""
|
||
return replace(self._state)
|
||
|
||
def _recompute_valid(self) -> None:
|
||
self._state.position_valid = (self._state.has_position
|
||
and self._state.fix_type >= _GPS_FIX_3D)
|
||
|
||
# --- 遥测订阅（异步胶水）----------------------------------------------
|
||
async def start_telemetry(self) -> None:
|
||
"""订阅四路遥测流，后台任务持续刷新缓存。"""
|
||
t = self._drone.telemetry
|
||
self._tasks = [
|
||
asyncio.ensure_future(self._consume(t.position(), self.update_position)),
|
||
asyncio.ensure_future(
|
||
self._consume(t.attitude_euler(), self.update_attitude)),
|
||
asyncio.ensure_future(self._consume(t.gps_info(), self.update_gps)),
|
||
asyncio.ensure_future(
|
||
self._consume(t.velocity_body(), self.update_velocity_body)),
|
||
]
|
||
logger.info("[Px4Gateway] 遥测订阅已启动")
|
||
|
||
async def _consume(self, stream: Any, handler: Callable[[Any], None]) -> None:
|
||
try:
|
||
async for msg in stream:
|
||
handler(msg)
|
||
except asyncio.CancelledError:
|
||
raise
|
||
except Exception as exc:
|
||
logger.warning("[Px4Gateway] 遥测流中断: %s", exc)
|
||
|
||
async def stop_telemetry(self) -> None:
|
||
for task in self._tasks:
|
||
task.cancel()
|
||
if self._tasks:
|
||
await asyncio.gather(*self._tasks, return_exceptions=True)
|
||
self._tasks = []
|
||
logger.info("[Px4Gateway] 遥测订阅已停止")
|
||
|
||
# --- offboard 指令（委托 MavsdkCommandGateway）-------------------------
|
||
async def start_offboard(self) -> bool:
|
||
return await self._cmd.start()
|
||
|
||
async def send_velocity_heading(self, output: Any,
|
||
time_boot_ms: Optional[int] = None) -> bool:
|
||
return await self._cmd.send_velocity_heading(output, time_boot_ms)
|
||
|
||
async def send_brake_hover(self, time_boot_ms: Optional[int] = None) -> bool:
|
||
return await self._cmd.send_brake_hover(time_boot_ms)
|
||
|
||
async def stop_offboard(self) -> None:
|
||
await self._cmd.stop()
|