精简仓库结构并增强任务链路：归档双目可选后端、新增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。

utils/task_runner/__init__.py

@@ -1,9 +1,20 @@
 """task_runner —— 任务编排器（预定流程.md 的天空侧任务级状态机）。
 
 把已通的 L1（航向伺服 TrackingControlLaw）与 L2（地理引导 GuidanceLaw）按任务阶段
-编排成一条可运行的全流程：WAIT → ARM_TAKEOFF → GOTO → TRACK → APPROACH → RTL。
+编排成一条可运行的全流程：
+
+    WAIT → ARM_TAKEOFF → GOTO → SEARCH → TRACK → RTL
+
+  * GOTO   ：L2 地理引导飞向**参考点**（地面给的坐标是搜索起点，不是终点）；
+  * SEARCH ：到达参考点后原地偏航扫视，靠吊舱/视觉找目标；超时未发现 → RTL；
+  * TRACK  ：L1 角度直入 + 无距离降级（slow_approach 缓慢靠近）；超时 → RTL；
+  * RTL    ：自定义返航（回起飞点 → 恢复初始航向 → 垂直降落），不爬高；
+             异常/未起飞等情形兜底退回 PX4 原生 RTL。
 
 模块划分：
   * state_machine.py  纯逻辑任务状态机（无 IO，可单测）
-  * runner.py         编排器（MAVSDK 飞行编排 + 状态机驱动 + PodBridge 闭环，A3）
+  * runner.py         编排器（MAVSDK 飞行编排 + 状态机驱动 + PodBridge 闭环）
+  * return_home.py    自定义返航纯逻辑（HomingController：转场/航向/高度）
+  * pod_control.py    真机吊舱识别/锁定指令下发（0x07/0x0F/0x0E/0x08）
+  * log.py            结构化任务日志（事件 JSONL + 遥测 CSV + 简报）
 """

utils/task_runner/log.py

@@ -6,15 +6,21 @@
   * 事件日志（JSONL）：状态机转移、飞行编排事件（解锁/起飞/返航）、异常/紧急。
     低频、带语义、结构化，便于 grep / jq 定位「什么时候发生了什么」。
   * 周期遥测日志（CSV）：每控制周期的位姿 / GPS / 目标 / 控制输出，便于回放与画图。
+  * 简洁任务简报（task_brief.log，纯文本）：一行一事件（阶段切换/地面站命令/
+    飞控告警）+ 运行结束汇总（时长/最高/最近距目标/状态链），供人工阅读与归档；
+    机器日志（JSONL/CSV）格式不变。
 
 时间戳：单调毫秒（now_ms，与运行时代码同源）+ 相对时间 t_s（自首个 now_ms 起）。
-文件 IO 集中在 open / close，热路径只追加一行，不做 flush 决策。
+文件 IO 集中在 open / close；热路径只追加（并立即 flush 落盘——缓冲期间文件不可见，
+地面站页面运行中需要读到最新状态与遥测，2026-10-14 实测）。
 """
 
 import csv
 import json
+import math
 import time
 from dataclasses import dataclass
+from datetime import datetime
 from pathlib import Path
 from typing import Optional
 
@@ -60,13 +66,26 @@ class TaskLogConfig:
     dir: str = "logs"                       # 输出目录（相对工作目录）
     event_file: str = "task_events.jsonl"  # 事件日志
     telemetry_file: str = "task_telemetry.csv"  # 周期遥测日志
+    brief_file: str = "task_brief.log"     # 人类可读任务简报（追加）
     telemetry_every_n: int = 1             # 每 N 周期记一次遥测（1 = 每周期）
+    telemetry_flush_ms: int = 1000         # 遥测落盘间隔（毫秒）；0 = 每行立即 flush
 
 
 def _monotonic_ms() -> int:
     return int(time.monotonic() * 1000)
 
 
+_STATE_CN = {"wait": "待命", "arm_takeoff": "起飞", "goto": "坐标引导",
+             "search": "搜索", "track": "追踪靠近", "approach": "接近悬停",
+             "rtl": "返航"}
+
+
+def _fmt_t_s(t_s: float) -> str:
+    """相对时间 → mm:ss.s（简报用）。"""
+    m, s = divmod(max(0.0, float(t_s or 0.0)), 60.0)
+    return f"{int(m):02d}:{s:04.1f}"
+
+
 class TaskLogger:
     """结构化任务日志。用法：
 
@@ -86,12 +105,20 @@ class TaskLogger:
         self._event_fp = None
         self._tel_fp = None
         self._tel_writer = None
+        self._brief_fp = None
         self._t0_ms: Optional[int] = None
         self._tick = 0
+        self._last_tel_flush_ms: Optional[int] = None  # 遥测上次落盘时刻
+        # 简报汇总统计（随遥测行/事件更新，close 时写一行汇总）
+        self._n_events = 0
+        self._last_t_s = 0.0
+        self._max_rel_alt_m: Optional[float] = None
+        self._min_dist_m: Optional[float] = None
+        self._state_chain: list = []
 
     # --- 生命周期 -----------------------------------------------------------
     def open(self) -> None:
-        """创建输出目录并打开两个日志文件；disabled 时跳过。"""
+        """创建输出目录并打开日志文件；disabled 时跳过。"""
         if not self._cfg.enabled:
             return
         out_dir = Path(self._cfg.dir)
@@ -103,11 +130,22 @@ class TaskLogger:
         self._tel_writer = csv.writer(self._tel_fp)
         if (out_dir / self._cfg.telemetry_file).stat().st_size == 0:
             self._tel_writer.writerow(_TELEMETRY_FIELDS)
+        self._brief_fp = (out_dir / self._cfg.brief_file).open(
+            "a", encoding="utf-8")
+        self._brief_fp.write(
+            f"===== 任务开始 {datetime.now().strftime('%Y-%m-%d %H:%M:%S')} =====\n")
+        self._last_tel_flush_ms = None
         self._event_fp.flush()
         self._tel_fp.flush()
+        self._brief_fp.flush()
 
     def close(self) -> None:
-        """flush 并关闭两个日志文件。"""
+        """写简报汇总并关闭全部日志文件。"""
+        if self._brief_fp is not None:
+            self._brief_fp.write(self._brief_summary() + "\n\n")
+            self._brief_fp.flush()
+            self._brief_fp.close()
+            self._brief_fp = None
         if self._event_fp is not None:
             self._event_fp.flush()
             self._event_fp.close()
@@ -120,16 +158,32 @@ class TaskLogger:
 
     # --- 记录 ---------------------------------------------------------------
     def log_event(self, now_ms: int, event_type: str, **fields) -> None:
-        """追加一条 JSONL 事件。"""
+        """追加一条 JSONL 事件并**立即落盘**（事件低频且语义关键：地面站要实时看阶段）。
+
+        注意：遥测行不在这里 flush——见 log_telemetry 的节流落盘说明。
+        """
         if self._event_fp is None:
             return
         record = {"type": event_type, "ts_ms": now_ms,
                   "t_s": self._rel_s(now_ms)}
         record.update(fields)
         self._event_fp.write(json.dumps(record, ensure_ascii=False) + "\n")
+        self._event_fp.flush()
+        self._n_events += 1
+        if self._brief_fp is not None:
+            self._brief_fp.write(self._brief_text(record) + "\n")
+            self._brief_fp.flush()
 
     def log_telemetry(self, now_ms: int, rec: TelemetryRecord) -> None:
-        """追加一行遥测（按 telemetry_every_n 节流）。"""
+        """追加一行遥测（按 telemetry_every_n 节流）。
+
+        落盘策略：**写入靠 Python 缓冲、按 telemetry_flush_ms 定期 flush**，
+        而不是每行 fsync 式 flush。原因：本方法在主控制循环里以 20Hz 调用，
+        每行都 flush 等于每秒 20 次同步磁盘 IO，在树莓派 SD 卡上会阻塞事件
+        循环（实测 MAVSDK 回调队列因此积压告警）；事件行仍即时落盘，故页面
+        的阶段显示不受影响，只有图表数据最多滞后 telemetry_flush_ms。
+        close() 会做最后一次 flush，进程正常退出不丢数据。
+        """
         if self._tel_writer is None:
             return
         self._tick += 1
@@ -137,8 +191,75 @@ class TaskLogger:
             return
         rec.t_s = self._rel_s(now_ms)
         self._tel_writer.writerow([getattr(rec, f) for f in _TELEMETRY_FIELDS])
+        self._flush_telemetry(now_ms)
+        # 简报汇总统计（只随真正落盘的行更新）
+        self._last_t_s = rec.t_s
+        if math.isfinite(rec.relative_altitude_m):
+            if (self._max_rel_alt_m is None
+                    or rec.relative_altitude_m > self._max_rel_alt_m):
+                self._max_rel_alt_m = rec.relative_altitude_m
+        dist = rec.distance_to_target_m
+        if math.isfinite(dist) and dist > 0:
+            if self._min_dist_m is None or dist < self._min_dist_m:
+                self._min_dist_m = dist
+        if rec.state and (not self._state_chain
+                          or self._state_chain[-1] != rec.state):
+            if len(self._state_chain) < 32:
+                self._state_chain.append(rec.state)
+
+    # --- 简报（人类可读，一行一事件）----------------------------------------
+    @staticmethod
+    def _brief_text(record: dict) -> str:
+        t = _fmt_t_s(record.get("t_s", 0.0))
+        etype = record.get("type", "?")
+        if etype == "state":
+            to = _STATE_CN.get(record.get("to_state"),
+                               record.get("to_state", "?"))
+            if record.get("from_state") == record.get("to_state"):
+                return f"[{t}] 任务启动（当前: {to}）"
+            frm = _STATE_CN.get(record.get("from_state"),
+                                record.get("from_state", "?"))
+            return f"[{t}] 阶段: {frm} → {to}（{record.get('reason', '')}）"
+        if etype == "command":
+            return f"[{t}] 地面站命令: {record.get('name', '')}"
+        if etype == "fc_status":
+            return (f"[{t}] 飞控 {record.get('severity', '')}: "
+                    f"{str(record.get('text', '')).strip()}")
+        if etype == "takeoff":
+            return f"[{t}] 起飞至 {record.get('altitude_m', '?')} m"
+        if etype == "takeoff_failed":
+            return f"[{t}] 起飞失败: {record.get('error', '')}"
+        if etype == "rtl":
+            return f"[{t}] 开始返航"
+        fields = [f"{k}={v}" for k, v in record.items()
+                  if k not in ("type", "ts_ms", "t_s")]
+        return f"[{t}] {etype}: {', '.join(fields)}"
+
+    def _brief_summary(self) -> str:
+        parts = [f"时长 {self._last_t_s:.1f}s"]
+        if self._max_rel_alt_m is not None:
+            parts.append(f"最高 {self._max_rel_alt_m:.1f}m")
+        if self._min_dist_m is not None:
+            parts.append(f"最近距目标 {self._min_dist_m:.1f}m")
+        parts.append(f"事件 {self._n_events} 条")
+        if self._state_chain:
+            parts.append("状态链 " + "→".join(
+                _STATE_CN.get(s, s) for s in self._state_chain))
+        return "----- 本次运行汇总: " + " · ".join(parts) + " -----"
 
     # --- 内部 ---------------------------------------------------------------
+    def _flush_telemetry(self, now_ms: int) -> None:
+        """按 telemetry_flush_ms 节流落盘遥测（0 = 每行立即落盘）。"""
+        interval = self._cfg.telemetry_flush_ms
+        if interval <= 0:
+            self._tel_fp.flush()
+            self._last_tel_flush_ms = now_ms
+            return
+        if (self._last_tel_flush_ms is None
+                or now_ms - self._last_tel_flush_ms >= interval):
+            self._tel_fp.flush()
+            self._last_tel_flush_ms = now_ms
+
     def _rel_s(self, now_ms: int) -> float:
         if self._t0_ms is None:
             self._t0_ms = now_ms

utils/task_runner/pod_control.py

@@ -8,7 +8,8 @@
   * 0x08 目标识别关（真正退出识别）
 
 本模块把这些指令序列封装成 PodController，供 TaskRunner 在 GOTO / RTL 阶段调用。
-SITL 用 VirtualPod（直接合成锁定态），不注入 PodController。
+另含地面站手动命令（stop_track / auto_lock / center），与 pod_console.py 按键
+u / a / c 语义对齐。SITL 用 VirtualPod（直接合成锁定态），不注入 PodController。
 """
 
 from pod_bridge.protocol import ControlCode, ControlCommand
@@ -37,3 +38,18 @@ class PodController:
         """停止跟踪 + 关闭目标识别（RTL 前调用）。"""
         self._bridge.send_control(ControlCommand(code=ControlCode.STOP_TRACK))
         self._bridge.send_control(ControlCommand(code=ControlCode.TARGET_DETECT_OFF))
+
+    def stop_track(self) -> None:
+        """停止跟踪（只停当前这一次，0x0E；0x0F 常驻自动锁定仍生效）。"""
+        self._bridge.send_control(ControlCommand(code=ControlCode.STOP_TRACK))
+
+    def auto_lock(self) -> None:
+        """发现目标自动锁定 + 识别转跟踪（0x0F + 0x10）。"""
+        self._bridge.send_control(ControlCommand(
+            code=ControlCode.AUTO_LOCK_ON_FOUND))
+        self._bridge.send_control(ControlCommand(
+            code=ControlCode.DETECT_TO_TRACK))
+
+    def center(self) -> None:
+        """手动归中（0x2B）。"""
+        self._bridge.send_control(ControlCommand(code=ControlCode.CENTER))

utils/task_runner/return_home.py

@@ -0,0 +1,130 @@
+"""return_home.py —— 自定义返航（回起飞点 + 恢复初始航向 + 就地降落）的纯逻辑控制。
+
+背景：PX4 自带的 RTL 会先爬到 `RTL_RETURN_ALT`（默认数十米）再飞回、再下降到
+`RTL_DESCEND_ALT` 后落地——即"飞很高再降落"。本模块给出替代方案：
+
+  RETURN 阶段：保持在**转场高度**（取进入返航时的高度与起飞高度的较大者，
+              绝不比任务高度更高）飞向起飞点，机头朝目标方向；
+  YAW 阶段   ：到位后原地把机头转回**起飞时刻的航向**；
+  LAND 阶段   ：交给飞控 `land()` 垂直降落（不爬升）。
+
+设计约束（与项目铁律一致）：
+  * 纯逻辑：输入为载机位置/航向/相对高度，输出速度与角速度意图，不碰 IO，
+    可确定性单测；实际下发由 runner 负责。
+  * NED 语义：vz 下正。目标高度高于当前 → vz 为负（爬升）。
+  * 只做"航位推算"级别的控制：无距离传感器时不需要它（返航点是已知识别坐标），
+    因此这里用比例控制 + 限幅即可，不引入积分项（避免返航中积分饱和）。
+"""
+
+import math
+from dataclasses import dataclass
+
+from pod_bridge.guidance import haversine_distance_m, initial_bearing_deg
+from visual_track_control.rate_limiter import clamp
+
+
+def wrap_deg_180(deg: float) -> float:
+    """角度回绕到 [-180, 180)。"""
+    d = math.fmod(deg + 180.0, 360.0)
+    if d < 0.0:
+        d += 360.0
+    return d - 180.0
+
+
+@dataclass
+class ReturnConfig:
+    """自定义返航参数（全外置；数值为占位，须按机体/场地标定）。"""
+
+    enabled: bool = True              # False → 退回 PX4 原生 RTL
+    speed_mps: float = 2.0            # 转场水平速度上限
+    arrival_radius_m: float = 3.0     # 距起飞点该半径内视为"已回到起飞点"
+    timeout_s: float = 120.0          # 转场阶段超时（超时 → 退回 PX4 RTL 兜底）
+    yaw_kp: float = 1.0               # 航向误差 → 偏航角速度（dps/deg）
+    yaw_rate_limit_dps: float = 20.0  # 偏航角速度上限
+    align_kp: float = 1.0             # 恢复初始航向阶段的比例增益
+    align_rate_limit_dps: float = 15.0
+    yaw_align_tolerance_deg: float = 5.0  # 航向对齐容差
+    yaw_align_timeout_s: float = 15.0     # 航向对齐超时（超时也进入降落）
+    alt_kp: float = 0.5               # 高度误差 → vz（m/s per m）
+    alt_limit_mps: float = 0.5        # 垂直速度上限（与 L1 vertical 限速同量级）
+
+
+@dataclass
+class HomingOutput:
+    """单周期归航意图（机体系前向 + 垂直 NED + 偏航角速度）。"""
+
+    vx_mps: float = 0.0
+    vz_mps: float = 0.0
+    yaw_rate_dps: float = 0.0
+    distance_m: float = math.inf
+    bearing_err_deg: float = 0.0
+    arrived: bool = False
+
+
+class HomingController:
+    """把载机当前位置/航向/高度 → 飞回起飞点的速度与航向意图。
+
+    用法：进入返航时用起飞点经纬度、起飞时刻航向、转场高度构造一次；
+    之后每周期 `update(lat, lon, yaw_rad, rel_alt_m)`。
+    """
+
+    def __init__(self, cfg: ReturnConfig, home_lat_deg: float,
+                 home_lon_deg: float, initial_yaw_rad: float,
+                 transit_alt_m: float) -> None:
+        self._cfg = cfg
+        self._home_lat = float(home_lat_deg)
+        self._home_lon = float(home_lon_deg)
+        self._initial_yaw_rad = float(initial_yaw_rad)
+        self._transit_alt_m = float(transit_alt_m)
+
+    @property
+    def home(self) -> tuple[float, float]:
+        return self._home_lat, self._home_lon
+
+    @property
+    def initial_yaw_rad(self) -> float:
+        return self._initial_yaw_rad
+
+    def update(self, lat_deg: float, lon_deg: float, yaw_rad: float,
+               rel_alt_m: float) -> HomingOutput:
+        """转场阶段单周期意图：朝起飞点飞，同时保持转场高度。"""
+        cfg = self._cfg
+        dist = haversine_distance_m(lat_deg, lon_deg,
+                                    self._home_lat, self._home_lon)
+        bearing = initial_bearing_deg(lat_deg, lon_deg,
+                                      self._home_lat, self._home_lon)
+        err = wrap_deg_180(bearing - math.degrees(yaw_rad))
+
+        yaw_rate = clamp(cfg.yaw_kp * err,
+                         -cfg.yaw_rate_limit_dps, cfg.yaw_rate_limit_dps)
+        arrived = dist <= cfg.arrival_radius_m
+        # 对准门控：没对准先转，避免斜着"蹭"过去（与 L2 引导同思路）
+        alignment = max(0.0, math.cos(math.radians(err)))
+        vx = 0.0 if arrived else cfg.speed_mps * alignment
+        return HomingOutput(
+            vx_mps=vx,
+            vz_mps=self.alt_hold_vz(rel_alt_m),
+            yaw_rate_dps=yaw_rate,
+            distance_m=dist,
+            bearing_err_deg=err,
+            arrived=arrived,
+        )
+
+    def alt_hold_vz(self, rel_alt_m: float) -> float:
+        """保持转场高度（NED 下正）：低于目标 → 负值（爬升），高于 → 正值（下降）。"""
+        cfg = self._cfg
+        return clamp(cfg.alt_kp * (rel_alt_m - self._transit_alt_m),
+                     -cfg.alt_limit_mps, cfg.alt_limit_mps)
+
+    def yaw_align(self, yaw_rad: float) -> tuple[float, float]:
+        """恢复初始航向：返回 (航向误差 deg, 偏航角速度 dps)。"""
+        cfg = self._cfg
+        err = wrap_deg_180(math.degrees(self._initial_yaw_rad)
+                           - math.degrees(yaw_rad))
+        rate = clamp(cfg.align_kp * err,
+                     -cfg.align_rate_limit_dps, cfg.align_rate_limit_dps)
+        return err, rate
+
+    def yaw_aligned(self, yaw_rad: float) -> bool:
+        err, _ = self.yaw_align(yaw_rad)
+        return abs(err) <= self._cfg.yaw_align_tolerance_deg

utils/task_runner/runner.py

@@ -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:

utils/task_runner/state_machine.py

@@ -5,23 +5,29 @@ L2（地理引导 GuidanceLaw）按任务阶段串成一条全流程。本模块
 + 守卫」，不驱动 MAVSDK/offboard —— 那些由 runner.py（编排器）负责。每周期吃一份
 TaskInputs 快照，返回是否转移 + 原因。
 
-状态链：
-  WAIT ──收到有效坐标──▶ ARM_TAKEOFF ──到起飞高度──▶ GOTO（L2 坐标引导）
-                                                      │ 距目标 < switch_radius
-                                                      │ 且锁定(+可选视觉)
+状态链（2026-10-15 按实机作业预期重构）：
+  WAIT ──收到有效坐标──▶ ARM_TAKEOFF ──到起飞高度──▶ GOTO（L2 飞到参考点）
+                                                      │ 距参考点 < arrival_radius
                                                       ▼
-                   APPROACH ◀──居中稳定── TRACK（L1 航向伺服）
-                       │
-  任意状态 ──超时 / 返航 / 紧急──▶ RTL
+                           SEARCH（原地偏航扫视找目标）──锁定(+可选视觉)──▶ TRACK
+                                 │ 搜索超时                        （L1 视觉追踪 +
+                                 │                             slow_approach 缓慢靠近）
+                                 ▼                                    │ 追踪超时
+                                RTL  ◀────────────────────────────────┘
 
 设计要点（铁律对齐）：
   * 纯逻辑：输入/输出都是 dataclass，时间用外部传入的 now_ms（单调毫秒），
     不碰时钟/IO/网络，可确定性单测。
   * 全局最高优先级：emergency（丢控/RC 接管/失控）与 rtl_requested（人工返航）
     → 任何非 RTL 状态立即转 RTL（安全第一）。
-  * GOTO→TRACK 守卫复用 L1 方案 §2.3：目标锁定 bit2 +（可选）我方视觉第二判据，
-    避免 bit2 粘滞导致误入追踪。
-  * TRACK 丢目标：若注入坐标仍有效 → 退回 GOTO 重新引导；否则 → RTL。
+  * 「参考点」语义：地面给的坐标是**搜索起点**，不是终点——单点坐标精度不足以
+    直接覆盖目标，故进入到达半径即转 SEARCH 扫视，由吊舱/视觉去发现目标。
+  * GOTO/SEARCH→TRACK 守卫复用 L1 方案 §2.3：目标锁定 bit2 +（可选）我方视觉
+    第二判据，避免 bit2 粘滞导致误入追踪。
+  * TRACK 丢目标 → 回 SEARCH 重新扫视（**不回 GOTO**：飞机本就在参考点附近，
+    回 GOTO 只会反复重跑进入动作，而参考点坐标本身不精确）；坐标同时失效 → RTL。
+  * TRACK 不因「目标居中」退出：居中正是吊舱对准目标、应当继续缓慢靠近的时刻。
+    接近的收尾只由超时 / 人工返航 / 紧急终止（无距离输入，判不出"已到目标"）。
   * 阈值/超时全外置（TaskStateConfig），代码不写死数值。
 """
 
@@ -35,9 +41,10 @@ class TaskState(Enum):
 
     WAIT = "wait"                # 待命：等地面坐标
     ARM_TAKEOFF = "arm_takeoff"  # 解锁 + 起飞到悬停高度
-    GOTO = "goto"                # L2 坐标引导（飞向注入坐标）
-    TRACK = "track"              # L1 航向伺服（吊舱角度追踪）
-    APPROACH = "approach"        # 接近/悬停目标上空
+    GOTO = "goto"                # L2 地理引导：飞向参考点（地面给的搜索起点）
+    SEARCH = "search"            # 到点后原地偏航扫视搜索目标（吊舱/视觉）
+    TRACK = "track"              # L1 视觉追踪 + 缓慢靠近（slow_approach 保守速度）
+    APPROACH = "approach"        # 到点悬停（真机可选收尾；SITL/常规流程不用）
     RTL = "rtl"                  # 返航 + 落地 + 上锁（终态，reset() 才回 WAIT）
 
 
@@ -45,15 +52,19 @@ class TaskState(Enum):
 class TaskStateConfig:
     """状态机配置（全外置；数值均为占位，须台架/实飞标定后回填）。"""
 
-    switch_radius_m: float = 30.0        # GOTO→TRACK 切换半径（米）
-    require_vision: bool = False         # GOTO→TRACK 是否需视觉第二判据（S3 起开）
+    arrival_radius_m: float = 30.0       # GOTO→SEARCH 到达参考点半径（米）
     arm_takeoff_timeout_s: float = 60.0  # 起飞阶段超时
-    goto_timeout_s: float = 120.0        # 坐标引导超时
-    track_timeout_s: float = 120.0       # 追踪超时
-    center_hold_s: float = 3.0           # TRACK→APPROACH 居中稳定时长（秒）
-    approach_hover_s: float = 10.0       # APPROACH 悬停时长（到点 → RTL）
+    goto_timeout_s: float = 120.0        # 飞向参考点超时
+    search_timeout_s: float = 20.0       # 搜索超时（未发现目标 → 返航）
+    track_timeout_s: float = 20.0        # 追踪/靠近段上限（到点 → 返航）
+    approach_hover_s: float = 10.0       # APPROACH 悬停时长（到点 → RTL；已非主流程）
     takeoff_altitude_m: float = 5.0      # 起飞目标高度（runner 起飞 + takeoff_reached 判据）
+    takeoff_reached_tolerance_m: float = 0.5  # 起飞到达判据容差（低于目标不超过此值也算到达；
+                                              # 悬停/高度估计误差下避免差厘米误判"起飞超时"）
     center_threshold_deg: float = 1.0    # 脱靶量居中阈值（|off_target| < 此值判居中）
+    sweep_rate_dps: float = 15.0         # SEARCH 原地扫视角速度（度/秒）
+    sweep_reverse_s: float = 4.0         # SEARCH 扫视换向周期（秒）；0 = 单向扫视
+    rtl_timeout_s: float = 180.0         # RTL 等待落地上限（秒）；超时即结束任务，防无限等待
     loop_frequency_hz: float = 20.0      # runner 主循环频率（bridge.step 节拍）
 
 
@@ -66,12 +77,11 @@ class TaskInputs:
     """
 
     target_injected: bool = False       # 地面坐标有效注入（新鲜、未超龄）
-    distance_to_target_m: float = math.inf  # 到注入目标的水平距离（GOTO 用）
+    distance_to_target_m: float = math.inf  # 到参考点的水平距离（GOTO 用）
     takeoff_reached: bool = False       # 已到达起飞目标高度
-    target_locked: bool = False         # 吊舱锁定（bit2 + 遥测新鲜）
-    vision_has_target: bool = False     # 我方视觉第二判据（可选，require_vision 时必看）
-    target_centered: bool = False       # 目标居中（脱靶量长期≈0）
-    centered_duration_s: float = 0.0    # 居中持续时长（外部累加，秒）
+    target_locked: bool = False         # 目标有效（吊舱 bit2 + 遥测新鲜 + LOS 一致性，见 bridge）
+    target_centered: bool = False       # 目标居中（脱靶量≈0）——仅供日志/诊断
+    centered_duration_s: float = 0.0    # 居中持续时长（外部累加，秒）——不再作转移判据
     rtl_requested: bool = False         # 人工返航命令
     emergency: bool = False             # 紧急（丢控 / RC 接管 / 失控）
 
@@ -103,6 +113,15 @@ class TaskStateMachine:
         self._state = TaskState.WAIT
         self._entered_ms = 0
 
+    def force_state(self, state: TaskState, now_ms: int) -> None:
+        """强制落到某状态（仅供编排器异常兜底；不产生 TaskTransition）。
+
+        正常转移一律走 update()。此入口用于「主循环抛异常、必须立刻进 RTL」这类
+        非判据驱动的安全路径——此时再喂一拍快照不可靠。
+        """
+        self._state = state
+        self._entered_ms = now_ms
+
     def update(self, inputs: TaskInputs, now_ms: int) -> TaskTransition:
         """吃一份快照，返回转移结果；无转移时 occurred=False、to_state 保持原状态。"""
         state = self._state
@@ -127,23 +146,31 @@ class TaskStateMachine:
         elif state is TaskState.GOTO:
             if not inputs.target_injected:
                 return self._goto(TaskState.RTL, "注入坐标失效", now_ms)
-            if (inputs.distance_to_target_m <= self._cfg.switch_radius_m
-                    and self._lock_ok(inputs)):
-                return self._goto(TaskState.TRACK, "进入追踪半径且目标锁定", now_ms)
+            # 到参考点即结束地理引导 → 转扫视搜索（参考点是搜索起点，不是终点）
+            if inputs.distance_to_target_m <= self._cfg.arrival_radius_m:
+                return self._goto(TaskState.SEARCH, "已到参考点，开始搜索", now_ms)
             if self._elapsed_s(now_ms) >= self._cfg.goto_timeout_s:
-                return self._goto(TaskState.RTL, "坐标引导超时", now_ms)
+                return self._goto(TaskState.RTL, "飞向参考点超时", now_ms)
+
+        elif state is TaskState.SEARCH:
+            # 扫视中一旦发现并锁定目标 → 进追踪（缓慢靠近）
+            if self._lock_ok(inputs):
+                return self._goto(TaskState.TRACK, "搜索发现目标并锁定", now_ms)
+            if not inputs.target_injected:
+                return self._goto(TaskState.RTL, "注入坐标失效", now_ms)
+            if self._elapsed_s(now_ms) >= self._cfg.search_timeout_s:
+                return self._goto(TaskState.RTL, "搜索超时未发现目标", now_ms)
 
         elif state is TaskState.TRACK:
             if not inputs.target_locked:
-                # 丢目标：坐标仍有效 → 退回 GOTO 重新引导；否则终止返航。
+                # 丢目标：坐标仍有效 → 回参考点附近重新扫视；否则终止返航。
                 if inputs.target_injected:
-                    return self._goto(TaskState.GOTO, "目标丢失，退回坐标引导", now_ms)
+                    return self._goto(TaskState.SEARCH, "目标丢失，重新搜索", now_ms)
                 return self._goto(TaskState.RTL, "目标丢失且坐标失效", now_ms)
-            if (inputs.target_centered
-                    and inputs.centered_duration_s >= self._cfg.center_hold_s):
-                return self._goto(TaskState.APPROACH, "目标居中稳定", now_ms)
+            # 注意：不再有「居中稳定 → APPROACH」分支。居中表示吊舱已对准目标，
+            # 正是应当继续按 slow_approach 缓慢靠近的时刻，不是终止条件。
             if self._elapsed_s(now_ms) >= self._cfg.track_timeout_s:
-                return self._goto(TaskState.RTL, "追踪超时", now_ms)
+                return self._goto(TaskState.RTL, "追踪/靠近超时", now_ms)
 
         elif state is TaskState.APPROACH:
             if self._elapsed_s(now_ms) >= self._cfg.approach_hover_s:
@@ -154,15 +181,13 @@ class TaskStateMachine:
 
     # --- 内部辅助 -----------------------------------------------------------
     def _lock_ok(self, inputs: TaskInputs) -> bool:
-        """GOTO→TRACK 的目标锁定判据（复用 L1 方案 §2.3）。
+        """进 TRACK 的目标判据（GOTO/SEARCH→TRACK）。
 
-        需吊舱锁定 bit2；若 require_vision 还需我方视觉第二判据（bit2 粘滞兜底）。
+        `target_locked` 已由编排层合成：吊舱 bit2 + 遥测新鲜 + **LOS 运动学一致性**
+        （见 bridge.pod_locked / los_consistency）。状态机不再重复做视觉判据 ——
+        吊舱已取代双目+YOLO 成为唯一目标源，"我方视觉"不复存在。
         """
-        if not inputs.target_locked:
-            return False
-        if self._cfg.require_vision and not inputs.vision_has_target:
-            return False
-        return True
+        return inputs.target_locked
 
     def _elapsed_s(self, now_ms: int) -> float:
         """进入当前状态的时长（秒）。_entered_ms=0（未打戳）时返回 0。"""