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