实现预定流程全链路: 任务状态机 + 编排器 + 地面站 + 真机/SITL入口
- utils/task_runner: 6态状态机(WAIT→ARM_TAKEOFF→GOTO(L2)→TRACK(L1)→APPROACH→RTL) + 编排器(快照采集/状态动作) + 双层日志(事件JSONL/遥测CSV) + 吊舱控制指令(0x07/0x0F/0x0E/0x08) - pod_bridge: ground_link 地面坐标UDP注入(超龄判无效,无激光替代表4); set_heading_servo_enabled() 运行时L1/L2切换(切换即reset清积分防输出跳变); pod_locked 提为公共门控; config 新增 ground_inject 段 - virtual_pod 模拟吊舱(无电机语义) + verify_px4 增量④仅飞控验证 + pod_sitl_test L1 SITL全流程(飞行编排不塞进 run_bridge, 保持纯offboard闭环入口) - 入口: task_runner_sitl(VirtualPod+虚拟目标, SITL端到端跑通约1m24s) / task_runner_real(真串口+UDP注入+吊舱控制) - 地面站 ground_station: 坐标注入 + 状态可视化(Flask) - S1标定回填(2026-10-09): beta_sign=+1 / alpha_sign=-1, 加标定回归测试守护 - 新增测试: task_runner x4 + ground_link/virtual_pod*/bridge/px4_gateway 增量; 全量 483 passed - 文档: 预定流程(需求) + 预定流程-操作指南(操作/待办) + L1方案补S1结论与实现状态
This commit is contained in:
keita
@@ -0,0 +1,165 @@
|
||||
#!/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())
|
||||
Reference in New Issue
Block a user