新增pod_bridge吊舱桥接中间件(树莓派4B部署)
- pod_bridge: 协议G版编解码/地理位置引导律/PX4网关/串口链路/闭环主循环 - 含三项安全修复: position_time_ms拆分、表2-2锁定门控、run()异常护栏 - 测试92条全绿; 附树莓派4B(Ubuntu 24.04)部署注意事项与最小依赖清单
This commit is contained in:
keita
@@ -0,0 +1,207 @@
|
||||
"""bridge.py —— 吊舱 ↔ PX4 桥接主循环（asyncio 胶水层）。
|
||||
|
||||
闭环（每控制周期，guidance.frequency_hz）：
|
||||
① poll 吊舱串口 → 缓存最新 表4 目标经纬度 / 表2-2 遥测（带单调时间戳）
|
||||
② 按 link.attitude_downlink_hz 节流，读 PX4 遥测 → 打包 表3 载机姿态下发吊舱
|
||||
（吊舱据此解算目标经纬度，文档要求 1~60Hz）
|
||||
③ 目标门控：表2-2 状态字节1 bit2=锁定 且遥测在 link.telemetry_timeout_ms 内，
|
||||
表4 坐标才视为有效（规格：表4 周期发送，搜索状态下坐标无意义；激光无效时
|
||||
吊舱不解算、表4 自然中断，由 guidance.target_stale_ms 兜底）
|
||||
④ 自身状态 + 最新目标 → GuidanceInput → GuidanceLaw.update → ControlOutput
|
||||
⑤ 按模式分发：VELOCITY_HEADING → offboard 速度；BRAKE_HOVER → 零速刹车
|
||||
（引导律对一切不安全情形输出 BRAKE_HOVER，故 offboard 始终有心跳，无失联悬停）
|
||||
|
||||
run() 主循环带异常护栏：单周期异常（如串口拔线）按节流节奏告警并补发零速刹车
|
||||
保 offboard 心跳，任务不终止。
|
||||
|
||||
时钟一致性：Px4Gateway 与 PodBridge 必须共用同一单调毫秒时钟（OwnState.time_ms
|
||||
与 step(now_ms) 同源），否则 position/target 超龄判定失效。connect() 工厂已统一。
|
||||
"""
|
||||
|
||||
import asyncio
|
||||
import logging
|
||||
import math
|
||||
import time
|
||||
from datetime import datetime, timezone
|
||||
from typing import Optional
|
||||
|
||||
from visual_track_control.staleness_checker import is_stale
|
||||
from visual_track_control.types import ControlMode
|
||||
|
||||
from .config import BridgeConfig
|
||||
from .guidance import GuidanceInput, GuidanceLaw
|
||||
from .pod_serial import PodLink, PodSerial
|
||||
from .protocol import (CarrierAttitude, ControlCommand, TargetGeo, Telemetry,
|
||||
encode_attitude, encode_control)
|
||||
from .px4_gateway import Px4Gateway
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
def _monotonic_ms() -> int:
|
||||
return int(time.monotonic() * 1000)
|
||||
|
||||
|
||||
class PodBridge:
|
||||
"""px4：Px4Gateway（或等价 mock）；pod：PodLink（PodSerial 或内存 FakePod）。"""
|
||||
|
||||
def __init__(self, cfg: BridgeConfig, px4: Px4Gateway, pod: PodLink,
|
||||
now_ms_fn=_monotonic_ms) -> None:
|
||||
self._cfg = cfg
|
||||
self._px4 = px4
|
||||
self._pod = pod
|
||||
self._now_ms = now_ms_fn
|
||||
self._guidance = GuidanceLaw(cfg.guidance)
|
||||
self._attitude_period_ms = max(
|
||||
1, int(round(1000.0 / cfg.link.attitude_downlink_hz)))
|
||||
self._last_attitude_ms: Optional[int] = None
|
||||
self._latest_target = None # (TargetGeo, time_ms)
|
||||
self._latest_telemetry = None # (Telemetry, time_ms)
|
||||
self._step_failures = 0 # run() 连续异常计数（告警节流用）
|
||||
|
||||
# --- 生命周期 -----------------------------------------------------------
|
||||
async def start(self) -> bool:
|
||||
"""启动 PX4 遥测订阅与 offboard；offboard 失败返回 False（不应继续闭环）。"""
|
||||
await self._px4.start_telemetry()
|
||||
if not await self._px4.start_offboard():
|
||||
logger.error("[Bridge] offboard 启动失败，桥接未运行")
|
||||
return False
|
||||
logger.info("[Bridge] 启动：引导 %.1fHz，姿态下发 %.1fHz",
|
||||
self._cfg.guidance.frequency_hz,
|
||||
self._cfg.link.attitude_downlink_hz)
|
||||
return True
|
||||
|
||||
async def stop(self) -> None:
|
||||
"""安全停机：先零速刹车，再停 offboard / 遥测 / 串口。"""
|
||||
try:
|
||||
await self._px4.send_brake_hover(self._now_ms())
|
||||
finally:
|
||||
await self._px4.stop_offboard()
|
||||
await self._px4.stop_telemetry()
|
||||
self._pod.close()
|
||||
self._guidance.reset()
|
||||
logger.info("[Bridge] 已停止")
|
||||
|
||||
# --- 主循环 -------------------------------------------------------------
|
||||
async def run(self, stop_event: asyncio.Event) -> None:
|
||||
"""按引导频率循环 step()，直到 stop_event 置位。
|
||||
|
||||
异常护栏：单周期抛错（典型：吊舱串口拔线 SerialException/OSError）不终止
|
||||
任务，按「第 1 次 + 每满 100 次」节奏告警（对齐 WarnThrottle 语义），并补发
|
||||
零速刹车保住 offboard 心跳——否则 PX4 数秒内触发 offboard 失联保护。
|
||||
CancelledError 继承 BaseException，不在此拦截。
|
||||
"""
|
||||
period_s = 1.0 / self._cfg.guidance.frequency_hz
|
||||
while not stop_event.is_set():
|
||||
try:
|
||||
await self.step(self._now_ms())
|
||||
self._step_failures = 0
|
||||
except Exception as exc:
|
||||
self._step_failures += 1
|
||||
if self._step_failures == 1 or self._step_failures % 100 == 0:
|
||||
logger.warning("[Bridge] 控制周期异常(第 %d 次): %s",
|
||||
self._step_failures, exc)
|
||||
try:
|
||||
await self._px4.send_brake_hover(self._now_ms())
|
||||
except Exception:
|
||||
pass # 补发失败不掩盖原异常，下周期重试
|
||||
await asyncio.sleep(period_s)
|
||||
|
||||
async def step(self, now_ms: int):
|
||||
"""单控制周期（显式传入时钟，便于确定性单测）。返回 ControlOutput。"""
|
||||
self._ingest_pod_frames(now_ms)
|
||||
|
||||
own = self._px4.own_state()
|
||||
self._downlink_attitude(own, now_ms)
|
||||
|
||||
out = self._guidance.update(self._build_guidance_input(own, now_ms), now_ms)
|
||||
await self._dispatch(out, now_ms)
|
||||
return out
|
||||
|
||||
# --- 吊舱侧 -------------------------------------------------------------
|
||||
def _ingest_pod_frames(self, now_ms: int) -> None:
|
||||
for frame in self._pod.poll():
|
||||
if isinstance(frame, TargetGeo):
|
||||
self._latest_target = (frame, now_ms)
|
||||
elif isinstance(frame, Telemetry):
|
||||
self._latest_telemetry = (frame, now_ms)
|
||||
|
||||
def _downlink_attitude(self, own, now_ms: int) -> None:
|
||||
if (self._last_attitude_ms is not None
|
||||
and now_ms - self._last_attitude_ms < self._attitude_period_ms):
|
||||
return
|
||||
self._pod.send(encode_attitude(self._build_attitude(own)))
|
||||
self._last_attitude_ms = now_ms
|
||||
|
||||
def _build_attitude(self, own) -> CarrierAttitude:
|
||||
t = datetime.now(timezone.utc)
|
||||
return CarrierAttitude(
|
||||
roll_deg=math.degrees(own.roll_rad),
|
||||
pitch_deg=math.degrees(own.pitch_rad),
|
||||
yaw_deg=math.degrees(own.yaw_rad),
|
||||
latitude_deg=own.latitude_deg,
|
||||
longitude_deg=own.longitude_deg,
|
||||
abs_alt_m=own.absolute_altitude_m,
|
||||
rel_alt_m=own.relative_altitude_m,
|
||||
year=t.year, month=t.month, day=t.day,
|
||||
hour=t.hour, minute=t.minute, second=t.second,
|
||||
centisecond=t.microsecond // 10000,
|
||||
airspeed_mps=0.0, # 无空速管，置 0（文档允许）
|
||||
ground_speed_mps=own.ground_speed_mps,
|
||||
)
|
||||
|
||||
def send_control(self, cmd: ControlCommand) -> None:
|
||||
"""触发式下发 表2-1 控制指令（如开目标识别 / 框选跟踪）。"""
|
||||
self._pod.send(encode_control(cmd))
|
||||
|
||||
# --- 飞控侧 -------------------------------------------------------------
|
||||
def _build_guidance_input(self, own, now_ms: int) -> GuidanceInput:
|
||||
tgt = self._latest_target
|
||||
locked = self._pod_locked(now_ms)
|
||||
return GuidanceInput(
|
||||
own_lat_deg=own.latitude_deg,
|
||||
own_lon_deg=own.longitude_deg,
|
||||
own_yaw_deg=math.degrees(own.yaw_rad),
|
||||
own_position_valid=own.position_valid,
|
||||
own_position_time_ms=own.position_time_ms, # position 流专用时间戳
|
||||
target_lat_deg=tgt[0].latitude_deg if tgt else 0.0,
|
||||
target_lon_deg=tgt[0].longitude_deg if tgt else 0.0,
|
||||
target_valid=tgt is not None and locked, # 未锁定=目标无效
|
||||
target_time_ms=tgt[1] if tgt else 0,
|
||||
)
|
||||
|
||||
def _pod_locked(self, now_ms: int) -> bool:
|
||||
"""吊舱锁定门控：表2-2 遥测在 link.telemetry_timeout_ms 内且状态字节1
|
||||
bit2=锁定 才算锁定。从未收到遥测 / 遥测超龄 / 搜索状态一律视为未锁定
|
||||
（宁可刹车，不朝搜索状态下的无意义坐标飞）。"""
|
||||
tel = self._latest_telemetry
|
||||
if tel is None:
|
||||
return False
|
||||
frame, t_ms = tel
|
||||
if is_stale(t_ms, now_ms, self._cfg.link.telemetry_timeout_ms):
|
||||
return False
|
||||
return frame.is_target_locked
|
||||
|
||||
async def _dispatch(self, out, now_ms: int) -> None:
|
||||
if out.mode is ControlMode.VELOCITY_HEADING and out.valid:
|
||||
await self._px4.send_velocity_heading(out, now_ms)
|
||||
elif out.mode is ControlMode.BRAKE_HOVER and out.valid:
|
||||
await self._px4.send_brake_hover(now_ms)
|
||||
|
||||
# --- 诊断访问 -----------------------------------------------------------
|
||||
@property
|
||||
def latest_target(self):
|
||||
return self._latest_target
|
||||
|
||||
@property
|
||||
def latest_telemetry(self):
|
||||
return self._latest_telemetry
|
||||
|
||||
# --- 实机工厂 -----------------------------------------------------------
|
||||
@classmethod
|
||||
async def connect(cls, cfg: BridgeConfig) -> "PodBridge":
|
||||
"""按配置连接 PX4(MAVSDK) 与吊舱(串口)，共用单一单调时钟。"""
|
||||
px4 = await Px4Gateway.connect(cfg.px4.connection_string,
|
||||
now_ms_fn=_monotonic_ms)
|
||||
pod = PodSerial(cfg.pod_serial.port, cfg.pod_serial.baudrate)
|
||||
return cls(cfg, px4, pod, now_ms_fn=_monotonic_ms)
|
||||
Reference in New Issue
Block a user