#!/usr/bin/env python3 """l1_sitl_test.py —— L1 航向伺服 SITL 全流程集成测试。 连接 PX4 SITL，流程： 解锁 → 起飞 → 记录悬停锚点/航向 → 进 offboard → L1 航向伺服闭环 20 Hz（VirtualPod 按虚拟目标几何合成表2-2 遥测 → PodBridge → TrackingControlLaw 角度直入 → offboard 速度设定点） → 目标消失 → 判丢零速刹车 → 退出 offboard → RTL → 落地 → 上锁 只覆盖 L1 控制律这一段；任务级全流程见根目录 `task_runner_sitl.py`。 要点： * 走 L1 角度直入（吊舱脱靶量 off_target），无像素/检测框/追踪器； * 无距离（distance_valid=False）→ 距离通道按 no_distance_action 降级 （slow_approach，0.2 m/s 极低限速）； * 无电机 → 框架角恒 0；无激光 → laser=None； * 符号使用部署配置（S1 标定：beta_sign=+1 / alpha_sign=-1）， 本脚本即对该标定的端到端复核。 共享件（时钟/遥测订阅/连接准备）在 `utils/sitl_support.py`。 运行前：启动 PX4 SITL（默认本机 14540）。用法： python l1_sitl_test.py python l1_sitl_test.py --conn udpin://0.0.0.0:14540 --config utils/config/pod_bridge.json """ import argparse import asyncio import csv import logging import math import sys import time from pathlib import Path _UTILS_DIR = Path(__file__).resolve().parent / "utils" if str(_UTILS_DIR) not in sys.path: sys.path.insert(0, str(_UTILS_DIR)) from mavsdk import System from mavsdk.action import ActionError from pod_bridge.bridge import PodBridge from pod_bridge.config import load_from_json from pod_bridge.px4_gateway import Px4Gateway from pod_bridge.virtual_pod import (CarrierView, VirtualPod, VirtualTarget, VirtualTargetConfig) logger = logging.getLogger("l1_sitl") # ==================== 配置 ==================== CONNECTION = "udpin://0.0.0.0:14540" CONFIG_PATH = _UTILS_DIR / "config" / "pod_bridge.json" TAKEOFF_ALTITUDE = 5.0 # 起飞高度 (m) # 虚拟目标：相对悬停锚点、按锚点机头朝向放置（前/右/下），随后水平圆弧运动。 TARGET_AHEAD_M = 30.0 TARGET_RIGHT_M = 15.0 TARGET_DOWN_M = 0.0 # 正 = 目标更低（地面目标场景，垂直通道会下降） TARGET_ARC_RADIUS_M = 12.0 TARGET_ARC_SPEED_MPS = 1.0 TARGET_ARC_CCW = True TRACK_NORMAL_SECS = 20.0 # 目标保持可见时长（之后模拟丢失） TRACK_BRAKE_SECS = 5.0 # 判丢后零速刹车时长 TRACK_TIMEOUT_SECS = 120.0 # 跟踪段整体安全超时 STATUS_EVERY_TICKS = 10 # 20 Hz 下每 0.5 s 打印一行状态 CSV_PATH = Path(__file__).resolve().parent / "l1_sitl_trajectory.csv" # 共享件（时钟/遥测订阅/连接准备）：utils/sitl_support.py from sitl_support import (TelemetryHub, now_ms, print_flight_mode, set_telemetry_rates, wait_until_ready) async def takeoff(drone) -> bool: """解锁 + 起飞到 TAKEOFF_ALTITUDE。失败返回 False（调用方执行降落兜底）。""" print("-- 解锁") try: await drone.action.arm() except ActionError as e: print(f"解锁失败: {e}") return False print(f"-- 起飞到 {TAKEOFF_ALTITUDE} m") await drone.action.set_takeoff_altitude(TAKEOFF_ALTITUDE) await drone.action.takeoff() # 全速排水等待爬升（test.py 问题一的经验：循环内不 sleep，避免队列积压） print("-- 等待起飞完成...") reached = False try: async with asyncio.timeout(30.0): async for position in drone.telemetry.position(): if position.relative_altitude_m >= TAKEOFF_ALTITUDE * 0.90: print(f"-- 已到达起飞高度: {position.relative_altitude_m:.2f} m") reached = True break except TimeoutError: pass if not reached: print("! 30 s 内未达到起飞高度") return reached async def land_fallback(drone): try: await drone.action.land() except ActionError as e: print(f"! 降落指令失败: {e}") async def read_anchor(drone): """读取悬停锚点 NED 与机头航向（虚拟目标按机头朝向放置）。""" try: async with asyncio.timeout(10.0): async for pos_vel in drone.telemetry.position_velocity_ned(): anchor_ned = (pos_vel.position.north_m, pos_vel.position.east_m, pos_vel.position.down_m) break async with asyncio.timeout(10.0): async for euler in drone.telemetry.attitude_euler(): anchor_yaw_deg = euler.yaw_deg break except TimeoutError: raise RuntimeError("10 s 内未取得悬停锚点/航向") from None print(f" 悬停锚点 NED: N={anchor_ned[0]:.2f}, E={anchor_ned[1]:.2f}, " f"D={anchor_ned[2]:.2f}，机头 {anchor_yaw_deg:.1f}°") return anchor_ned, anchor_yaw_deg async def run_tracking(bridge, hub, target, started_ms: int) -> bool: """L1 航向伺服闭环段：按生效律频率循环 bridge.step()，返回是否正常走完场景。""" period_s = 1.0 / bridge._active_frequency_hz() target_gone_ms = started_ms + TRACK_NORMAL_SECS * 1000.0 lost_at_ms = None next_deadline = time.monotonic() + period_s tick = 0 rows: list = [] try: while True: now = now_ms() if now - started_ms >= TRACK_TIMEOUT_SECS * 1000.0: logger.warning("跟踪段超时（%.0f s），提前收尾", TRACK_TIMEOUT_SECS) break if target.visible and now >= target_gone_ms: target.set_visible(False) print("-- 模拟目标消失，吊舱转搜索态 → 应判丢刹车") try: out = await bridge.step(now) except Exception as exc: # 对齐 bridge.run() 的护栏：异常不终止，补发刹车保 offboard 心跳 logger.warning("[SITL] 控制周期异常: %s", exc) try: await bridge._px4.send_brake_hover(now) except Exception: pass await asyncio.sleep(period_s) continue if tick % STATUS_EVERY_TICKS == 0: t_s = (now - started_ms) / 1000.0 tel = bridge.latest_telemetry off = f"off=({tel[0].off_target_x_deg:+.1f},{tel[0].off_target_y_deg:+.1f})°" \ if tel else "off=(无遥测)" print(f"[跟踪 t={t_s:5.1f}s] {off} | [{out.mode.name}] " f"yaw_rate={out.yaw_rate_dps:+6.2f}°/s " f"vx_body={out.vx_body_mps:+.2f} vz={out.vz_mps:+.2f}") rows.append((t_s, hub.north_m, hub.east_m, hub.down_m, *target.pos_ned, out.yaw_rate_dps, out.vx_body_mps, out.vz_mps, out.mode.name)) tick += 1 if out.mode.name == "BRAKE_HOVER": if lost_at_ms is None: lost_at_ms = now print("-- 判丢，执行零速刹车") elif now - lost_at_ms >= TRACK_BRAKE_SECS * 1000.0: print("-- 判丢刹车完成，退出跟踪段") break next_deadline += period_s sleep_s = next_deadline - time.monotonic() if sleep_s > 0: await asyncio.sleep(sleep_s) else: next_deadline = time.monotonic() + period_s # 节拍落后，重锚 finally: _write_csv(rows) return True def _write_csv(rows) -> None: if not rows: return try: with open(CSV_PATH, "w", newline="", encoding="utf-8") as fp: w = csv.writer(fp) w.writerow(["t_s", "drone_n", "drone_e", "drone_d", "tgt_n", "tgt_e", "tgt_d", "yaw_rate_dps", "vx_body", "vz", "mode"]) w.writerows(rows) print(f"-- 轨迹 CSV 已保存: {CSV_PATH}") except OSError as e: print(f"! CSV 写入失败: {e}") async def run(args) -> int: cfg = load_from_json(args.config) if not cfg.heading_servo.enabled: print("! 配置 heading_servo.enabled=false：本脚本跑的是 L1 路径，" "请用 pod_bridge.json（enabled=true）") return 2 drone = System() await drone.connect(system_address=args.conn) try: await wait_until_ready(drone) except RuntimeError as e: print(f"! {e}") return 1 await set_telemetry_rates(drone) initial_mode = await print_flight_mode(drone) if not await takeoff(drone): await land_fallback(drone) return 1 await asyncio.sleep(1) try: anchor_ned, anchor_yaw_deg = await read_anchor(drone) except RuntimeError as e: print(f"! {e}") await land_fallback(drone) return 1 # ---- L1 闭环组件装配：真 SITL 飞控 + 模拟吊舱，全部复用生产代码路径 ---- hub = TelemetryHub() hub.start(drone) target = VirtualTarget( VirtualTargetConfig(ahead_m=TARGET_AHEAD_M, right_m=TARGET_RIGHT_M, down_m=TARGET_DOWN_M, arc_radius_m=TARGET_ARC_RADIUS_M, arc_speed_mps=TARGET_ARC_SPEED_MPS, arc_ccw=TARGET_ARC_CCW), anchor_ned, math.radians(anchor_yaw_deg)) pod = VirtualPod(target, hub.view, fov_h_deg=cfg.tracking.camera.fov_h_deg, fov_v_deg=cfg.tracking.camera.fov_v_deg) px4 = Px4Gateway(drone) bridge = PodBridge(cfg, px4, pod, now_ms_fn=now_ms) print(f"-- 进入 L1 航向伺服：虚拟目标按机头朝向放置于 " f"前 {TARGET_AHEAD_M:.0f} m / 右 {TARGET_RIGHT_M:.0f} m / " f"下 {TARGET_DOWN_M:.0f} m（世界 NED: N={target.pos_ned[0]:.1f}, " f"E={target.pos_ned[1]:.1f}, D={target.pos_ned[2]:.1f}）") print(f" 场景: 正常跟踪 {TRACK_NORMAL_SECS:.0f}s → 目标消失 → " f"判丢刹车 {TRACK_BRAKE_SECS:.0f}s") print(f" 符号(部署配置): beta_sign={cfg.heading_servo.beta_sign:+d} " f"alpha_sign={cfg.heading_servo.alpha_sign:+d} | 目标偏右→yaw_rate 正，" f"目标偏下→vz 正") if not await bridge.start(): print("! offboard 启动失败，放弃跟踪段") await bridge.stop() await hub.stop() await land_fallback(drone) return 1 await run_tracking(bridge, hub, target, now_ms()) await bridge.stop() await hub.stop() # ---- 退出 offboard 后回到 Hold，稍候确认模式 ---- await asyncio.sleep(1) await print_flight_mode(drone) # ---- RTL → 等待落地 → 上锁 ---- print("-- 执行 RTL") await drone.action.return_to_launch() print("-- 等待落地...") try: async with asyncio.timeout(240.0): async for is_in_air in drone.telemetry.in_air(): if not is_in_air: print("-- 已落地") break except TimeoutError: print("! 240 s 内未检测到落地") await asyncio.sleep(2) print("-- 上锁") try: await drone.action.disarm() print("-- 上锁成功") except ActionError as e: print(f"上锁失败（可能已自动上锁）: {e}") print(f"-- 初始模式是: {initial_mode}") print("\n===== L1 全流程测试完成 =====") return 0 def main() -> int: ap = argparse.ArgumentParser(description="L1 航向伺服 SITL 全流程集成测试") ap.add_argument("--conn", default=CONNECTION, help="MAVSDK 连接串（默认 SITL 14540）") ap.add_argument("--config", default=str(CONFIG_PATH), help="桥接配置 JSON 路径") 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())