"""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.tracking_control_law import TrackingControlLaw from visual_track_control.types import (ControlMode, ControlSnapshot, TrackState) from .config import BridgeConfig from .ground_link import GroundTarget 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, vision_target_fn=None, ground_target_fn=None) -> None: self._cfg = cfg self._px4 = px4 self._pod = pod self._now_ms = now_ms_fn self._vision_target_fn = vision_target_fn # §2.3 第二判据（可选，S3+ 接入我方视觉） self._ground_target_fn = ground_target_fn # 地面注入坐标访问器（A2，无激光替代表4） self._use_heading_servo = cfg.heading_servo.enabled # 运行时控制律选择（A3 状态机切换） self._guidance = GuidanceLaw(cfg.guidance) self._tracking = TrackingControlLaw(cfg.tracking) # L1 航向伺服（方案 §0） 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() self._tracking.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._active_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) def _active_frequency_hz(self) -> float: """当前生效控制律的循环频率：L1 用 tracking.control，L2 用 guidance。""" if self._use_heading_servo: return self._cfg.tracking.control.frequency_hz return self._cfg.guidance.frequency_hz def set_heading_servo_enabled(self, enabled: bool) -> None: """运行时切换控制律（任务状态机 GOTO(L2) ↔ TRACK(L1) 切换，A3）。 切换时 reset 即将启用的律（清积分/限幅器历史），使指令从零渐变， 避免切换瞬间输出跳变。L1 用吊舱角度直入，L2 用地理引导（注入坐标/表4）。 """ if enabled == self._use_heading_servo: return self._use_heading_servo = enabled if enabled: self._tracking.reset() else: self._guidance.reset() async def step(self, now_ms: int): """单控制周期（显式传入时钟，便于确定性单测）。返回 ControlOutput。 按运行时选择的控制律在 L1（TrackingControlLaw，吊舱角度直入）与 L2（GuidanceLaw，表4/注入坐标地理引导）间切换（方案 §7）。 """ self._ingest_pod_frames(now_ms) own = self._px4.own_state() self._downlink_attitude(own, now_ms) if self._use_heading_servo: out = self._tracking.update(self._build_snapshot(own, now_ms), now_ms) else: 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: injected = self._injected_target(now_ms) if injected is not None: # 地面注入坐标优先（本吊舱无激光 → 表4 永不出，目标坐标改走地面站） 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=injected.latitude_deg, target_lon_deg=injected.longitude_deg, target_valid=True, # 注入坐标已验证新鲜 target_time_ms=injected.time_ms, ) # 回退表4（有激光吊舱 / 未接入地面站） 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 _injected_target(self, now_ms: int) -> Optional[GroundTarget]: """地面注入坐标（新鲜有效则返回，否则 None → 回退表4 / 无目标）。 本吊舱无激光 → 表4 永不出，目标坐标改由地面站注入（预定流程「地面给坐标」）。 未启用 / 未注入 / 超龄 → None（下游按无目标刹车）。""" if not self._cfg.ground_inject.enabled: return None if self._ground_target_fn is None: return None target = self._ground_target_fn() if target is None or target.time_ms <= 0: return None if is_stale(target.time_ms, now_ms, self._cfg.ground_inject.timeout_ms): return None return target def pod_locked(self, now_ms: int) -> bool: """吊舱目标门控（公共接口，供编排层/状态机复用，A3）。""" return self._pod_locked(now_ms) def _pod_locked(self, now_ms: int) -> bool: """吊舱目标门控：表2-2 遥测在 link.telemetry_timeout_ms 内、状态字节1 bit2=锁定、且（若 link.require_laser_valid）激光测距有效，才算目标有效。 实测依据：部分吊舱目标丢失后**锁定位置位不复位**（脱靶量也没有无效标志位， 恒为 0），仅凭 bit2 会朝过期坐标继续飞；激光无效时吊舱不解算目标经纬度， 故此兜底。无激光或激光恒无效的吊舱把 require_laser_valid 置 False。 从未收到遥测 / 遥测超龄 / 搜索状态 / 激光无效 一律视为目标无效（宁可刹车）。""" 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 if not frame.is_target_locked: return False if self._cfg.link.require_laser_valid and frame.laser_range_m is None: return False return True def _build_snapshot(self, own, now_ms: int) -> ControlSnapshot: """L1：PX4 姿态 + 吊舱表2-2 角度 → ControlSnapshot（角度直入，方案 §2.1/§2.3）。 姿态（yaw / attitude_present / attitude_time_ms）来自 PX4；视线角 β/α 来自 吊舱脱靶量与框架角，信源与符号由 heading_servo 配置合成： β = beta_sign·(beta_from_frame·frame_yaw + beta_from_off_target·off_target_x) 无电机吊舱 frame_yaw 恒 0（框架角无用），仅脱靶量生效；装电机后开启框架角项。 L1 无距离：distance_valid=False，控制律按 no_distance_action 降级（只修正方向）。 """ hs = self._cfg.heading_servo tel = self._latest_telemetry frame, t_ms = tel if tel is not None else (None, 0) beta_deg = 0.0 alpha_deg = 0.0 if frame is not None: if hs.beta_from_frame: beta_deg += frame.frame_yaw_deg if hs.beta_from_off_target: beta_deg += frame.off_target_x_deg if hs.alpha_from_frame: alpha_deg += frame.frame_pitch_deg if hs.alpha_from_off_target: alpha_deg += frame.off_target_y_deg beta_deg *= hs.beta_sign alpha_deg *= hs.alpha_sign if not hs.vertical_enabled: alpha_deg = 0.0 tracked = self._pod_locked(now_ms) # §2.3：require_vision 时需我方视觉作第二判据（bit2 粘滞兜底）。未接入视觉源 # 时 fail-safe 为 False（无法确认 → 宁可刹车）。 if tracked and hs.require_vision: tracked = self._vision_target_fn() if self._vision_target_fn else False return ControlSnapshot( track=TrackState(tracked=tracked), track_time_ms=t_ms, # 无遥测(t_ms=0) → 控制律判视觉超龄 → 降级 los_beta_deg=beta_deg, los_alpha_deg=alpha_deg, distance_valid=False, # L1 无距离（L2 换 stereo_ranger.forward_distance_m） attitude_present=own.has_attitude, attitude_time_ms=own.time_ms, yaw_rad=own.yaw_rad, ) 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) elif out.mode is ControlMode.DEGRADED_HOLD: # L1 控制律超龄/退出会降级（valid=False）：仍补发零速刹车， # 保住 offboard 心跳（否则 PX4 数秒内触发失联保护）。 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)