实现L1航向伺服(路线A1): 吊舱角度直入控制律 + 桥接胶水 + 干跑入口
- visual_track_control: ControlSnapshot 加可选 los_beta_deg/los_alpha_deg, 第④步角度直入优先否则回退像素路径(向后兼容); config 拆出 load_from_dict 供内嵌复用 - pod_bridge: BridgeConfig 内嵌 TrackingConfig + 新 HeadingServoConfig; bridge._build_snapshot 合成 β/α(姿态来自PX4/角度来自吊舱,信号双路适配无电机), step 按 heading_servo.enabled 切 L1/L2, DEGRADED_HOLD 补发刹车保 offboard 心跳; run() 循环频率按生效律取(修 L1 频率错配) - run_bridge.py: 闭环 CLI 入口, --dry-run(S2 只接吊舱不发offboard) 与实飞优雅停机 - 配置落 L1 首飞值(slow_approach/0.2mps、垂直压低、无电机仅用脱靶量) - 新增测试 29 条(vtc5+config9+bridgeL1 10+runner5); 全量 401 passed
This commit is contained in:
keita
@@ -0,0 +1,209 @@
|
||||
"""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())
|
||||
Reference in New Issue
Block a user