"""run_bridge.py —— 吊舱↔PX4 桥接闭环可执行入口（L1 航向伺服 / L2 地理引导）。 把 PodBridge 库串成能跑的进程：读配置 → 连接吊舱串口(+PX4) → 按控制频率闭环 → 优雅停机（先零速刹车再拆链路）。控制律选择由配置 heading_servo.enabled 决定 （true=L1 TrackingControlLaw 角度伺服；false=L2 GuidanceLaw 地理引导）。 两种模式： * --dry-run（S2 台架干跑）：**只接吊舱、不接飞控、不下发 offboard**。PX4 侧用 合成姿态桩（yaw=0、恒新鲜），每周期打印 mode / yaw_rate / vx_body / vz 与吊舱 脱靶量、框架角，用于验证 β→yaw_rate 的符号与量级。**电机不转、飞机不动。** * 实飞（S3+，无 --dry-run）：PodBridge.connect() 连真 PX4(MAVSDK) + 真吊舱串口， start() 起 offboard，run() 闭环，SIGINT/SIGTERM 或 --duration 到点后安全停机。 ⚠ 实飞前必须先在 --dry-run 下确认符号正确；不装桨 / 空旷场地 / RC 随时可切手动。 用法： # S2 干跑（先在 utils/config/pod_bridge.json 填 pod_serial.port，或用 --port 覆盖） python3 utils/pod_bridge/run_bridge.py --config utils/config/pod_bridge.json \\ --port /dev/ttyAMA0 --dry-run python3 utils/pod_bridge/run_bridge.py --dry-run --duration 30 # 定时结束 # S3+ 实飞（连真 PX4；连接串取配置 px4.connection_string） python3 utils/pod_bridge/run_bridge.py --config utils/config/pod_bridge.json """ import argparse import asyncio import logging import signal import sys import time from pathlib import Path from typing import Optional sys.path.insert(0, str(Path(__file__).resolve().parents[1])) # utils/ 入 sys.path from pod_bridge.bridge import PodBridge, _monotonic_ms # noqa: E402 from pod_bridge.config import load_from_json # noqa: E402 from pod_bridge.pod_serial import PodSerial # noqa: E402 from pod_bridge.px4_gateway import OwnState # noqa: E402 logger = logging.getLogger("run_bridge") _DEFAULT_CONFIG = str(Path(__file__).resolve().parents[1] / "config" / "pod_bridge.json") class DryRunPx4: """S2 干跑用的 PX4 桩：合成新鲜姿态，offboard 指令一律不下发（仅计数）。 接口对齐 Px4Gateway，故可直接注入 PodBridge。own_state() 每次调用都用当前 单调时钟打戳，保证控制律第①步姿态超龄校验恒通过（台架无真飞控也能跑闭环）。 """ def __init__(self, now_ms_fn=_monotonic_ms, yaw_rad: float = 0.0) -> None: self._now = now_ms_fn self._yaw = yaw_rad self.velocity_calls = 0 self.brake_calls = 0 def own_state(self) -> OwnState: t = self._now() return OwnState(has_attitude=True, has_position=True, position_valid=True, fix_type=3, num_satellites=12, yaw_rad=self._yaw, time_ms=t, position_time_ms=t) async def start_telemetry(self) -> None: pass async def stop_telemetry(self) -> None: pass async def start_offboard(self) -> bool: return True async def stop_offboard(self) -> None: pass async def send_velocity_heading(self, out, time_boot_ms=None) -> bool: self.velocity_calls += 1 # 干跑：只计数，绝不发 offboard return True async def send_brake_hover(self, time_boot_ms=None) -> bool: self.brake_calls += 1 return True def _format_status(now_s: float, out, tel_pair, locked: bool) -> str: """把控制输出 + 吊舱源角度拼成一行（S2 符号/量级判读用）。""" if tel_pair is not None: frame, _ = tel_pair off = f"off=({frame.off_target_x_deg:+.2f},{frame.off_target_y_deg:+.2f})°" fyaw = f"frame_yaw={frame.frame_yaw_deg:+.2f}°" lock = f"locked={int(frame.is_target_locked)}" else: off, fyaw, lock = "off=(无遥测)", "frame_yaw=-", "locked=-" return (f"[dry] t={now_s:6.1f}s {out.mode.name:<16} " f"yaw_rate={out.yaw_rate_dps:+7.2f}dps vx_body={out.vx_body_mps:+.2f} " f"vz={out.vz_mps:+.2f} | {off} {fyaw} {lock}") async def _run_dry(cfg, args) -> int: port = args.port or cfg.pod_serial.port if not port: logger.error("干跑需要吊舱串口：在配置填 pod_serial.port 或用 --port 指定") return 2 pod = PodSerial(port, cfg.pod_serial.baudrate) px4 = DryRunPx4(yaw_rad=0.0) bridge = PodBridge(cfg, px4, pod, now_ms_fn=_monotonic_ms) period_s = 1.0 / bridge._active_frequency_hz() print_period_s = 1.0 / args.print_hz if args.print_hz > 0 else 0.0 law = "L1 航向伺服(TrackingControlLaw)" if cfg.heading_servo.enabled \ else "L2 地理引导(GuidanceLaw)" logger.info("[干跑] %s @ %.1fHz | 吊舱 %s | offboard 不下发（飞机不会动）", law, 1.0 / period_s, port) t0 = time.monotonic() last_print = -1.0 failures = 0 try: while True: now_s = time.monotonic() - t0 if args.duration > 0 and now_s >= args.duration: break now_ms = _monotonic_ms() try: out = await bridge.step(now_ms) failures = 0 if now_s - last_print >= print_period_s: last_print = now_s print(_format_status(now_s, out, bridge.latest_telemetry, bridge._pod_locked(now_ms))) except OSError as exc: failures += 1 if failures == 1 or failures % 100 == 0: logger.warning("[干跑] 吊舱串口异常(第 %d 次): %s", failures, exc) await asyncio.sleep(period_s) except KeyboardInterrupt: pass finally: try: pod.close() except OSError: pass logger.info("[干跑] 结束：velocity 拍 %d，brake 拍 %d", px4.velocity_calls, px4.brake_calls) return 0 async def _run_live(cfg, args) -> int: bridge = await PodBridge.connect(cfg) # 连真 PX4(MAVSDK) + 真吊舱串口 stop = asyncio.Event() loop = asyncio.get_running_loop() for sig in (signal.SIGINT, signal.SIGTERM): try: loop.add_signal_handler(sig, stop.set) except (NotImplementedError, RuntimeError): pass # 平台不支持（如 Windows）→ 靠 KeyboardInterrupt 兜底 if not await bridge.start(): logger.error("offboard 启动失败，未进入闭环") await bridge.stop() return 1 logger.info("[实飞] 闭环运行中；Ctrl+C 或 --duration 到点后安全停机") runner = asyncio.ensure_future(bridge.run(stop)) try: if args.duration > 0: await asyncio.sleep(args.duration) stop.set() await runner except KeyboardInterrupt: stop.set() runner.cancel() finally: stop.set() await bridge.stop() # 先零速刹车，再拆 offboard/遥测/串口 return 0 async def _amain(args) -> int: cfg = load_from_json(args.config) return await (_run_dry(cfg, args) if args.dry_run else _run_live(cfg, args)) def main(argv: Optional[list] = None) -> int: ap = argparse.ArgumentParser(description="吊舱↔PX4 桥接闭环入口（L1/L2）") ap.add_argument("--config", default=_DEFAULT_CONFIG, help="桥接配置 JSON 路径") ap.add_argument("--port", help="覆盖配置里的吊舱串口设备号（如 /dev/ttyAMA0）") ap.add_argument("--dry-run", action="store_true", help="S2 干跑：只接吊舱、不接飞控、不下发 offboard，仅打印") ap.add_argument("--duration", type=float, default=0.0, help="运行时长(秒)；0=一直跑到 Ctrl+C") ap.add_argument("--print-hz", type=float, default=5.0, help="干跑打印频率(秒级节流)；0=每拍都打") ap.add_argument("--verbose", action="store_true", help="打印 DEBUG 日志") args = ap.parse_args(argv) logging.basicConfig( level=logging.DEBUG if args.verbose else logging.INFO, format="%(asctime)s %(levelname)s %(message)s", datefmt="%H:%M:%S") try: return asyncio.run(_amain(args)) except KeyboardInterrupt: return 0 if __name__ == "__main__": sys.exit(main())