精简仓库结构并增强任务链路：归档双目可选后端、新增LOS判据与自定义返航

归档 legacy/：stereo_yolo/stereo_ranger/target_tracker 三包 + 175 条测试 + demo/配置/权重/三份规格；运行链路不再依赖，默认 pytest 446 条，归档测试按需运行。目标有效性第二判据改为 LOS 运动学一致性（pod_bridge/los_consistency.py），删除 require_vision；pod_locked() 叠加该判据，状态机随之后退重搜。自定义返航（task_runner/return_home.py）：保持转场高度回起飞点 → 恢复初始航向 → 垂直降落，不再爬 RTL_RETURN_ALT；未起飞/无起飞点/超时/异常仍退回 PX4 原生 RTL。任务链安全兜底：主循环异常→紧急返航、stop 幂等、landing_stop 保留遥测、in_air 流复用+超时、解锁/起飞与吊舱识别仅执行一次、rtl_timeout 上限。入口共享件拆到 utils/sitl_support.py；pod_sitl_test.py→l1_sitl_test.py；删除旧架构 test.py；相机脚本移入 tools/。新增 .gitignore 并剔除历史误跟踪的 59 个 .pyc；requirements 按实际依赖重写；文档全面对齐，新增 legacy/README.md 与 tools/README.md。
2026-10-21 14:48:41 +08:00
parent 53fdd3b13f
commit df91b1110e
159 changed files with 6121 additions and 1617 deletions
@@ -7,6 +7,8 @@ L2（地理引导 GuidanceLaw）按任务阶段编排，配合地面坐标注入
 职责边界：
  * 状态机（TaskStateMachine）只做「状态 + 转移判据」决策；
  * 本编排器负责「采集快照 → 喂状态机 → 按状态执行动作」；
+  * 地面站命令（RTL/紧急/吊舱手动指令，经 GroundLink 队列）主循环逐拍分派；
+  * 飞控状态文本（STATUSTEXT，警告/异常）逐拍落任务日志（fc_status 事件）；
  * 闭环控制复用 PodBridge（L1/L2 运行时切换走 set_heading_servo_enabled）；
  * 飞行编排（解锁/起飞/返航）通过 duck-typed `drone`（MAVSDK System）完成，
    便于 mock 单测；真实 SITL 装配见仓库根 task_runner_sitl.py。
@@ -23,13 +25,38 @@ from typing import Callable, Optional

 from pod_bridge.guidance import haversine_distance_m
 from visual_track_control.staleness_checker import is_stale
+from visual_track_control.types import ControlMode, ControlOutput

 from .log import TaskLogger, TelemetryRecord
+from .return_home import HomingController, ReturnConfig
 from .state_machine import (TaskInputs, TaskState, TaskStateConfig,
                            TaskStateMachine)

 logger = logging.getLogger(__name__)

+# 自定义返航的阶段
+_RTL_RETURN = "return"      # 转场：飞回起飞点，保持转场高度
+_RTL_YAW = "yaw"            # 到位：把机头转回初始航向
+_RTL_LAND = "land"          # 降落：交给飞控垂直降落
+_RTL_FALLBACK = "fallback"  # 兜底：退回 PX4 原生 RTL（会爬升，仅异常时使用）
+_RTL_IDLE = "idle"
+
+# 地面站吊舱命令 → PodController 方法（与 ground_station/app.py 白名单对齐）
+_POD_COMMAND_METHODS = {
+    "pod_detect_on": "arm_detection",
+    "pod_auto_lock": "auto_lock",
+    "pod_stop_track": "stop_track",
+    "pod_detect_off": "stop_tracking",
+    "pod_center": "center",
+}
+
+# 搜索扫视换向周期计数中「向右」的半周期序号（0 → 右，1 → 左，交替）
+_SWEEP_RIGHT, _SWEEP_LEFT = 0, 1
+
+# RTL 落地判定：单次等待 in_air 遥测的上限（秒）。MAVSDK 遥测流挂起时
+# `async for` 会永久阻塞并卡死主循环，故必须有兜底（超时 → 下一拍重试）。
+_IN_AIR_TIMEOUT_S = 5.0
+

 def _monotonic_ms() -> int:
    return int(time.monotonic() * 1000)
@@ -45,14 +72,14 @@ class TaskRunner:
    sm       TaskStateMachine       —— update()
    ground   GroundLink             —— latest()（地面注入坐标）
    drone    MAVSDK System           —— action.arm/takeoff/return_to_launch/land/disarm
-    vision_fn callable->bool         —— 我方视觉第二判据（可选，require_vision 时用）
    """

    def __init__(self, cfg, sm_cfg: TaskStateConfig, px4, bridge, sm: TaskStateMachine,
-                 ground, drone, vision_fn: Optional[Callable[[], bool]] = None,
+                 ground, drone,
                 now_ms_fn: Callable[[], int] = _monotonic_ms,
                 logger: Optional[TaskLogger] = None,
-                 pod_control=None) -> None:
+                 pod_control=None, land_instead_of_rtl: bool = False,
+                 return_cfg: Optional[ReturnConfig] = None) -> None:
        self._cfg = cfg
        self._sm_cfg = sm_cfg
        self._px4 = px4
@@ -60,14 +87,32 @@ class TaskRunner:
        self._sm = sm
        self._ground = ground
        self._drone = drone
-        self._vision_fn = vision_fn
        self._now = now_ms_fn
        self._logger = logger  # 结构化任务日志（可选，B 阶段）
        self._pod_control = pod_control  # 吊舱识别/锁定控制（可选，真机）
+        self._land_instead_of_rtl = land_instead_of_rtl  # 收尾改用就地降落
+        self._return_cfg = return_cfg or ReturnConfig()
        self._centered_since_ms: Optional[int] = None
        self._rtl_requested = False
        self._emergency = False
        self._rtl_done = False
+        # SEARCH 扫视状态：当前扫向（右/左）与本次扫向的起始时刻
+        self._sweep_dir = _SWEEP_RIGHT
+        self._sweep_since_ms: Optional[int] = None
+        self._pod_detection_on = False  # 吊舱识别/自动锁定只开一次（防重发 0x07）
+        self._takeoff_done = False      # 解锁/起飞只执行一次（防空中重复起飞）
+        self._in_air_stream = None      # in_air 遥测流复用（避免每拍重新订阅）
+        self._rtl_entered_ms: Optional[int] = None  # 进入 RTL 时刻（落地等待上限用）
+        # --- 自定义返航（回起飞点 → 恢复初始航向 → 降落）---
+        self._home_lat: Optional[float] = None   # 起飞点纬度（解锁时刻记录）
+        self._home_lon: Optional[float] = None
+        self._initial_yaw_rad: Optional[float] = None  # 起飞时刻航向
+        self._bridge_started = False             # 是否已进入 offboard（未起飞则不能自主返航）
+        self._rtl_phase = _RTL_IDLE
+        self._return_ctrl: Optional[HomingController] = None
+        self._return_started_ms: Optional[int] = None
+        self._yaw_started_ms: Optional[int] = None
+        self._land_cmd_sent = False

    # --- 外部控制 -----------------------------------------------------------
    def request_rtl(self) -> None:
@@ -78,6 +123,43 @@ class TaskRunner:
        """外部触发紧急终止（丢控 / RC 接管 / 失控）。"""
        self._emergency = True

+    # --- 地面站命令分派 -----------------------------------------------------
+    def _apply_ground_commands(self) -> None:
+        """排空地面站命令队列并逐条分派（主循环每拍调用）。"""
+        for name in self._ground.drain_commands():
+            self._apply_command(name)
+
+    def _apply_command(self, name: str) -> None:
+        """分派一条地面站命令；未知命令名忽略并警告。"""
+        if name == "rtl":
+            self.request_rtl()
+        elif name == "emergency":
+            self.trigger_emergency()
+        elif name in _POD_COMMAND_METHODS:
+            if self._pod_control is None:
+                logger.warning("[Task] 地面站命令 %s 被忽略：未配置吊舱控制（SITL）",
+                               name)
+                return
+            try:
+                getattr(self._pod_control, _POD_COMMAND_METHODS[name])()
+            except Exception as exc:
+                logger.warning("[Task] 吊舱命令 %s 执行失败: %s", name, exc)
+                return
+        else:
+            logger.warning("[Task] 未知地面站命令: %s", name)
+            return
+        logger.info("[Task] 地面站命令: %s", name)
+        if self._logger is not None:
+            self._logger.log_event(self._now(), "command", name=name)
+
+    def _log_fc_messages(self) -> None:
+        """把飞控状态文本（STATUSTEXT）落任务日志并输出控制台（每拍调用）。"""
+        for sev, text in self._px4.take_fc_messages():
+            logger.info("[FC] %s: %s", sev, text)
+            if self._logger is not None:
+                self._logger.log_event(self._now(), "fc_status",
+                                       severity=sev, text=text)
+
    @property
    def state(self) -> TaskState:
        return self._sm.state
@@ -92,24 +174,29 @@ class TaskRunner:
        await self._px4.start_telemetry()  # 幂等：先拿遥测，供起飞到位/距离计算
        if self._logger is not None:
            self._logger.open()
+            # 记初始状态：页面「任务状态」取日志最后一个 state 事件，缺此行会显示上次旧状态
+            self._logger.log_event(self._now(), "state",
+                                   from_state=self._sm.state.value,
+                                   to_state=self._sm.state.value,
+                                   reason="任务启动")
        period_s = 1.0 / self._sm_cfg.loop_frequency_hz
        try:
            while not stop_event.is_set():
                now_ms = self._now()
-                inputs = self._collect_inputs(now_ms)
-                tr = self._sm.update(inputs, now_ms)
-                if tr.occurred:
-                    logger.info("[Task] %s -> %s (%s)",
-                                tr.from_state.value, tr.to_state.value, tr.reason)
+                try:
+                    await self._iteration(now_ms)
+                except asyncio.CancelledError:
+                    raise
+                except Exception as exc:
+                    # 单周期异常不得让任务带着 offboard 指令静默停摆：转紧急 → 下一拍
+                    # 状态机转 RTL（与 bridge.run() 的按周期护栏同思路，但这里必须
+                    # 升级为返航，因为再没有别的循环会接管飞机）。
+                    logger.error("[Task] 任务周期异常，转紧急返航: %s", exc)
+                    self._emergency = True
                    if self._logger is not None:
-                        self._logger.log_event(
-                            now_ms, "state", from_state=tr.from_state.value,
-                            to_state=tr.to_state.value, reason=tr.reason)
-                    await self._on_enter(tr.to_state, now_ms)
-                out = await self._tick(now_ms)
-                if self._logger is not None:
-                    self._logger.log_telemetry(
-                        now_ms, self._build_telemetry_record(now_ms, inputs, out))
+                        self._logger.log_event(self._now(), "iteration_error",
+                                               error=f"{type(exc).__name__}: {exc}")
+                    await self._safe_abort()
                if self._finished():
                    break
                await asyncio.sleep(period_s)
@@ -118,10 +205,52 @@ class TaskRunner:
            if self._logger is not None:
                self._logger.close()

+    async def _iteration(self, now_ms: int) -> None:
+        """单周期：命令 → 快照 → 状态机 → 进入动作 → 持续动作 → 日志。"""
+        self._apply_ground_commands()  # 地面站命令先于快照采集生效
+        self._log_fc_messages()
+        inputs = self._collect_inputs(now_ms)
+        tr = self._sm.update(inputs, now_ms)
+        if tr.occurred:
+            logger.info("[Task] %s -> %s (%s)",
+                        tr.from_state.value, tr.to_state.value, tr.reason)
+            if self._logger is not None:
+                self._logger.log_event(
+                    now_ms, "state", from_state=tr.from_state.value,
+                    to_state=tr.to_state.value, reason=tr.reason)
+            await self._on_enter(tr.to_state, now_ms)
+        out = await self._tick(now_ms)
+        if self._logger is not None:
+            self._logger.log_telemetry(
+                now_ms, self._build_telemetry_record(now_ms, inputs, out))
+
+    async def _safe_abort(self) -> None:
+        """异常兜底：直接进入 RTL 流程（不依赖状态机下一拍转移）。
+
+        异常可能来自采集/控制律/日志任意环节，下一拍未必能正常跑完转移；因此在
+        兜底里显式走一次 RTL 进入动作（停机 + 返航），保证飞机不会停在 offboard
+        速度指令上。
+        """
+        if self._sm.state is TaskState.RTL:
+            return
+        self._sm.force_state(TaskState.RTL, self._now())
+        self._rtl_entered_ms = self._now()
+        if self._pod_control is not None:
+            try:
+                self._pod_control.stop_tracking()
+            except Exception:
+                pass
+        try:
+            # 兜底不走自定义返航：异常后不再信任自身状态/定位，直接交给 PX4 原生 RTL
+            await self._begin_rtl(self._now(), force_fallback=True)
+        except Exception as exc:
+            logger.error("[Task] 异常兜底返航失败: %s", exc)
+
    # --- 输入采集 -----------------------------------------------------------
    def _collect_inputs(self, now_ms: int) -> TaskInputs:
        """聚合 PX4 / 注入坐标 / 吊舱 / 视觉为一份 TaskInputs 快照。"""
        own = self._px4.own_state()
+        self._maybe_record_home(own)   # 起飞阶段定位可能晚到：每拍补记一次

        injected = self._ground.latest()
        target_injected = bool(
@@ -135,10 +264,12 @@ class TaskRunner:
                own.latitude_deg, own.longitude_deg,
                injected.latitude_deg, injected.longitude_deg)

+        # 起飞到达判据带容差：悬停/高度估计通常略低于设定目标（实测 SITL 4.99m vs 5.0m），
+        # 无容差会差厘米误判"起飞超时"→ 真机上更明显
        takeoff_reached = (own.relative_altitude_m
-                           >= self._sm_cfg.takeoff_altitude_m)
+                           >= self._sm_cfg.takeoff_altitude_m
+                           - self._sm_cfg.takeoff_reached_tolerance_m)
        target_locked = self._bridge.pod_locked(now_ms)
-        vision = bool(self._vision_fn and self._vision_fn())
        centered, centered_s = self._update_centered(now_ms)

        return TaskInputs(
@@ -146,7 +277,6 @@ class TaskRunner:
            distance_to_target_m=distance,
            takeoff_reached=takeoff_reached,
            target_locked=target_locked,
-            vision_has_target=vision,
            target_centered=centered,
            centered_duration_s=centered_s,
            rtl_requested=self._rtl_requested,
@@ -174,6 +304,12 @@ class TaskRunner:

    # --- 状态进入动作 -------------------------------------------------------
    async def _on_enter(self, state: TaskState, now_ms: int) -> None:
+        """进入状态的**一次性**动作。
+
+        注意：这些动作只在「首次」进入时执行——任务中途可能需要多次经过 GOTO/
+        SEARCH（丢目标回搜），重复 `offboard.start()` / `arm_detection()`
+        （0x07 目标识别开会打断正在进行的跟踪）会造成真机自激，故以标志位守卫。
+        """
        if state is TaskState.ARM_TAKEOFF:
            await self._arm_takeoff()
        elif state is TaskState.GOTO:
@@ -181,17 +317,50 @@ class TaskRunner:
                logger.error("[Task] offboard 启动失败，紧急终止")
                self._emergency = True
                return
+            self._bridge_started = True
            self._bridge.set_heading_servo_enabled(False)  # L2 地理引导
-            if self._pod_control is not None:
-                self._pod_control.arm_detection()  # 真机：开启目标识别 + 自动锁定
+            self._start_pod_detection()                    # 首次：开识别 + 自动锁定
+        elif state is TaskState.SEARCH:
+            self._bridge.set_heading_servo_enabled(False)  # 搜索不用控制律
+            self._sweep_dir = _SWEEP_RIGHT                 # 每次进入从右向起扫
+            self._sweep_since_ms = now_ms
        elif state is TaskState.TRACK:
-            self._bridge.set_heading_servo_enabled(True)   # L1 航向伺服
+            self._bridge.set_heading_servo_enabled(True)   # L1 视觉追踪 + 缓慢靠近
        elif state is TaskState.APPROACH:
            pass  # 悬停由 _tick 持续发零速
        elif state is TaskState.RTL:
+            self._rtl_entered_ms = now_ms
            if self._pod_control is not None:
-                self._pod_control.stop_tracking()  # 真机：停止跟踪 + 识别关（bridge 停机前）
-            await self._start_rtl()
+                self._pod_control.stop_tracking()  # 真机：停止跟踪 + 识别关
+            await self._begin_rtl(now_ms)
+
+    def _start_pod_detection(self) -> None:
+        """首次到达引导/搜索阶段时开启吊舱目标识别 + 自动锁定（只发一次）。"""
+        if self._pod_control is None or self._pod_detection_on:
+            return
+        self._pod_detection_on = True
+        self._pod_control.arm_detection()
+
+    # --- SEARCH 扫视 --------------------------------------------------------
+    def _search_yaw_rate(self, now_ms: int) -> float:
+        """SEARCH 扫视角速度：按 sweep_reverse_s 交替左右，0 = 单向扫视。
+
+        吊舱相机刚性固连机体（无电机、不独立转动），故只能靠机头旋转改变观察
+        方向；扫视期间零线速度，飞机保持悬停在参考点附近。
+        """
+        rate = self._sm_cfg.sweep_rate_dps
+        if rate == 0.0:
+            return 0.0
+        period_s = self._sm_cfg.sweep_reverse_s
+        if period_s > 0.0:
+            if self._sweep_since_ms is None:
+                self._sweep_since_ms = now_ms
+            elif (now_ms - self._sweep_since_ms) >= period_s * 1000.0:
+                self._sweep_since_ms = now_ms
+                self._sweep_dir = (_SWEEP_LEFT if self._sweep_dir == _SWEEP_RIGHT
+                                   else _SWEEP_RIGHT)
+        sign = 1.0 if self._sweep_dir == _SWEEP_RIGHT else -1.0
+        return sign * rate

    # --- 每周期持续动作 -----------------------------------------------------
    async def _tick(self, now_ms: int):
@@ -199,10 +368,13 @@ class TaskRunner:
        state = self._sm.state
        if state in (TaskState.GOTO, TaskState.TRACK):
            return await self._bridge.step(now_ms)
+        elif state is TaskState.SEARCH:
+            # 原地偏航扫视（同时 poll 吊舱，保遥测新鲜，否则搜到也切不进 TRACK）
+            await self._bridge.search_step(now_ms, self._search_yaw_rate(now_ms))
        elif state is TaskState.APPROACH:
            await self._px4.send_brake_hover(now_ms)  # 悬停目标上空（保 offboard 心跳）
        elif state is TaskState.RTL:
-            await self._check_landed()
+            await self._tick_rtl(now_ms)
        return None
        # WAIT / ARM_TAKEOFF：无持续动作（待命 / 等起飞高度）

@@ -238,6 +410,17 @@ class TaskRunner:

    # --- 飞行编排 -----------------------------------------------------------
    async def _arm_takeoff(self) -> None:
+        """解锁 + 起飞到悬停高度（每次任务只执行一次）。
+
+        重复执行会导致：机已在空中时再次 arm/takeoff → PX4 拒绝甚至触发保护，
+        且日志看起来像"重新起飞"。故用标志位守卫，重复进入只告警不改动飞机。
+        """
+        if self._takeoff_done:
+            logger.warning("[Task] 已在飞行中，忽略重复的解锁/起飞请求")
+            return
+        self._takeoff_done = True
+        # 解锁前记录起飞点与初始航向（自定义返航要用；PX4 的 home 此时已固定）
+        self._record_home()
        try:
            await self._drone.action.arm()
            await self._drone.action.set_takeoff_altitude(
@@ -254,9 +437,175 @@ class TaskRunner:
                self._logger.log_event(self._now(), "takeoff_failed",
                                       error=str(exc))

-    async def _start_rtl(self) -> None:
+    def _record_home(self) -> bool:
+        """尝试记录起飞点与初始航向；成功返回 True（可重复调用，只记一次）。"""
+        if self._home_lat is not None:
+            return True
        try:
-            await self._bridge.stop()  # 先零速刹车 + 停 offboard/遥测/串口
+            own = self._px4.own_state()
+        except Exception as exc:            # 遥测不可用时不阻塞起飞
+            logger.warning("[Task] 读取起飞点失败: %s", exc)
+            return False
+        return self._maybe_record_home(own)
+
+    def _maybe_record_home(self, own) -> bool:
+        """在**起飞前/起飞中**用首个有效定位记下起飞点与初始航向。
+
+        ⚠ SITL 实测踩到的坑：解锁发生在地面站坐标到达后约 0.3s，此时 `gps_info`
+        （2Hz）可能还没到，`position_valid` 仍为 False——若只在解锁那一刻记一次，
+        起飞点就会丢失、自定义返航只能退回 PX4 RTL。故起飞阶段每拍补记。
+        只接受 WAIT / ARM_TAKEOFF 阶段的定位：进入 GOTO 后飞机已在空中，
+        那时的位置不该再当作"起飞点"。
+        """
+        if self._home_lat is not None:
+            return True
+        if not own.position_valid:
+            return False
+        if self._sm.state not in (TaskState.WAIT, TaskState.ARM_TAKEOFF):
+            return False
+        self._home_lat = own.latitude_deg
+        self._home_lon = own.longitude_deg
+        self._initial_yaw_rad = own.yaw_rad
+        logger.info("[Task] 起飞点已记录: (%.7f, %.7f) 初始航向 %.1f°",
+                    self._home_lat, self._home_lon,
+                    math.degrees(self._initial_yaw_rad))
+        return True
+
+    # --- 返航（自定义：回起飞点 → 恢复初始航向 → 降落）---------------------
+    def _custom_return_possible(self) -> bool:
+        """能否走自定义返航（已起飞 + 有起飞点 + 位置有效）。"""
+        if not self._return_cfg.enabled:
+            return False
+        if not self._bridge_started:
+            return False                    # 没进过 offboard：还在待命/起飞段
+        if self._home_lat is None or self._home_lon is None:
+            return False
+        if self._initial_yaw_rad is None:
+            return False
+        try:
+            return bool(self._px4.own_state().position_valid)
+        except Exception:
+            return False
+
+    async def _begin_rtl(self, now_ms: int, force_fallback: bool = False) -> None:
+        """进入返航：选择「自定义返航」或「就地降落」或「PX4 RTL 兜底」。"""
+        if self._land_instead_of_rtl:
+            self._rtl_phase = _RTL_LAND
+            await self._start_land()
+            return
+        if force_fallback or not self._custom_return_possible():
+            reason = "异常兜底" if force_fallback else "未起飞/无起飞点"
+            logger.info("[Task] 使用 PX4 原生 RTL（%s）", reason)
+            await self._fallback_rtl()
+            return
+        own = self._px4.own_state()
+        # 转场高度：取"当前高度"与"起飞高度"的较大者 —— 不低于任务高度，
+        # 也不会为了返航额外爬高（这正是原生 RTL 会做的事）。
+        transit_alt = max(own.relative_altitude_m,
+                          self._sm_cfg.takeoff_altitude_m)
+        self._return_ctrl = HomingController(
+            self._return_cfg, self._home_lat, self._home_lon,
+            self._initial_yaw_rad, transit_alt)
+        self._return_started_ms = now_ms
+        self._rtl_phase = _RTL_RETURN
+        logger.info("[Task] 自定义返航：回起飞点 (%.7f, %.7f)，转场高度 %.1fm，"
+                    "到位后恢复航向 %.1f° 并降落",
+                    self._home_lat, self._home_lon, transit_alt,
+                    math.degrees(self._initial_yaw_rad))
+        if self._logger is not None:
+            self._logger.log_event(
+                now_ms, "return_home", home_lat=self._home_lat,
+                home_lon=self._home_lon, transit_alt_m=transit_alt,
+                initial_yaw_deg=math.degrees(self._initial_yaw_rad))
+
+    async def _return_step(self, now_ms: int) -> None:
+        """转场阶段：朝起飞点飞，保持转场高度。"""
+        cfg = self._return_cfg
+        if (self._return_started_ms is not None
+                and now_ms - self._return_started_ms >= cfg.timeout_s * 1000.0):
+            logger.warning("[Task] 返航转场超时 %.0fs，退回 PX4 RTL", cfg.timeout_s)
+            if self._logger is not None:
+                self._logger.log_event(now_ms, "return_timeout",
+                                       timeout_s=cfg.timeout_s)
+            await self._fallback_rtl()
+            return
+        own = self._px4.own_state()
+        if not own.position_valid:
+            # 定位丢失：不能自主返航，先刹停等恢复（不盲目飞）
+            logger.warning("[Task] 返航中定位失效，刹停等待")
+            await self._px4.send_brake_hover(now_ms)
+            return
+        cmd = self._return_ctrl.update(own.latitude_deg, own.longitude_deg,
+                                       own.yaw_rad, own.relative_altitude_m)
+        out = ControlOutput(mode=ControlMode.VELOCITY_HEADING, valid=True,
+                            vx_body_mps=cmd.vx_mps, vz_mps=cmd.vz_mps,
+                            yaw_rate_dps=cmd.yaw_rate_dps)
+        await self._px4.send_velocity_heading(out, now_ms)
+        if cmd.arrived:
+            logger.info("[Task] 已回到起飞点（距 %.1fm），开始恢复初始航向",
+                        cmd.distance_m)
+            self._rtl_phase = _RTL_YAW
+            self._yaw_started_ms = now_ms
+
+    async def _yaw_align_step(self, now_ms: int) -> None:
+        """恢复初始航向阶段：原地转回起飞时刻的航向。"""
+        cfg = self._return_cfg
+        own = self._px4.own_state()
+        err, rate = self._return_ctrl.yaw_align(own.yaw_rad)
+        # 保持转场高度（不额外爬升）
+        out = ControlOutput(mode=ControlMode.VELOCITY_HEADING, valid=True,
+                            vx_body_mps=0.0,
+                            vz_mps=self._return_ctrl.alt_hold_vz(
+                                own.relative_altitude_m),
+                            yaw_rate_dps=rate)
+        await self._px4.send_velocity_heading(out, now_ms)
+
+        timed_out = (self._yaw_started_ms is not None
+                     and now_ms - self._yaw_started_ms
+                     >= cfg.yaw_align_timeout_s * 1000.0)
+        if abs(err) <= cfg.yaw_align_tolerance_deg:
+            logger.info("[Task] 航向已恢复（误差 %.1f°），开始降落", err)
+        elif timed_out:
+            logger.warning("[Task] 航向对齐超时（误差 %.1f°），仍进入降落", err)
+        else:
+            return
+        if self._logger is not None:
+            self._logger.log_event(now_ms, "yaw_restored",
+                                   err_deg=err, timed_out=timed_out)
+        self._rtl_phase = _RTL_LAND
+        await self._start_land()
+
+    async def _start_land(self) -> None:
+        """退出 offboard 并垂直降落（不爬升）。"""
+        if self._land_cmd_sent:
+            return
+        self._land_cmd_sent = True
+        # 只退 offboard、保留遥测：落地判定与高度曲线都依赖遥测（SITL 实测过：
+        # 若在这里就全停，日志会冻结在降落前的高度，看不出下降）。
+        stop_for_landing = getattr(self._bridge, "landing_stop", None)
+        try:
+            if stop_for_landing is not None:
+                await stop_for_landing()
+            else:
+                await self._bridge.stop()
+        except Exception as exc:
+            logger.warning("[Task] 停机异常: %s", exc)
+        try:
+            await self._drone.action.land()
+        except Exception as exc:
+            logger.warning("[Task] 降落指令失败，尝试 PX4 RTL 兜底: %s", exc)
+            try:
+                await self._drone.action.return_to_launch()
+            except Exception:
+                pass
+        if self._logger is not None:
+            self._logger.log_event(self._now(), "rtl", action="land")
+
+    async def _fallback_rtl(self) -> None:
+        """兜底：停机后交给 PX4 原生 RTL（会先爬升到 RTL_RETURN_ALT）。"""
+        self._rtl_phase = _RTL_FALLBACK
+        try:
+            await self._bridge.stop()
        except Exception as exc:
            logger.warning("[Task] 停机异常: %s", exc)
        try:
@@ -268,21 +617,57 @@ class TaskRunner:
            except Exception:
                pass
        if self._logger is not None:
-            self._logger.log_event(self._now(), "rtl")
+            self._logger.log_event(self._now(), "rtl", action="rtl")
+
+    async def _tick_rtl(self, now_ms: int) -> None:
+        """RTL 状态机：按阶段推进（转场 → 恢复航向 → 降落 / 兜底）。"""
+        phase = self._rtl_phase
+        if phase == _RTL_RETURN:
+            await self._return_step(now_ms)
+        elif phase == _RTL_YAW:
+            await self._yaw_align_step(now_ms)
+        elif phase == _RTL_LAND:
+            await self._check_landed()
+        elif phase == _RTL_FALLBACK:
+            await self._check_landed()
+        # 整体落地等待上限：任何阶段都不得无限等
+        if (not self._rtl_done and self._rtl_entered_ms is not None
+                and now_ms - self._rtl_entered_ms
+                >= self._sm_cfg.rtl_timeout_s * 1000.0):
+            logger.warning("[Task] RTL 等待落地超过 %.0fs，结束任务（无法确认落地）",
+                           self._sm_cfg.rtl_timeout_s)
+            self._rtl_done = True
+            if self._logger is not None:
+                self._logger.log_event(now_ms, "rtl_timeout",
+                                       timeout_s=self._sm_cfg.rtl_timeout_s)

    async def _check_landed(self) -> None:
-        """RTL 状态每周期取一帧 in_air 遥测；落地后 disarm 并标记完成。"""
+        """RTL 状态每周期取一帧 in_air 遥测；落地后 disarm 并标记完成。
+
+        两个要点：
+          * `in_air()` 是 MAVSDK 遥测流：流挂起时 `async for` 会**永久阻塞**，
+            主循环随之卡死（rtl_done 永不置位）。故加超时兜底，超时下拍重试。
+          * 生成器**复用**（`_in_air_stream`）：每拍新建一个 `in_air()` 会在
+            mavsdk_server 侧反复订阅/退订同一条流（20Hz × 整个 RTL 段），是
+            回调队列的无谓负担；复用后只订阅一次。
+        """
        try:
-            async for is_in_air in self._drone.telemetry.in_air():
-                if not is_in_air and not self._rtl_done:
-                    self._rtl_done = True
-                    try:
-                        await self._drone.action.disarm()
-                    except Exception:
-                        pass
-                break  # 只取最新一帧
+            if self._in_air_stream is None:
+                self._in_air_stream = self._drone.telemetry.in_air()
+            async with asyncio.timeout(_IN_AIR_TIMEOUT_S):
+                async for is_in_air in self._in_air_stream:
+                    if not is_in_air and not self._rtl_done:
+                        self._rtl_done = True
+                        try:
+                            await self._drone.action.disarm()
+                        except Exception:
+                            pass
+                    break  # 只取最新一帧
+        except (TimeoutError, asyncio.TimeoutError):
+            logger.debug("[Task] in_air 遥测超时 %.1fs，下拍重试", _IN_AIR_TIMEOUT_S)
        except Exception:
-            pass
+            # 建流或消费失败都算流不可用：丢弃缓存，下拍重建（不永久卡在坏流上）
+            self._in_air_stream = None

    # --- 收尾 ---------------------------------------------------------------
    def _finished(self) -> bool: