code-test-drone/task_runner_real.py

#!/usr/bin/env python3
"""task_runner_real.py —— 真机任务编排入口（真实吊舱串口 + 地面站 UDP 注入 + PX4）。

与 task_runner_sitl.py（仿真）的差异：
  * 吊舱 = PodSerial（真实串口表2-2 遥测），非 VirtualPod；
  * 地面坐标 = GroundLink.run()（UDP 接收地面站 JSON），非 inject() 直接注入；
  * 吊舱目标识别/锁定 = PodController（GOTO 进入发 0x07 识别 + 0x0F 自动锁定，
    RTL 前发 0x0E 停止 + 0x08 识别关）；
  * PX4 连接 = cfg.px4.connection_string（真机串口/遥测），非 SITL 14540。

前置（真机联调增量：纯软单测 → 串口回环 → 仅吊舱 → 仅飞控 → 地面联调）：
  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
"""

import argparse
import asyncio
import logging
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.runner import TaskRunner
from task_runner.state_machine import TaskStateConfig, TaskStateMachine
from pod_sitl_test 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,
        switch_radius_m=30.0,      # GOTO→TRACK 切换半径（待标定）
        goto_timeout_s=180.0,
        track_timeout_s=180.0,
        center_hold_s=3.0,
        center_threshold_deg=3.0,
        approach_hover_s=5.0,
        loop_frequency_hz=20.0,
    )


async def run(args) -> int:
    cfg = load_from_json(args.config)
    if not cfg.pod_serial.port:
        print("! 未配置吊舱串口：在 pod_bridge.json 填 pod_serial.port，或用 --port 覆盖")
        return 2
    if args.port:
        cfg.pod_serial.port = args.port
    cfg.ground_inject.enabled = True  # 地面注入必须开（真机无激光，表4 永不出）
    sm_cfg = build_sm_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)

    stop = asyncio.Event()
    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}）")
    print(f"   等待地面站注入坐标（每 0.5~1s 一次 JSON）… 日志 → {log_dir}")

    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}")
            if runner_task.done():
                exc = runner_task.exception()
                if exc is not None:
                    print(f"! runner 异常退出: {type(exc).__name__}: {exc}")
                break
            if time.monotonic() - started > TASK_TIMEOUT_S:
                print("! 全流程超时，强制返航")
                runner.request_rtl()
        if runner.rtl_done:
            print("-- 任务完成（RTL 落地）")
    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===== 真机任务全流程完成 =====")
    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="日志输出目录（默认按时间戳）")
    args = ap.parse_args()

    logging.basicConfig(level=logging.INFO,
                        format="%(asctime)s.%(msecs)03d %(levelname)s %(message)s",
                        datefmt="%H:%M:%S")
    return asyncio.run(run(args))


if __name__ == "__main__":
    sys.exit(main())