新增能运行的仿真脚本

test.py

@@ -0,0 +1,842 @@
+#!/usr/bin/env python3
+"""
+PX4 SITL Offboard 视觉跟踪集成测试 (MAVSDK-Python)
+连接: udpin://0.0.0.0:14540
+流程:
+  解锁 → 起飞 → 切换 Offboard（速度设定点）
+  → 视觉跟踪闭环 20 Hz（追踪器 → 控制律 → offboard 网关）
+  → 目标消失 → Coast 预测 → 判丢零速刹车 → 退出 Offboard → RTL → 落地 → 上锁
+适用于 PX4 1.17
+
+YOLO 检测未接入：VirtualTarget 用无人机真实位置/航向做针孔反投影生成
+检测框与激光测距（几何一致闭环），无人机会实际飞向该虚拟目标。
+"""
+
+import asyncio
+import logging
+import math
+import sys
+import time
+from pathlib import Path
+
+import matplotlib
+import matplotlib.pyplot as plt
+from matplotlib import font_manager
+from matplotlib.ticker import MaxNLocator
+
+
+def _setup_chinese_font() -> str | None:
+    """为 matplotlib 配置可用的中文字体，避免中文标题/图例乱码。
+
+    优先从字体文件注册，再按 family 名称匹配（Linux Noto/WQY、
+    Windows 雅黑/黑体、macOS 苹方等）。返回实际字体名；失败返回 None。
+    """
+    # 1) 直接按路径注册（TTC/TTF）
+    path_candidates = [
+        "/usr/share/fonts/opentype/noto/NotoSansCJK-Regular.ttc",
+        "/usr/share/fonts/opentype/noto/NotoSerifCJK-Regular.ttc",
+        "/usr/share/fonts/truetype/droid/DroidSansFallbackFull.ttf",
+        "/usr/share/fonts/truetype/wqy/wqy-microhei.ttc",
+        "/usr/share/fonts/truetype/wqy/wqy-zenhei.ttc",
+        "C:/Windows/Fonts/msyh.ttc",
+        "C:/Windows/Fonts/msyhbd.ttc",
+        "C:/Windows/Fonts/simhei.ttf",
+        "/System/Library/Fonts/PingFang.ttc",
+        "/System/Library/Fonts/STHeiti Light.ttc",
+    ]
+    chosen = None
+    for path in path_candidates:
+        if not Path(path).is_file():
+            continue
+        try:
+            font_manager.fontManager.addfont(path)
+            prop = font_manager.FontProperties(fname=path)
+            chosen = prop.get_name()
+            break
+        except Exception:
+            continue
+
+    # 2) 按已注册 family 名称匹配
+    if chosen is None:
+        name_candidates = [
+            "Noto Sans CJK SC",
+            "Noto Sans CJK JP",
+            "Noto Serif CJK SC",
+            "Noto Serif CJK JP",
+            "Droid Sans Fallback",
+            "WenQuanYi Micro Hei",
+            "WenQuanYi Zen Hei",
+            "Source Han Sans SC",
+            "Source Han Sans CN",
+            "Microsoft YaHei",
+            "SimHei",
+            "PingFang SC",
+            "Heiti SC",
+            "Arial Unicode MS",
+        ]
+        available = {f.name for f in font_manager.fontManager.ttflist}
+        for name in name_candidates:
+            if name in available:
+                chosen = name
+                break
+
+    if chosen:
+        matplotlib.rcParams["font.sans-serif"] = [chosen, "DejaVu Sans"]
+        matplotlib.rcParams["font.family"] = "sans-serif"
+        # 解决坐标轴负号显示为方块的问题
+        matplotlib.rcParams["axes.unicode_minus"] = False
+    return chosen
+
+
+_CHINESE_FONT = _setup_chinese_font()
+
+_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 utils.target_tracker import Detection, FrameShape, TargetTracker, TrackerConfig
+from utils.demo_track_control import track_result_to_state
+from utils.visual_track_control.config import load_from_json
+from utils.visual_track_control.mavsdk_gateway import MavsdkCommandGateway
+from utils.visual_track_control.tracking_control_law import TrackingControlLaw
+from utils.visual_track_control.types import ControlMode, ControlOutput, ControlSnapshot
+
+logger = logging.getLogger("test")
+
+# ==================== 配置 ====================
+CONNECTION = "udpin://0.0.0.0:14540"   # 推荐写法，也可用 "udp://127.0.0.1:14540"
+TAKEOFF_ALTITUDE = 5.0                 # 起飞高度 (m)
+
+TRACK_CONFIG_PATH = _UTILS_DIR / "config" / "track_control.json"
+TRACK_MAX_LOST_FRAMES = 60             # 追踪器 Coast 窗口（20 Hz 下 = 3 s）
+TRACK_NORMAL_SECS = 20.0               # 虚拟目标保持可见时长（之后进入丢失场景）
+TRACK_BRAKE_SECS = 5.0                 # 判丢后零速刹车时长
+TRACK_TIMEOUT_SECS = 120.0             # 跟踪段整体安全超时
+STATUS_EVERY_TICKS = 10                # 每 0.5 s 打印一行状态
+PLOT_TRAJECTORY = False                 # 跟踪结束后绘制无人机与虚拟目标轨迹
+TRAJECTORY_SAVE_PATH = Path(__file__).resolve().parent / "trajectory_plot.png"
+
+# 虚拟目标：相对悬停锚点、按锚点机头朝向放置的机体系偏移
+# （前方 AHEAD / 右方 RIGHT，横偏用于激发航向通道）。
+# 默认与悬停点同高：地面目标场景下垂直通道会持续下降趋近目标高度，
+# SITL 中有触发 PX4 着陆检测的风险；TARGET_DOWN_M 取正值（目标更低，
+# 如 +5.0 = 地面）即可切换为地面目标场景。
+TARGET_AHEAD_M = 30.0
+TARGET_RIGHT_M = 15.0
+TARGET_DOWN_M = 0.0
+MOCK_BOX_W = 120.0                     # 模拟检测框尺寸 (px)
+MOCK_BOX_H = 160.0
+MOCK_CONFIDENCE = 0.9
+MOCK_TARGET_SPEED_MPS = 1.0            # 圆弧切向速度 (m/s)
+TARGET_ARC_RADIUS_M = 12.0             # 圆弧半径 (m)
+# 旋转方向：+1 = NED 水平面逆时针（从北向东转），-1 = 顺时针
+TARGET_ARC_CCW = 1
+
+
+def now_ms() -> int:
+    return int(time.monotonic() * 1000.0)
+
+
+def plot_trajectories(
+    drone_traj: list,
+    target_traj: list,
+    save_path=None,
+) -> None:
+    """绘制无人机与虚拟目标的 NED 轨迹。
+
+    参数:
+        drone_traj / target_traj: 列表元素为 (t_s, north_m, east_m, down_m)
+        save_path: 若给定则保存图片，否则仅弹窗显示
+    """
+    if not drone_traj and not target_traj:
+        print("! 无轨迹数据，跳过绘图")
+        return
+
+    if _CHINESE_FONT is None:
+        print("! 未找到中文字体，图中中文可能显示为方框。"
+              "可安装 fonts-noto-cjk 或文泉驿，或改用英文标签。")
+    else:
+        # 再次确保本进程内 rc 生效（部分后端会重置）
+        matplotlib.rcParams["font.sans-serif"] = [_CHINESE_FONT, "DejaVu Sans"]
+        matplotlib.rcParams["axes.unicode_minus"] = False
+
+    fig, axes = plt.subplots(1, 2, figsize=(12, 5))
+
+    # ---- 左图：水平面 N-E 轨迹（俯视图）----
+    ax = axes[0]
+    if drone_traj:
+        dn = [p[1] for p in drone_traj]
+        de = [p[2] for p in drone_traj]
+        ax.plot(de, dn, "b-", linewidth=1.5, label="无人机")
+        ax.scatter(de[0], dn[0], c="blue", s=60, marker="o", zorder=5, label="无人机起点")
+        ax.scatter(de[-1], dn[-1], c="blue", s=80, marker="x", zorder=5, label="无人机终点")
+    if target_traj:
+        tn = [p[1] for p in target_traj]
+        te = [p[2] for p in target_traj]
+        ax.plot(te, tn, "r--", linewidth=1.5, label="虚拟目标")
+        ax.scatter(te[0], tn[0], c="red", s=60, marker="o", zorder=5, label="目标起点")
+        ax.scatter(te[-1], tn[-1], c="red", s=80, marker="x", zorder=5, label="目标终点")
+    ax.set_xlabel("East (m)")
+    ax.set_ylabel("North (m)")
+    ax.set_title("水平轨迹 (NED 俯视)")
+    ax.set_aspect("equal", adjustable="datalim")
+    ax.grid(True, linestyle="--", alpha=0.6)
+    ax.legend(loc="best", fontsize=8)
+    ax.xaxis.set_major_locator(MaxNLocator(nbins=6))
+    ax.yaxis.set_major_locator(MaxNLocator(nbins=6))
+
+    # ---- 右图：高度随时间（Down 取负号显示为高度）----
+    ax2 = axes[1]
+    if drone_traj:
+        dt = [p[0] for p in drone_traj]
+        d_alt = [-p[3] for p in drone_traj]  # NED down → 高度
+        ax2.plot(dt, d_alt, "b-", linewidth=1.5, label="无人机高度")
+    if target_traj:
+        tt = [p[0] for p in target_traj]
+        t_alt = [-p[3] for p in target_traj]
+        ax2.plot(tt, t_alt, "r--", linewidth=1.5, label="目标高度")
+    ax2.set_xlabel("时间 (s)")
+    ax2.set_ylabel("高度 (m)")
+    ax2.set_title("高度-时间曲线")
+    ax2.grid(True, linestyle="--", alpha=0.6)
+    ax2.legend(loc="best", fontsize=8)
+
+    fig.suptitle("视觉跟踪段轨迹", fontsize=12)
+    fig.tight_layout()
+
+    if save_path is not None:
+        fig.savefig(str(save_path), dpi=150, bbox_inches="tight")
+        print(f"-- 轨迹图已保存: {save_path}")
+    try:
+        plt.show()
+    except Exception as e:
+        print(f"! 无法弹窗显示图像（无图形界面？）: {e}")
+    finally:
+        plt.close(fig)
+
+
+class VirtualTarget:
+    """几何一致虚拟目标 —— YOLO 检测与激光雷达的替身。
+
+    目标初始位置：
+        以起飞后的悬停锚点为参考，
+        按起飞时机头方向放置：
+            前方 TARGET_AHEAD_M
+            右方 TARGET_RIGHT_M
+            下方 TARGET_DOWN_M
+
+    目标运动（水平圆弧）：
+        跟踪开始时刻 t=0，目标以切向速度 MOCK_TARGET_SPEED_MPS、
+        半径 TARGET_ARC_RADIUS_M 在 NED 水平面内做匀速圆周运动。
+
+        圆心：初始位置沿“起飞时右方”偏移半径 R（目标从圆心左侧/右侧
+        起转，初始切向近似沿机头前方），高度保持不变。
+
+        角速度 ω = v / R，方向由 TARGET_ARC_CCW 控制（+1 逆时针）。
+
+    每次 observe() 调用时，根据经过的时间实时更新目标的
+    NED 世界坐标，然后再进行针孔投影。
+
+    相机假定与机体固连、无视差、忽略 roll/pitch；
+    仅使用 yaw 将世界 NED 坐标转换到机体系。
+    """
+
+    def __init__(
+        self,
+        cfg,
+        anchor_ned: tuple[float, float, float],
+        anchor_yaw_rad: float,
+    ) -> None:
+
+        # ============================================================
+        # 1. 根据起飞后的锚点 + 起飞时航向，计算目标初始位置
+        # ============================================================
+
+        cos_y = math.cos(anchor_yaw_rad)
+        sin_y = math.sin(anchor_yaw_rad)
+
+        # 机体系：
+        #   前方 = (cos(yaw), sin(yaw))
+        #   右方 = (-sin(yaw), cos(yaw))
+
+        self._start_n = (
+            anchor_ned[0]
+            + TARGET_AHEAD_M * cos_y
+            - TARGET_RIGHT_M * sin_y
+        )
+
+        self._start_e = (
+            anchor_ned[1]
+            + TARGET_AHEAD_M * sin_y
+            + TARGET_RIGHT_M * cos_y
+        )
+
+        self._start_d = (
+            anchor_ned[2]
+            + TARGET_DOWN_M
+        )
+
+        # ============================================================
+        # 2. 当前目标位置
+        # ============================================================
+
+        self._pos_n = self._start_n
+        self._pos_e = self._start_e
+        self._pos_d = self._start_d
+
+        # ============================================================
+        # 3. 圆弧参数
+        #
+        # 圆心取在初始点沿“起飞时右方”偏移 R 处，使 t=0 时目标
+        # 位于圆心左侧（相对机头），初始切向大致沿机头前方：
+        #
+        #   右方单位向量: (-sin(yaw), cos(yaw))
+        #   圆心 = start + R * 右方
+        #   初始相位：从圆心指向 start 的向量 = -R * 右方
+        # ============================================================
+
+        self._radius_m = max(0.1, float(TARGET_ARC_RADIUS_M))
+        self._omega = (
+            float(MOCK_TARGET_SPEED_MPS) / self._radius_m
+        ) * (1.0 if TARGET_ARC_CCW >= 0 else -1.0)
+
+        right_n = -sin_y
+        right_e = cos_y
+
+        self._center_n = self._start_n + self._radius_m * right_n
+        self._center_e = self._start_e + self._radius_m * right_e
+
+        # 初始角度：圆心 → 起点 在 NE 平面的方位角
+        # atan2(E, N) 与常见航向定义一致
+        self._theta0 = math.atan2(
+            self._start_e - self._center_e,
+            self._start_n - self._center_n,
+        )
+
+        self._move_yaw_rad = anchor_yaw_rad  # 仅作参考/日志
+
+        # ============================================================
+        # 4. 记录目标运动开始时间
+        # ============================================================
+
+        self._start_time = time.monotonic()
+
+        # ============================================================
+        # 5. 相机参数
+        # ============================================================
+
+        cam = cfg.camera
+
+        self._img_w = float(cam.image_width)
+        self._img_h = float(cam.image_height)
+
+        # 针孔模型焦距（像素）：
+        #
+        #     f = W / (2 * tan(FOV_h / 2))
+        #
+        self._f_px = self._img_w / (
+            2.0 * math.tan(
+                math.radians(cam.fov_h_deg) / 2.0
+            )
+        )
+
+        # ============================================================
+        # 6. 目标可见状态
+        # ============================================================
+
+        self._visible = True
+
+    @property
+    def visible(self) -> bool:
+        """返回目标当前是否可见。"""
+        return self._visible
+
+    def set_visible(self, visible: bool) -> None:
+        """设置目标是否可见。"""
+        self._visible = visible
+
+    def update_position(self) -> None:
+        """根据经过的时间更新目标世界坐标（水平匀速圆弧）。
+
+        θ(t) = θ0 + ω * t
+        N = center_N + R * cos(θ)
+        E = center_E + R * sin(θ)
+        D 保持初始高度不变。
+
+        切向速度 v = |ω| * R = MOCK_TARGET_SPEED_MPS。
+        """
+
+        elapsed_s = time.monotonic() - self._start_time
+        elapsed_s = max(0.0, elapsed_s)
+
+        theta = self._theta0 + self._omega * elapsed_s
+
+        self._pos_n = self._center_n + self._radius_m * math.cos(theta)
+        self._pos_e = self._center_e + self._radius_m * math.sin(theta)
+        self._pos_d = self._start_d
+
+    def observe(
+        self,
+        ned_n: float,
+        ned_e: float,
+        ned_d: float,
+        yaw_rad: float,
+    ):
+        """返回：
+
+            (detections, radar_distance_m, associated)
+
+        不可见时：
+
+            ([], 0.0, False)
+
+        每次调用都会先更新虚拟目标位置，
+        然后根据无人机当前位置和当前 yaw
+        计算目标在相机画面中的投影。
+        """
+
+        # ============================================================
+        # 1. 始终更新目标真实世界坐标（即使“不可见”也继续运动，
+        #    便于轨迹记录；仅检测输出被屏蔽）
+        # ============================================================
+
+        self.update_position()
+
+        # ============================================================
+        # 2. 目标不可见 → 不返回检测框
+        # ============================================================
+
+        if not self._visible:
+            return [], 0.0, False
+
+        # ============================================================
+        # 3. 目标相对于无人机的 NED 坐标
+        # ============================================================
+
+        rel_n = self._pos_n - ned_n
+        rel_e = self._pos_e - ned_e
+        rel_d = self._pos_d - ned_d
+
+        # ============================================================
+        # 4. 世界坐标 NED → 当前无人机机体系
+        #
+        # 这里只考虑 yaw，不考虑 roll / pitch。
+        #
+        # fwd  ：目标在机头前方的距离
+        # right：目标在机体右侧的距离
+        # ============================================================
+
+        cos_y = math.cos(yaw_rad)
+        sin_y = math.sin(yaw_rad)
+
+        fwd = (
+            rel_n * cos_y
+            + rel_e * sin_y
+        )
+
+        right = (
+            -rel_n * sin_y
+            + rel_e * cos_y
+        )
+
+        # ============================================================
+        # 5. 目标在侧面或后方
+        # ============================================================
+
+        if fwd <= 0.5:
+            return [], 0.0, False
+
+        # ============================================================
+        # 6. 针孔相机投影
+        # ============================================================
+
+        u = (
+            self._img_w * 0.5
+            + self._f_px * (right / fwd)
+        )
+
+        v = (
+            self._img_h * 0.5
+            + self._f_px * (rel_d / fwd)
+        )
+
+        # ============================================================
+        # 7. 目标投影超出画面
+        # ============================================================
+
+        if not (
+            0.0 <= u < self._img_w
+            and
+            0.0 <= v < self._img_h
+        ):
+            return [], 0.0, False
+
+        # ============================================================
+        # 8. 构造模拟 YOLO 检测框
+        # ============================================================
+
+        box = Detection(
+            u - MOCK_BOX_W / 2.0,
+            v - MOCK_BOX_H / 2.0,
+            u + MOCK_BOX_W / 2.0,
+            v + MOCK_BOX_H / 2.0,
+            MOCK_CONFIDENCE,
+        )
+
+        # ============================================================
+        # 9. 计算模拟激光雷达距离
+        # ============================================================
+
+        slant = math.sqrt(
+            fwd * fwd
+            + right * right
+            + rel_d * rel_d
+        )
+
+        return [box], slant, True
+
+
+class TelemetryHub:
+    """后台订阅姿态/位置遥测，控制回路每拍读取最新快照（§7.1 同拍取样）。"""
+
+    def __init__(self) -> None:
+        self.yaw_rad = 0.0
+        self.roll_rad = 0.0
+        self.pitch_rad = 0.0
+        self.attitude_time_ms = 0
+        self.north_m = 0.0
+        self.east_m = 0.0
+        self.down_m = 0.0
+        self._tasks: list[asyncio.Task] = []
+
+    def start(self, drone) -> None:
+        self._tasks.append(asyncio.create_task(self._attitude_loop(drone)))
+        self._tasks.append(asyncio.create_task(self._position_loop(drone)))
+
+    async def _attitude_loop(self, drone) -> None:
+        async for euler in drone.telemetry.attitude_euler():
+            self.yaw_rad = math.radians(euler.yaw_deg)
+            self.roll_rad = math.radians(euler.roll_deg)
+            self.pitch_rad = math.radians(euler.pitch_deg)
+            self.attitude_time_ms = now_ms()
+
+    async def _position_loop(self, drone) -> None:
+        async for pos_vel in drone.telemetry.position_velocity_ned():
+            pos = pos_vel.position
+            self.north_m = pos.north_m
+            self.east_m = pos.east_m
+            self.down_m = pos.down_m
+
+    async def wait_attitude(self, timeout_s: float = 10.0) -> bool:
+        deadline = time.monotonic() + timeout_s
+        while self.attitude_time_ms == 0 and time.monotonic() < deadline:
+            await asyncio.sleep(0.1)
+        return self.attitude_time_ms > 0
+
+    async def stop(self) -> None:
+        for task in self._tasks:
+            task.cancel()
+        for task in self._tasks:
+            try:
+                await task
+            except asyncio.CancelledError:
+                pass
+        self._tasks.clear()
+
+
+async def dispatch(sender, out: ControlOutput, tick_ms: int) -> None:
+    """按模式分发指令。
+
+    DEGRADED_HOLD 在胶水层 demo 中不发送，但真实 offboard 会话中断流
+    （约 0.5~1 s）会触发 PX4 offboard failsafe，故此处改发零速设定点
+    （等效刹停悬停）保住设定点流。
+    """
+    if out.mode is ControlMode.VELOCITY_HEADING and out.valid:
+        await sender.send_velocity_heading(out, tick_ms)
+    else:
+        await sender.send_brake_hover(tick_ms)
+
+
+async def run_tracking(drone, anchor_ned: tuple[float, float, float],
+                       anchor_yaw_deg: float) -> bool:
+    """视觉跟踪闭环段：20 Hz 控制回路，返回是否正常走完场景。"""
+    cfg = load_from_json(str(TRACK_CONFIG_PATH))
+    period_s = 1.0 / cfg.control.frequency_hz
+
+    hub = TelemetryHub()
+    hub.start(drone)
+    if not await hub.wait_attitude():
+        logger.error("姿态遥测未就绪，放弃跟踪段")
+        await hub.stop()
+        return False
+
+    tracker = TargetTracker(TrackerConfig(max_lost_frames=TRACK_MAX_LOST_FRAMES))
+    law = TrackingControlLaw(cfg)
+    gateway = MavsdkCommandGateway(drone)
+    target = VirtualTarget(cfg, anchor_ned, math.radians(anchor_yaw_deg))
+    frame_shape = FrameShape(height=cfg.camera.image_height,
+                             width=cfg.camera.image_width)
+
+    if not await gateway.start():
+        logger.error("offboard 启动失败，放弃跟踪段")
+        await hub.stop()
+        return False
+
+    print(f"-- 进入视觉跟踪：锚点航向 {anchor_yaw_deg:.1f}°，虚拟目标按机头朝向放置于 "
+          f"前 {TARGET_AHEAD_M:.0f} m / 右 {TARGET_RIGHT_M:.0f} m / 下 {TARGET_DOWN_M:.0f} m "
+          f"(世界 NED: N={target._pos_n:.1f}, E={target._pos_e:.1f}, D={target._pos_d:.1f})")
+    print(f"   目标运动: 圆弧 R={TARGET_ARC_RADIUS_M:.1f} m, v={MOCK_TARGET_SPEED_MPS:.1f} m/s, "
+          f"{'逆时针' if TARGET_ARC_CCW >= 0 else '顺时针'} "
+          f"(圆心 N={target._center_n:.1f}, E={target._center_e:.1f})")
+    print(f"   场景: 正常跟踪 {TRACK_NORMAL_SECS:.0f}s → 目标消失 → "
+          f"Coast {TRACK_MAX_LOST_FRAMES / cfg.control.frequency_hz:.0f}s → "
+          f"判丢刹车 {TRACK_BRAKE_SECS:.0f}s")
+
+    started_ms = now_ms()
+    target_gone_ms = started_ms + TRACK_NORMAL_SECS * 1000.0
+    lost_at_ms = None
+    next_deadline = time.monotonic() + period_s
+    tick = 0
+
+    # 轨迹记录：(t_s, north_m, east_m, down_m)
+    drone_traj = []
+    target_traj = []
+
+    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("-- 模拟目标消失，追踪器进入 Coast 预测")
+
+            detections, radar_m, associated = target.observe(
+                hub.north_m, hub.east_m, hub.down_m, hub.yaw_rad)
+
+            # 记录本拍位置（目标位置在 observe 内已 update）
+            t_s = (now - started_ms) / 1000.0
+            drone_traj.append((t_s, hub.north_m, hub.east_m, hub.down_m))
+            target_traj.append((t_s, target._pos_n, target._pos_e, target._pos_d))
+
+            result = tracker.update(detections, frame_shape)
+            snap = ControlSnapshot(
+                track=track_result_to_state(result),
+                track_time_ms=now,                 # 每拍取样即打戳（视觉始终新鲜）
+                radar_distance_m=radar_m,
+                associated_to_target=associated,
+                radar_time_ms=now if associated else 0,
+                attitude_present=True,
+                attitude_time_ms=hub.attitude_time_ms,
+                yaw_rad=hub.yaw_rad,
+                roll_rad=hub.roll_rad,
+                pitch_rad=hub.pitch_rad,
+            )
+            out = law.update(snap, now)
+            await dispatch(gateway, out, now)
+
+            if tick % STATUS_EVERY_TICKS == 0:
+                print(f"[跟踪 t={t_s:5.1f}s] "
+                      f"检测={len(detections)} "
+                      f"追踪[trk={result.tracked} pred={result.is_predicted} "
+                      f"lost={result.lost_frames}] 雷达={radar_m:5.1f}m | "
+                      f"[{out.mode.name}] vx_body={out.vx_body_mps:+.2f} "
+                      f"vz={out.vz_mps:+.2f} yaw_rate={out.yaw_rate_dps:+.2f}°/s "
+                      f"coast={out.coast_active}")
+            tick += 1
+
+            if not result.tracked:
+                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:
+        await gateway.stop()
+        await hub.stop()
+
+    # 跟踪段结束后绘制轨迹
+    if PLOT_TRAJECTORY:
+        print(f"-- 绘制轨迹：无人机 {len(drone_traj)} 点，目标 {len(target_traj)} 点")
+        plot_trajectories(drone_traj, target_traj, TRAJECTORY_SAVE_PATH)
+
+    return True
+
+
+async def print_flight_mode(drone):
+    """打印当前飞行模式"""
+    async for flight_mode in drone.telemetry.flight_mode():
+        print(f"[模式] 当前飞行模式: {flight_mode}")
+        return flight_mode
+
+async def wait_until_ready(drone):
+    """等待连接 + 位置估计就绪（各阶段限时 30 s）。"""
+    print("等待无人机连接...")
+    try:
+        async with asyncio.timeout(30.0):
+            async for state in drone.core.connection_state():
+                if state.is_connected:
+                    print("-- 已连接")
+                    break
+    except TimeoutError:
+        raise RuntimeError("30 s 内未建立 MAVSDK 连接（检查 SITL 是否在运行、"
+                           "14540 端口是否被占用）") from None
+
+    print("等待全球位置估计就绪...")
+    try:
+        async with asyncio.timeout(30.0):
+            async for health in drone.telemetry.health():
+                if health.is_global_position_ok and health.is_home_position_ok:
+                    print("-- 位置估计 OK")
+                    break
+    except TimeoutError:
+        raise RuntimeError("30 s 内位置估计未就绪") from None
+
+async def run():
+    drone = System()
+    await drone.connect(system_address=CONNECTION)
+
+    # 1. 等待就绪
+    try:
+        await wait_until_ready(drone)
+    except RuntimeError as e:
+        print(f"! {e}")
+        return
+
+    # 1.5 显式设定遥测流速率（不依赖默认配置；失败则沿用默认并告警）
+    for stream, rate in (("position", 50.0),
+                         ("position_velocity_ned", 30.0),
+                         ("attitude_euler", 50.0),
+                         ("in_air", 10.0)):
+        try:
+            await getattr(drone.telemetry, f"set_rate_{stream}")(rate)
+        except Exception as e:
+            print(f"! 设置 {stream} 速率失败（沿用默认值）: {e}")
+    print("-- 遥测速率已设定")
+
+    # 记录初始模式（可选，用于最后恢复）
+    initial_mode = await print_flight_mode(drone)
+
+    # 2. 解锁
+    print("-- 解锁")
+    try:
+        await drone.action.arm()
+    except ActionError as e:
+        print(f"解锁失败: {e}")
+        return
+
+    # 3. 起飞
+    print(f"-- 起飞到 {TAKEOFF_ALTITUDE} m")
+    await drone.action.set_takeoff_altitude(TAKEOFF_ALTITUDE)
+    await drone.action.takeoff()
+
+    # 等待起飞完成。注意：MAVSDK 遥测是带缓冲队列的推送流，循环体内
+    # 不能 sleep —— 否则消费速率（如 2 Hz）远低于推送速率（50 Hz），
+    # 队列积压导致读到的是几十秒前的旧高度（问题一的根因）。全速排水
+    # 使每条消息都是最新值，另加整体超时兜底。
+    print("-- 等待起飞完成...")
+    wait_start = time.monotonic()
+    next_log = wait_start + 2.0
+    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:
+                    wait_s = time.monotonic() - wait_start
+                    print(f"-- 已到达起飞高度: {position.relative_altitude_m:.2f} m "
+                          f"(等待 {wait_s:.1f} s)")
+                    reached = True
+                    break
+                now_t = time.monotonic()
+                if now_t >= next_log:
+                    print(f"   起飞爬升中: {position.relative_altitude_m:.2f} m")
+                    next_log = now_t + 2.0
+    except TimeoutError:
+        pass
+    if not reached:
+        print("! 30 s 内未达到起飞高度，放弃本次测试（执行降落兜底）")
+        try:
+            await drone.action.land()
+        except ActionError as e:
+            print(f"! 降落指令失败: {e}")
+        return
+
+    await asyncio.sleep(1)
+
+    # 4. 记录悬停锚点与锚点航向（虚拟目标按机头朝向放置）
+    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
+        anchor_yaw_deg = 0.0
+        async with asyncio.timeout(10.0):
+            async for euler in drone.telemetry.attitude_euler():
+                anchor_yaw_deg = euler.yaw_deg
+                break
+    except TimeoutError:
+        print("! 10 s 内未取得悬停锚点/航向，放弃本次测试（执行降落兜底）")
+        try:
+            await drone.action.land()
+        except ActionError as e:
+            print(f"! 降落指令失败: {e}")
+        return
+    print(f"   悬停锚点 NED: N={anchor_ned[0]:.2f}, "
+          f"E={anchor_ned[1]:.2f}, D={anchor_ned[2]:.2f}，"
+          f"机头 {anchor_yaw_deg:.1f}°")
+
+    # 5. 视觉跟踪闭环（offboard 速度设定点，20 Hz）
+    await run_tracking(drone, anchor_ned, anchor_yaw_deg)
+
+    # 6. 退出 offboard 后回到 Hold，稍候确认模式
+    await asyncio.sleep(1)
+    await print_flight_mode(drone)
+
+    # 7. 返航 (RTL)
+    print("-- 执行 RTL")
+    await drone.action.return_to_launch()
+
+    # 8. 等待落地（同样全速排水，避免 in_air 队列积压读到旧状态）
+    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)
+
+    # 9. 上锁
+    print("-- 上锁")
+    try:
+        await drone.action.disarm()
+        print("-- 上锁成功")
+    except ActionError as e:
+        print(f"上锁失败（可能已自动上锁）: {e}")
+
+    print(f"-- 初始模式是: {initial_mode}")
+    print("\n===== 测试流程全部完成 =====")
+
+if __name__ == "__main__":
+    logging.basicConfig(level=logging.INFO,
+                        format="%(asctime)s.%(msecs)03d %(levelname)s %(message)s",
+                        datefmt="%H:%M:%S")
+    asyncio.run(run())