#!/usr/bin/env python3 """task_runner_real.py —— 真机任务编排入口（真实吊舱串口 + 地面站 UDP 注入 + PX4）。 与 task_runner_sitl.py（仿真）的差异： * 吊舱 = PodSerial（真实串口表2-2 遥测），非 VirtualPod； * 地面坐标 = GroundLink.run()（UDP 接收地面站 JSON），非 inject() 直接注入； * 吊舱目标识别/锁定 = PodController（首次进入引导阶段发 0x07 识别 + 0x0F 自动锁定， RTL 前发 0x0E 停止 + 0x08 识别关；**只发一次**，重入不会重发打断跟踪）； * PX4 连接 = cfg.px4.connection_string（真机串口/遥测），非 SITL 14540； * 收尾 = RTL 返航（默认）或 `--land` 就地降落； * SIGINT/SIGTERM 优雅停机：先请状态机转 RTL，再退出（不把飞机留在 offboard 上）。 任务链（与 SITL 一致）：WAIT→ARM_TAKEOFF→GOTO(飞向参考点)→SEARCH(原地扫视)→TRACK(缓慢靠近)→RTL 前置（真机联调增量：纯软单测 → 串口回环 → 仅吊舱 → 仅飞控 → 地面联调）： 1. 填 utils/config/pod_bridge.json 的 pod_serial.port / px4.connection_string； 2. 吊舱上电、串口连通（run_bridge.py --dry-run 已验证遥测）； 3. 地面站往 ground_inject.port（默认 14555）**持续**发 JSON 坐标 {"lat":..,"lon":..,"alt":..}（每 0.5~1s 一次）；停发会导致坐标超龄 → 返航。 用法： python task_runner_real.py python task_runner_real.py --config utils/config/pod_bridge.json --target-type 1 python task_runner_real.py --port /dev/ttyAMA0 --land """ import argparse import asyncio import logging import signal import sys import time from datetime import datetime from pathlib import Path _ROOT = Path(__file__).resolve().parent _UTILS = _ROOT / "utils" for _p in (_UTILS, _ROOT): if str(_p) not in sys.path: sys.path.insert(0, str(_p)) from mavsdk import System from pod_bridge.bridge import PodBridge from pod_bridge.config import load_from_json from pod_bridge.ground_link import GroundLink from pod_bridge.pod_serial import PodSerial from pod_bridge.px4_gateway import Px4Gateway from task_runner.log import TaskLogConfig, TaskLogger from task_runner.pod_control import PodController from task_runner.return_home import ReturnConfig from task_runner.runner import TaskRunner from task_runner.state_machine import TaskStateConfig, TaskStateMachine from sitl_support import now_ms, print_flight_mode, set_telemetry_rates, wait_until_ready logger = logging.getLogger("task_real") CONFIG_PATH = _UTILS / "config" / "pod_bridge.json" TASK_TIMEOUT_S = 300.0 # 全流程总超时（真机放宽） def build_sm_config() -> TaskStateConfig: """真机状态机参数。⚠ 全部为占位值，须台架标定后回填，标定前不得实飞。""" return TaskStateConfig( takeoff_altitude_m=5.0, arrival_radius_m=30.0, # 到参考点半径 → 转搜索（待标定） goto_timeout_s=180.0, search_timeout_s=20.0, # 搜索 20s 未发现目标 → 返航 track_timeout_s=20.0, # 追踪/靠近段上限 → 返航（无距离，判不出"已到目标"） center_threshold_deg=3.0, # 仅日志/诊断 sweep_rate_dps=15.0, # 搜索扫视角速度（度/秒，待标定） sweep_reverse_s=4.0, # 每 4s 换向 loop_frequency_hz=20.0, ) def build_return_config() -> ReturnConfig: """自定义返航参数：回起飞点 → 恢复初始航向 → 垂直降落（不额外爬高）。 ⚠ 速度为占位值，须按机体/场地标定。 """ return ReturnConfig( enabled=True, speed_mps=2.0, # 转场水平速度 arrival_radius_m=3.0, # 距起飞点该半径内视为到位 timeout_s=120.0, # 转场超时 → PX4 RTL 兜底 yaw_kp=1.0, yaw_rate_limit_dps=20.0, align_kp=1.0, align_rate_limit_dps=15.0, yaw_align_tolerance_deg=5.0, yaw_align_timeout_s=15.0, alt_kp=0.5, alt_limit_mps=0.5, ) def _sync_staleness_with_injection(cfg) -> None: """把「坐标时效」口径统一到 `ground_inject.timeout_ms`。 ⚠ 这是 SITL 实测暴露的一个真机必踩的坑：`guidance.target_stale_ms` 默认 1000ms， 而地面站按 0.5~1s 上报 → 报文间隔稍有抖动（无线链路/页面节流）就会让引导律判 「目标超龄 → 零速刹车」，表现为飞机在路上走走停停；而任务层要等 `ground_inject.timeout_ms`（默认 5000ms）才判坐标失效。 SITL 入口是硬编码 5000ms 绕过的，真机这里改为**派生**，两个门限只有一个来源。 """ inject_ms = int(cfg.ground_inject.timeout_ms) if cfg.guidance.target_stale_ms < inject_ms: logger.info("[Task] guidance.target_stale_ms %d → %d（与 ground_inject 对齐）", cfg.guidance.target_stale_ms, inject_ms) cfg.guidance.target_stale_ms = inject_ms def _install_signal_handlers(stop: asyncio.Event, runner: TaskRunner) -> None: """SIGINT/SIGTERM → 请求返航并退出主循环（真机在树莓派上被 kill 也要安全收尾）。 只置位不抛异常：让 `runner.run()` 走正常的 RTL 分支（停机+返航+落地+上锁）， 而不是让进程带着 offboard 速度指令消失。 """ loop = asyncio.get_running_loop() def _handler(sig_name: str) -> None: logger.warning("[Task] 收到 %s：请求返航并退出", sig_name) runner.request_rtl() stop.set() for name in ("SIGINT", "SIGTERM"): sig = getattr(signal, name, None) if sig is None: continue try: loop.add_signal_handler(sig, _handler, name) except (NotImplementedError, RuntimeError, ValueError): # Windows / 非主线程不支持：退化为 signal.signal（可能不生效） try: signal.signal(sig, lambda *_a, _n=name: _handler(_n)) except (ValueError, OSError): pass async def run(args) -> int: cfg = load_from_json(args.config) if args.port: cfg.pod_serial.port = args.port # 先应用覆盖，再校验（否则 --port 救不了空配置） if not cfg.pod_serial.port: print("! 未配置吊舱串口：在 pod_bridge.json 填 pod_serial.port，或用 --port 覆盖") return 2 cfg.ground_inject.enabled = True # 地面注入必须开（真机无激光，表4 永不出） _sync_staleness_with_injection(cfg) # 坐标时效口径统一（SITL 实测踩过的坑） sm_cfg = build_sm_config() return_cfg = build_return_config() drone = System() await drone.connect(system_address=args.conn or cfg.px4.connection_string) try: await wait_until_ready(drone) except RuntimeError as e: print(f"! {e}") return 1 await set_telemetry_rates(drone) # 装配：真吊舱串口 + 地面站 UDP 注入 + 吊舱识别/锁定控制 px4 = Px4Gateway(drone) ground = GroundLink(cfg.ground_inject, now_ms_fn=now_ms) pod = PodSerial(cfg.pod_serial.port, cfg.pod_serial.baudrate) bridge = PodBridge(cfg, px4, pod, now_ms_fn=now_ms, ground_target_fn=lambda: ground.latest()) sm = TaskStateMachine(sm_cfg) pod_control = PodController(bridge, target_type=args.target_type) log_dir = args.log_dir or f"logs/real_{datetime.now().strftime('%Y%m%d_%H%M%S')}" task_log = TaskLogger(TaskLogConfig(dir=log_dir), now_ms_fn=now_ms) runner = TaskRunner(cfg, sm_cfg, px4, bridge, sm, ground, drone, now_ms_fn=now_ms, logger=task_log, pod_control=pod_control, land_instead_of_rtl=args.land, return_cfg=return_cfg) stop = asyncio.Event() _install_signal_handlers(stop, runner) ground_task = asyncio.ensure_future(ground.run(stop)) # UDP 接收地面站坐标 runner_task = asyncio.ensure_future(runner.run(stop)) print(f"-- 启动真机任务全流程（吊舱 {cfg.pod_serial.port}，" f"地面站 UDP {cfg.ground_inject.port}，目标类型 {args.target_type}，" f"收尾 {'就地降落' if args.land else '返航'}）", flush=True) print(f" 状态机: takeoff={sm_cfg.takeoff_altitude_m}m " f"arrival_radius={sm_cfg.arrival_radius_m}m " f"search={sm_cfg.search_timeout_s}s@{sm_cfg.sweep_rate_dps}°/s " f"track_max={sm_cfg.track_timeout_s}s | " f"坐标时效 ground_inject={cfg.ground_inject.timeout_ms}ms " f"guidance={cfg.guidance.target_stale_ms}ms", flush=True) print(f" 等待地面站注入坐标（每 0.5~1s 一次 JSON）… 日志 → {log_dir}", flush=True) started = time.monotonic() last_state = None try: while not runner.rtl_done: await asyncio.sleep(1.0) if runner.state is not last_state: last_state = runner.state print(f"[任务] 进入状态: {runner.state.value}", flush=True) if runner_task.done(): exc = runner_task.exception() if exc is not None: print(f"! runner 异常退出: {type(exc).__name__}: {exc}", flush=True) break if stop.is_set(): break # 信号处理已请求退出，交给 finally 收尾 if time.monotonic() - started > TASK_TIMEOUT_S: print("! 全流程超时，强制返航", flush=True) runner.request_rtl() if runner.rtl_done: print("-- 任务完成（落地）", flush=True) except asyncio.CancelledError: raise finally: stop.set() ground_task.cancel() runner_task.cancel() try: await runner_task except asyncio.CancelledError: pass try: await ground_task except asyncio.CancelledError: pass await print_flight_mode(drone) print("\n===== 真机任务全流程完成 =====", flush=True) return 0 def main() -> int: ap = argparse.ArgumentParser(description="真机任务状态机全流程") ap.add_argument("--config", default=str(CONFIG_PATH), help="桥接配置 JSON 路径") ap.add_argument("--conn", default=None, help="MAVSDK 连接串（覆盖 px4.connection_string）") ap.add_argument("--port", help="覆盖吊舱串口设备号（如 /dev/ttyAMA0）") ap.add_argument("--target-type", type=int, default=1, help="吊舱目标类型：0=人/车，1=无人机") ap.add_argument("--log-dir", default=None, help="日志输出目录（默认按时间戳）") ap.add_argument("--land", action="store_true", help="收尾改为就地降落（默认返航 RTL）") args = ap.parse_args() logging.basicConfig(level=logging.INFO, format="%(asctime)s.%(msecs)03d %(levelname)s %(message)s", datefmt="%H:%M:%S") # 真机常在树莓派上经 SSH 运行：stdout 非 tty 时块缓冲会让进度看不到，强制行缓冲 try: sys.stdout.reconfigure(line_buffering=True) except (AttributeError, ValueError): pass return asyncio.run(run(args)) if __name__ == "__main__": sys.exit(main())