实现预定流程全链路: 任务状态机 + 编排器 + 地面站 + 真机/SITL入口

- utils/task_runner: 6态状态机(WAIT→ARM_TAKEOFF→GOTO(L2)→TRACK(L1)→APPROACH→RTL)
  + 编排器(快照采集/状态动作) + 双层日志(事件JSONL/遥测CSV) + 吊舱控制指令(0x07/0x0F/0x0E/0x08)
- pod_bridge: ground_link 地面坐标UDP注入(超龄判无效,无激光替代表4);
  set_heading_servo_enabled() 运行时L1/L2切换(切换即reset清积分防输出跳变);
  pod_locked 提为公共门控; config 新增 ground_inject 段
- virtual_pod 模拟吊舱(无电机语义) + verify_px4 增量④仅飞控验证 +
  pod_sitl_test L1 SITL全流程(飞行编排不塞进 run_bridge, 保持纯offboard闭环入口)
- 入口: task_runner_sitl(VirtualPod+虚拟目标, SITL端到端跑通约1m24s) /
  task_runner_real(真串口+UDP注入+吊舱控制)
- 地面站 ground_station: 坐标注入 + 状态可视化(Flask)
- S1标定回填(2026-10-09): beta_sign=+1 / alpha_sign=-1, 加标定回归测试守护
- 新增测试: task_runner x4 + ground_link/virtual_pod*/bridge/px4_gateway 增量; 全量 483 passed
- 文档: 预定流程(需求) + 预定流程-操作指南(操作/待办) + L1方案补S1结论与实现状态

utils/tests/pod_bridge/bridge_test.py

@@ -13,6 +13,7 @@ import pytest
 
 from pod_bridge.bridge import PodBridge
 from pod_bridge.config import BridgeConfig, load_from_json
+from pod_bridge.ground_link import GroundTarget
 from pod_bridge.protocol import (ControlCode, ControlCommand, TargetGeo,
                                  Telemetry, decode_attitude, decode_control)
 from pod_bridge.px4_gateway import OwnState
@@ -523,3 +524,75 @@ def test_l1_require_vision_passes_with_source():
     assert out.mode.name == "VELOCITY_HEADING"
     assert out.yaw_rate_dps == pytest.approx(10.0, abs=1e-9)
 
+
+# --- 地面坐标注入（A2）-------------------------------------------------------
+def _inject_config(timeout_ms=5000):
+    cfg = BridgeConfig()
+    cfg.ground_inject.enabled = True
+    cfg.ground_inject.timeout_ms = timeout_ms
+    return cfg
+
+
+def test_ground_inject_priority_over_table4():
+    """注入坐标优先于表4：吊舱无表4/无锁定（本设备常态），注入新鲜坐标照常引导。"""
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    injected = GroundTarget(latitude_deg=0.001, longitude_deg=0.0, time_ms=1000)
+    b = PodBridge(_inject_config(), px4, pod, now_ms_fn=lambda: 0,
+                  ground_target_fn=lambda: injected)
+    out = asyncio.run(b.step(1000))
+    assert out.mode.name == "VELOCITY_HEADING"
+    assert len(px4.velocity_calls) == 1
+
+
+def test_ground_inject_stale_falls_back_to_brake():
+    """注入坐标超龄 → 视为无目标 → 刹车（不得朝过期坐标飞）。"""
+    px4 = FakePx4(_valid_state(2000))
+    pod = FakePod()
+    injected = GroundTarget(latitude_deg=0.001, longitude_deg=0.0,
+                            time_ms=1000)  # 1000ms 前注入
+    cfg = _inject_config(timeout_ms=500)
+    b = PodBridge(cfg, px4, pod, now_ms_fn=lambda: 0,
+                  ground_target_fn=lambda: injected)
+    out = asyncio.run(b.step(2000))  # 差值 1000ms > 500ms → 超龄
+    assert out.mode.name == "BRAKE_HOVER"
+    assert px4.velocity_calls == []
+
+
+def test_ground_inject_disabled_ignores_injection():
+    """未启用地面注入 → 忽略注入坐标，回退表4 原路径（无表4 → 刹车）。"""
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    injected = GroundTarget(latitude_deg=0.001, longitude_deg=0.0, time_ms=1000)
+    cfg = BridgeConfig()  # ground_inject.enabled=False
+    b = PodBridge(cfg, px4, pod, now_ms_fn=lambda: 0,
+                  ground_target_fn=lambda: injected)
+    out = asyncio.run(b.step(1000))
+    assert out.mode.name == "BRAKE_HOVER"
+
+
+# --- 运行时切换控制律（A3：GOTO(L2) ↔ TRACK(L1)）-----------------------------
+def test_set_heading_servo_enabled_runtime_switch():
+    """运行时切换控制律：初始 L1 → 切 L2（刹车）→ 切回 L1（速度）。"""
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    b = _bridge(px4, pod, _l1_config())  # enabled=True → 初始 L1
+
+    # 初始 L1：锁定 + 脱靶量 → 速度
+    pod.queue(_locked_off_target(x=10.0))
+    out = asyncio.run(b.step(1000))
+    assert out.mode.name == "VELOCITY_HEADING"
+    assert out.yaw_rate_dps == pytest.approx(10.0, abs=1e-9)
+
+    # 切 L2：无表4/无锁定 → 刹车
+    b.set_heading_servo_enabled(False)
+    out = asyncio.run(b.step(1100))
+    assert out.mode.name == "BRAKE_HOVER"
+
+    # 切回 L1：重新喂锁定脱靶量 → 速度
+    b.set_heading_servo_enabled(True)
+    pod.queue(_locked_off_target(x=10.0))
+    out = asyncio.run(b.step(1200))
+    assert out.mode.name == "VELOCITY_HEADING"
+    assert out.yaw_rate_dps == pytest.approx(10.0, abs=1e-9)
+

utils/tests/pod_bridge/config_test.py

@@ -133,6 +133,17 @@ def test_heading_servo_loads():
     assert hs.vertical_enabled is True              # 决策3：无人机 L1 开垂直
 
 
+def test_deployed_config_has_calibrated_signs():
+    """S1 台架标定回归(2026-10-09)：目标在左下时 off_target_x/y 均为负 →
+    吊舱 X 轴右正(与控制律 β 同向) beta_sign=+1；Y 轴上正(与 α 下正相反) alpha_sign=-1。
+    厂商确认本版电机无用 → 框架角路径必须关。改动这些值前必须重新台架标定。"""
+    cfg = load_from_json(str(_DEFAULT_CONFIG))
+    assert cfg.heading_servo.beta_sign == 1
+    assert cfg.heading_servo.alpha_sign == -1
+    assert cfg.heading_servo.beta_from_frame is False
+    assert cfg.heading_servo.alpha_from_frame is False
+
+
 def test_heading_servo_partial_override(tmp_path):
     cfg = load_from_json(_write(tmp_path, {
         "heading_servo": {"beta_sign": -1, "beta_from_frame": True},

utils/tests/pod_bridge/ground_link_test.py

@@ -0,0 +1,94 @@
+"""ground_link_test.py —— 地面坐标注入通道单元测试。
+
+覆盖：parse_payload 解析（合法/缺字段/非法 JSON/非对象/类型错误）、
+is_valid 新鲜度判定（未注入/新鲜/超龄边界）。
+"""
+
+import pytest
+
+from pod_bridge.ground_link import GroundInjectConfig, GroundLink, GroundTarget
+
+
+# --- parse_payload ----------------------------------------------------------
+def test_parse_valid_payload():
+    t = GroundLink.parse_payload(
+        b'{"lat": 39.9042, "lon": 116.4074, "alt": 50.0}')
+    assert t is not None
+    assert t.latitude_deg == pytest.approx(39.9042)
+    assert t.longitude_deg == pytest.approx(116.4074)
+    assert t.abs_alt_m == pytest.approx(50.0)
+    assert t.time_ms == 0  # 打戳由接收方（run）负责
+
+
+def test_parse_default_alt_is_zero():
+    t = GroundLink.parse_payload(b'{"lat": 1.0, "lon": 2.0}')
+    assert t is not None
+    assert t.abs_alt_m == 0.0
+
+
+def test_parse_missing_lat_returns_none():
+    assert GroundLink.parse_payload(b'{"lon": 2.0}') is None
+
+
+def test_parse_missing_lon_returns_none():
+    assert GroundLink.parse_payload(b'{"lat": 1.0}') is None
+
+
+def test_parse_invalid_json_returns_none():
+    assert GroundLink.parse_payload(b'not json') is None
+
+
+def test_parse_non_object_returns_none():
+    assert GroundLink.parse_payload(b'[1, 2, 3]') is None
+
+
+def test_parse_string_coords_returns_none():
+    assert GroundLink.parse_payload(b'{"lat": "x", "lon": 2.0}') is None
+
+
+def test_parse_bool_coords_returns_none():
+    # True 是 bool 不是数值 → 拒绝
+    assert GroundLink.parse_payload(b'{"lat": true, "lon": 2.0}') is None
+
+
+def test_parse_ignores_ts_field():
+    """ts 字段仅诊断，不参与解析（新鲜度用本地接收时刻）。"""
+    t = GroundLink.parse_payload(b'{"lat": 1.0, "lon": 2.0, "ts": 999}')
+    assert t is not None
+    assert t.latitude_deg == pytest.approx(1.0)
+
+
+# --- is_valid ---------------------------------------------------------------
+def _link(timeout_ms=5000):
+    return GroundLink(GroundInjectConfig(timeout_ms=timeout_ms),
+                      now_ms_fn=lambda: 0)
+
+
+def _inject(link, time_ms):
+    link._target = GroundTarget(latitude_deg=1.0, longitude_deg=2.0,
+                                time_ms=time_ms)
+
+
+def test_is_valid_false_when_never_injected():
+    assert _link().is_valid(1000) is False
+
+
+def test_is_valid_true_when_fresh():
+    link = _link(timeout_ms=5000)
+    _inject(link, 1000)
+    assert link.is_valid(1000) is True
+    assert link.is_valid(4000) is True
+
+
+def test_is_valid_timeout_boundary():
+    link = _link(timeout_ms=5000)
+    _inject(link, 1000)
+    assert link.is_valid(6000) is True   # 恰好 5000ms，未超龄
+    assert link.is_valid(6001) is False  # 5001ms，超龄
+
+
+def test_latest_returns_injected_target():
+    link = _link()
+    _inject(link, 1000)
+    assert link.latest().latitude_deg == pytest.approx(1.0)
+    assert link.latest().time_ms == 1000

utils/tests/pod_bridge/px4_gateway_test.py

@@ -5,6 +5,7 @@
 """
 
 import asyncio
+import math
 from enum import Enum
 from types import SimpleNamespace
 
@@ -62,7 +63,7 @@ class FakeTelemetry:
     def gps_info(self):
         return self._stream(self._gps)
 
-    def velocity_body(self):
+    def velocity_ned(self):
         return self._stream(self._velocity)
 
 
@@ -77,8 +78,9 @@ def _pos(lat=31.0, lon=121.0, aalt=100.0, ralt=50.0):
                            absolute_altitude_m=aalt, relative_altitude_m=ralt)
 
 
-def _att(roll=0.1, pitch=0.2, yaw=1.57):
-    return SimpleNamespace(roll_rad=roll, pitch_rad=pitch, yaw_rad=yaw)
+def _att(roll_deg=5.0, pitch_deg=10.0, yaw_deg=90.0):
+    """MAVSDK EulerAngle 字段是度（roll_deg/pitch_deg/yaw_deg），网关转弧度。"""
+    return SimpleNamespace(roll_deg=roll_deg, pitch_deg=pitch_deg, yaw_deg=yaw_deg)
 
 
 # --- 枚举转换 ---------------------------------------------------------------
@@ -120,12 +122,14 @@ def test_update_attitude_sets_radians():
     gw.update_attitude(_att())
     s = gw.own_state()
     assert s.has_attitude is True
-    assert s.yaw_rad == pytest.approx(1.57)
+    assert s.roll_rad == pytest.approx(math.radians(5.0))
+    assert s.pitch_rad == pytest.approx(math.radians(10.0))
+    assert s.yaw_rad == pytest.approx(math.radians(90.0))
 
 
-def test_update_velocity_body_ground_speed_is_horizontal_magnitude():
+def test_update_velocity_ned_ground_speed_is_horizontal_magnitude():
     gw = Px4Gateway(FakeDrone())
-    gw.update_velocity_body(SimpleNamespace(forward_m_s=3.0, right_m_s=4.0,
+    gw.update_velocity_ned(SimpleNamespace(north_m_s=3.0, east_m_s=4.0,
                                             down_m_s=-9.9))
     assert gw.own_state().ground_speed_mps == pytest.approx(5.0)  # 不含 down
 
@@ -173,9 +177,9 @@ def test_command_delegation_reaches_offboard():
 def test_start_telemetry_populates_cache_then_stop():
     telemetry = FakeTelemetry(
         position=[_pos(lat=22.0, lon=114.0)],
-        attitude=[_att(yaw=0.5)],
+        attitude=[_att(yaw_deg=30.0)],
         gps=[SimpleNamespace(fix_type=3, num_satellites=10)],
-        velocity=[SimpleNamespace(forward_m_s=2.0, right_m_s=0.0, down_m_s=0.0)],
+        velocity=[SimpleNamespace(north_m_s=2.0, east_m_s=0.0, down_m_s=0.0)],
     )
     gw = Px4Gateway(FakeDrone(telemetry))
 
@@ -184,7 +188,7 @@ def test_start_telemetry_populates_cache_then_stop():
         await asyncio.sleep(0.05)  # 让后台任务消费预设消息
         s = gw.own_state()
         assert s.latitude_deg == 22.0 and s.longitude_deg == 114.0
-        assert s.yaw_rad == pytest.approx(0.5)
+        assert s.yaw_rad == pytest.approx(math.radians(30.0))
         assert s.position_valid is True
         assert s.ground_speed_mps == pytest.approx(2.0)
         await gw.stop_telemetry()
@@ -210,7 +214,7 @@ def test_position_timestamp_not_refreshed_by_other_streams():
     clock[0] = 2000
     gw.update_attitude(_att())
     gw.update_gps(SimpleNamespace(fix_type=3, num_satellites=9))
-    gw.update_velocity_body(SimpleNamespace(forward_m_s=1.0, right_m_s=0.0,
+    gw.update_velocity_ned(SimpleNamespace(north_m_s=1.0, east_m_s=0.0,
                                             down_m_s=0.0))
     s = gw.own_state()
     assert s.time_ms == 2000            # 诊断时间戳照常刷新

utils/tests/pod_bridge/virtual_pod_bridge_test.py

@@ -0,0 +1,104 @@
+"""virtual_pod_bridge_test.py —— VirtualPod × PodBridge × TrackingControlLaw 集成测试。
+
+用部署配置(pod_bridge.json)把"SITL 闭环段"在无飞控下跑通，端到端复核 S1 标定：
+  目标偏右 → 控制律应输出正 yaw_rate（右转）；
+  目标偏下 → 应输出正 vz（下降）；
+  目标消失 → 应判丢零速刹车。
+"""
+
+import asyncio
+from dataclasses import replace
+from pathlib import Path
+
+import pytest
+
+from pod_bridge.bridge import PodBridge
+from pod_bridge.config import load_from_json
+from pod_bridge.px4_gateway import OwnState
+from pod_bridge.virtual_pod import (CarrierView, VirtualPod, VirtualTarget,
+                                    VirtualTargetConfig)
+
+_DEFAULT_CONFIG = Path(__file__).resolve().parents[2] / "config" / "pod_bridge.json"
+
+
+class FakePx4:
+    def __init__(self, yaw_rad=0.0):
+        self._yaw = yaw_rad
+        self.clock = 1000          # 测试循环每拍推进，与 step(now_ms) 同步
+        self.velocity_calls = []
+        self.brake_calls = []
+
+    def own_state(self):
+        return OwnState(has_attitude=True, has_position=True, position_valid=True,
+                        fix_type=3, yaw_rad=self._yaw, time_ms=self.clock,
+                        position_time_ms=self.clock)
+
+    async def start_telemetry(self): pass
+    async def stop_telemetry(self): pass
+    async def start_offboard(self): return True
+    async def stop_offboard(self): pass
+    async def send_velocity_heading(self, out, time_boot_ms=None):
+        self.velocity_calls.append(out); return True
+    async def send_brake_hover(self, time_boot_ms=None):
+        self.brake_calls.append(time_boot_ms); return True
+
+
+def _bridge(target, own):
+    cfg = load_from_json(str(_DEFAULT_CONFIG))  # 部署配置：L1 enabled、S1 符号
+    pod = VirtualPod(target, lambda: own)
+    px4 = FakePx4(yaw_rad=own.yaw_rad)
+    return PodBridge(cfg, px4, pod, now_ms_fn=lambda: 0), px4
+
+
+def _static_target(ahead=10.0, right=0.0, down=0.0):
+    cfg = VirtualTargetConfig(ahead_m=ahead, right_m=right, down_m=down,
+                              arc_speed_mps=0.0)
+    return VirtualTarget(cfg, (0.0, 0.0, 0.0), 0.0)
+
+
+def _run_ticks(bridge, px4, n, t0=1000, dt=50):
+    """跑 n 拍（每拍 50ms，节拍与 20Hz 一致），返回末拍输出。"""
+    out = None
+    for i in range(n):
+        t = t0 + i * dt
+        px4.clock = t
+        out = asyncio.run(bridge.step(t))
+    return out
+
+
+def test_deployed_config_turns_right_for_right_target():
+    # 目标在右前方：S1 标定 beta_sign=+1 → yaw_rate 应为正（右转）
+    tgt = _static_target(ahead=10.0, right=5.0)
+    bridge, px4 = _bridge(tgt, CarrierView())
+    out = _run_ticks(bridge, px4, 5)
+    assert out.mode.name == "VELOCITY_HEADING"
+    assert out.yaw_rate_dps > 0.0
+    assert px4.velocity_calls  # 正常速度航向指令
+    assert not px4.brake_calls
+
+
+def test_deployed_config_descends_for_below_target():
+    # 目标偏下：S1 标定 alpha_sign=-1（吊舱上正→控制律下正）→ vz 应为正（下降）
+    tgt = _static_target(ahead=10.0, down=2.0)
+    bridge, px4 = _bridge(tgt, CarrierView())
+    out = _run_ticks(bridge, px4, 10)
+    assert out.mode.name == "VELOCITY_HEADING"
+    assert out.vz_mps > 0.0
+
+
+def test_deployed_config_slow_approach_forward():
+    # L1 无距离：slow_approach 0.2 m/s 起步（决策3 低限速）
+    tgt = _static_target()
+    bridge, px4 = _bridge(tgt, CarrierView())
+    out = _run_ticks(bridge, px4, 10)
+    assert 0.0 < out.vx_body_mps <= 0.2 + 1e-9
+
+
+def test_target_lost_brakes_through_pipeline():
+    tgt = _static_target()
+    bridge, px4 = _bridge(tgt, CarrierView())
+    _run_ticks(bridge, px4, 3)
+    tgt.set_visible(False)  # 目标消失 → 吊舱转搜索态 → 判丢刹车
+    out = _run_ticks(bridge, px4, 2, t0=1000 + 3 * 50)
+    assert out.mode.name == "BRAKE_HOVER"
+    assert px4.brake_calls

utils/tests/pod_bridge/virtual_pod_test.py

@@ -0,0 +1,124 @@
+"""virtual_pod_test.py —— 模拟吊舱几何与 S1 符号约定回归测试。
+
+钉死 S1 台架标定结论（2026-10-09）：目标在左下时 off_target_x/y 均为负，
+即 X 右正、Y 上正；并验证不可见/超视场 → 搜索态 + 脱靶量归 0（对齐真实
+吊舱"丢失时脱靶量恒 0"的行为）。
+"""
+
+import math
+import time
+
+import pytest
+
+from pod_bridge.virtual_pod import (CarrierView, VirtualPod, VirtualTarget,
+                                    VirtualTargetConfig)
+
+
+def _pod(target, own):
+    return VirtualPod(target, lambda: own)
+
+
+def _static_target(ahead=30.0, right=0.0, down=0.0):
+    """放在锚点正前方（机头朝北）的静止目标。"""
+    cfg = VirtualTargetConfig(ahead_m=ahead, right_m=right, down_m=down,
+                              arc_speed_mps=0.0)
+    return VirtualTarget(cfg, (0.0, 0.0, 0.0), 0.0)
+
+
+def _carrier(yaw_deg=0.0):
+    return CarrierView(north_m=0.0, east_m=0.0, down_m=0.0,
+                       yaw_rad=math.radians(yaw_deg))
+
+
+def _telemetry(pod):
+    frames = pod.poll()
+    assert len(frames) == 1
+    return frames[0]
+
+
+# ---- S1 符号约定回归：目标位置 → 脱靶量符号 --------------------------------
+def test_target_right_gives_positive_off_x():
+    # 目标在右前方：rel_e>0、机头朝北 → 脱靶量 X 应为正（右正）
+    tgt = _static_target(ahead=10.0, right=5.0)
+    tel = _telemetry(_pod(tgt, _carrier()))
+    assert tel.off_target_x_deg > 0.0
+    assert tel.off_target_x_deg == pytest.approx(math.degrees(math.atan2(5.0, 10.0)),
+                                                 abs=1e-9)
+
+
+def test_target_left_gives_negative_off_x():
+    tgt = _static_target(ahead=10.0, right=-5.0)
+    tel = _telemetry(_pod(tgt, _carrier()))
+    assert tel.off_target_x_deg < 0.0
+
+
+def test_target_below_gives_negative_off_y():
+    # S1 实测：目标偏下 → off_target_y 为负（上正）。11.3° 在垂直视场(35°)内
+    tgt = _static_target(ahead=10.0, down=2.0)
+    tel = _telemetry(_pod(tgt, _carrier()))
+    assert tel.off_target_y_deg < 0.0
+    assert tel.off_target_y_deg == pytest.approx(-math.degrees(math.atan2(2.0, 10.0)),
+                                                 abs=1e-9)
+
+
+def test_target_above_gives_positive_off_y():
+    tgt = _static_target(ahead=10.0, down=-2.0)
+    tel = _telemetry(_pod(tgt, _carrier()))
+    assert tel.off_target_y_deg > 0.0
+
+
+def test_yaw_rotation_follows_body_frame():
+    # 机头偏东 20°：目标在正北 → 目标在机体左侧 20° → off_x 为负
+    tgt = _static_target(ahead=10.0)
+    tel = _telemetry(_pod(tgt, _carrier(yaw_deg=20.0)))
+    assert tel.off_target_x_deg == pytest.approx(-20.0, abs=1e-9)
+
+
+# ---- 无电机 / 无激光语义 ---------------------------------------------------
+def test_frame_angles_always_zero_and_laser_none():
+    tgt = _static_target()
+    tel = _telemetry(_pod(tgt, _carrier()))
+    assert tel.frame_yaw_deg == 0.0
+    assert tel.frame_pitch_deg == 0.0
+    assert tel.frame_roll_deg == 0.0
+    assert tel.laser_range_m is None
+
+
+# ---- 锁定门控 --------------------------------------------------------------
+def test_visible_target_locks():
+    tel = _telemetry(_pod(_static_target(), _carrier()))
+    assert tel.is_target_locked
+
+
+def test_hidden_target_searches_with_zero_off():
+    tgt = _static_target()
+    tgt.set_visible(False)
+    tel = _telemetry(_pod(tgt, _carrier()))
+    assert not tel.is_target_locked
+    assert tel.off_target_x_deg == 0.0 and tel.off_target_y_deg == 0.0
+
+
+def test_target_behind_searches():
+    # 机头朝北、目标在正南 10m → fwd<0 → 搜索态
+    cfg = VirtualTargetConfig(ahead_m=-10.0, right_m=0.0, down_m=0.0,
+                              arc_speed_mps=0.0)
+    tgt = VirtualTarget(cfg, (0.0, 0.0, 0.0), 0.0)
+    tel = _telemetry(_pod(tgt, _carrier()))
+    assert not tel.is_target_locked
+
+
+def test_target_out_of_fov_searches():
+    # 目标几乎在正右方（β→90°）超出默认 FOV 60° → 搜索态
+    tgt = _static_target(ahead=1.0, right=100.0)
+    tel = _telemetry(_pod(tgt, _carrier()))
+    assert not tel.is_target_locked
+
+
+# ---- PodLink 接口 ----------------------------------------------------------
+def test_send_counts_and_close_marks():
+    pod = _pod(_static_target(), _carrier())
+    pod.send(b"\x00" * 4)
+    pod.send(b"\x00" * 4)
+    assert pod.attitude_frames_rx == 2
+    pod.close()
+    assert pod.closed

utils/tests/task_runner/log_test.py

@@ -0,0 +1,123 @@
+"""log_test.py —— 任务日志模块单元测试。
+
+覆盖：open 建目录/写表头、事件 JSONL、遥测 CSV、telemetry_every_n 节流、
+disabled 时零副作用、close 收尾。
+"""
+
+import csv
+import json
+
+import pytest
+
+from task_runner.log import TaskLogConfig, TaskLogger, TelemetryRecord
+
+
+def _cfg(tmp_path, **kw):
+    c = TaskLogConfig(dir=str(tmp_path), **kw)
+    return c
+
+
+def test_open_creates_dir_and_csv_header(tmp_path):
+    logger = TaskLogger(_cfg(tmp_path))
+    logger.open()
+    logger.close()
+
+    assert (tmp_path / "task_events.jsonl").exists()
+    assert (tmp_path / "task_telemetry.csv").exists()
+
+    with open(tmp_path / "task_telemetry.csv", encoding="utf-8") as f:
+        header = next(csv.reader(f))
+    assert header[0] == "t_s"
+    assert "state" in header
+    assert "distance_to_target_m" in header
+
+
+def test_log_event_writes_jsonl(tmp_path):
+    logger = TaskLogger(_cfg(tmp_path))
+    logger.open()
+    logger.log_event(1000, "state", from_state="wait",
+                     to_state="arm_takeoff", reason="收到地面坐标")
+    logger.log_event(2000, "emergency", reason="RC 接管")
+    logger.close()
+
+    with open(tmp_path / "task_events.jsonl", encoding="utf-8") as f:
+        lines = [json.loads(x) for x in f if x.strip()]
+    assert len(lines) == 2
+    assert lines[0]["type"] == "state"
+    assert lines[0]["from_state"] == "wait"
+    assert lines[0]["to_state"] == "arm_takeoff"
+    assert lines[0]["ts_ms"] == 1000
+    assert lines[0]["t_s"] == 0.0          # 首个事件 t_s = 0
+    assert lines[1]["t_s"] == 1.0          # 第二个事件 t_s = 1.0s
+
+
+def test_log_telemetry_writes_csv_rows(tmp_path):
+    logger = TaskLogger(_cfg(tmp_path))
+    logger.open()
+    rec = TelemetryRecord(state="goto", pos_valid=True, latitude_deg=47.39,
+                          distance_to_target_m=33.5, mode="VELOCITY_HEADING",
+                          yaw_rate_dps=-30.0, vx_body_mps=1.7)
+    logger.log_telemetry(1000, rec)
+    logger.log_telemetry(1100, rec)
+    logger.close()
+
+    with open(tmp_path / "task_telemetry.csv", encoding="utf-8") as f:
+        rows = list(csv.DictReader(f))
+    assert len(rows) == 2
+    assert rows[0]["state"] == "goto"
+    assert rows[0]["pos_valid"] == "True"
+    assert rows[0]["distance_to_target_m"] == "33.5"
+    assert rows[0]["t_s"] == "0.0"
+    assert rows[1]["t_s"] == "0.1"
+
+
+def test_telemetry_throttled_by_every_n(tmp_path):
+    logger = TaskLogger(_cfg(tmp_path, telemetry_every_n=5))
+    logger.open()
+    rec = TelemetryRecord(state="goto")
+    for i in range(12):
+        logger.log_telemetry(1000 + i * 100, rec)
+    logger.close()
+
+    with open(tmp_path / "task_telemetry.csv", encoding="utf-8") as f:
+        rows = list(csv.DictReader(f))
+    assert len(rows) == 2  # 第 5、10 次各记一次
+
+
+def test_disabled_logs_nothing(tmp_path):
+    logger = TaskLogger(_cfg(tmp_path, enabled=False))
+    logger.open()
+    logger.log_event(1000, "state", from_state="wait", to_state="arm_takeoff")
+    logger.log_telemetry(1000, TelemetryRecord(state="goto"))
+    logger.close()
+
+    # 未 open（disabled）→ 不建目录、不写文件
+    assert not (tmp_path / "task_events.jsonl").exists()
+    assert not (tmp_path / "task_telemetry.csv").exists()
+
+
+def test_log_before_open_is_noop(tmp_path):
+    logger = TaskLogger(_cfg(tmp_path))
+    logger.log_event(1000, "state", from_state="wait", to_state="arm_takeoff")
+    logger.log_telemetry(1000, TelemetryRecord(state="goto"))
+    logger.close()
+    assert not (tmp_path / "task_events.jsonl").exists()
+
+
+def test_append_keeps_existing_rows(tmp_path):
+    """重复 open（追加模式）不清空既有内容，表头不重复写。"""
+    logger = TaskLogger(_cfg(tmp_path))
+    logger.open()
+    logger.log_telemetry(1000, TelemetryRecord(state="wait"))
+    logger.close()
+
+    logger2 = TaskLogger(_cfg(tmp_path))
+    logger2.open()
+    logger2.log_telemetry(2000, TelemetryRecord(state="goto"))
+    logger2.close()
+
+    with open(tmp_path / "task_telemetry.csv", encoding="utf-8") as f:
+        rows = list(csv.DictReader(f))
+    assert len(rows) == 2
+    assert rows[0]["state"] == "wait"
+    assert rows[1]["state"] == "goto"

utils/tests/task_runner/pod_control_test.py

@@ -0,0 +1,45 @@
+"""pod_control_test.py —— 吊舱目标识别/锁定控制器单元测试。
+
+覆盖：arm_detection 发 0x07(识别,目标类型)+0x0F(自动锁定)、
+stop_tracking 发 0x0E(停止)+0x08(识别关)。
+"""
+
+from pod_bridge.protocol import ControlCode
+from task_runner.pod_control import PodController
+
+
+class FakeBridge:
+    def __init__(self):
+        self.sent = []
+
+    def send_control(self, cmd):
+        self.sent.append(cmd)
+
+
+def test_arm_detection_sends_detect_and_autolock():
+    bridge = FakeBridge()
+    pc = PodController(bridge, target_type=1)
+    pc.arm_detection()
+
+    assert len(bridge.sent) == 2
+    assert bridge.sent[0].code == ControlCode.TARGET_DETECT_ON
+    assert bridge.sent[0].param_x == 1          # 目标类型 1 = 无人机
+    assert bridge.sent[1].code == ControlCode.AUTO_LOCK_ON_FOUND
+
+
+def test_arm_detection_default_target_type_is_person_vehicle():
+    bridge = FakeBridge()
+    pc = PodController(bridge)  # 默认 target_type=0 = 人/车
+    pc.arm_detection()
+    assert bridge.sent[0].code == ControlCode.TARGET_DETECT_ON
+    assert bridge.sent[0].param_x == 0
+
+
+def test_stop_tracking_sends_stop_and_detect_off():
+    bridge = FakeBridge()
+    pc = PodController(bridge)
+    pc.stop_tracking()
+
+    assert len(bridge.sent) == 2
+    assert bridge.sent[0].code == ControlCode.STOP_TRACK
+    assert bridge.sent[1].code == ControlCode.TARGET_DETECT_OFF

utils/tests/task_runner/runner_test.py

@@ -0,0 +1,311 @@
+"""runner_test.py —— 任务编排器单元测试（mock px4/bridge/ground/drone，无硬件）。
+
+覆盖：输入采集（注入坐标新鲜/超龄、起飞到位、锁定、居中累加）、
+状态进入动作（ARM_TAKEOFF 触发起飞、GOTO 切 L2、TRACK 切 L1）。
+"""
+
+import asyncio
+from dataclasses import replace
+
+import pytest
+
+from pod_bridge.config import BridgeConfig
+from pod_bridge.ground_link import GroundTarget
+from pod_bridge.px4_gateway import OwnState
+from pod_bridge.protocol import Telemetry
+from task_runner.runner import TaskRunner
+from task_runner.state_machine import (TaskState, TaskStateConfig,
+                                       TaskStateMachine)
+
+
+# --- mocks ------------------------------------------------------------------
+class FakePx4:
+    def __init__(self, state=None):
+        self._state = state or OwnState()
+        self.telemetry_started = 0
+        self.offboard_started = False
+        self.brake_calls = []
+
+    def own_state(self):
+        return replace(self._state)
+
+    async def start_telemetry(self):
+        self.telemetry_started += 1
+
+    async def stop_telemetry(self):
+        pass
+
+    async def start_offboard(self):
+        self.offboard_started = True
+        return True
+
+    async def stop_offboard(self):
+        pass
+
+    async def send_velocity_heading(self, out, t=None):
+        return True
+
+    async def send_brake_hover(self, t=None):
+        self.brake_calls.append(t)
+        return True
+
+
+class FakeBridge:
+    def __init__(self):
+        self.started = False
+        self.stopped = False
+        self.law = None              # set_heading_servo_enabled 最后值
+        self.steps = 0
+        self._locked = False
+        self._telemetry = None
+
+    async def start(self):
+        self.started = True
+        return True
+
+    async def stop(self):
+        self.stopped = True
+
+    def set_heading_servo_enabled(self, enabled):
+        self.law = enabled
+
+    def pod_locked(self, now_ms):
+        return self._locked
+
+    async def step(self, now_ms):
+        self.steps += 1
+
+    @property
+    def latest_telemetry(self):
+        return self._telemetry
+
+
+class FakeGround:
+    def __init__(self, target=None):
+        self._target = target
+
+    def latest(self):
+        return self._target or GroundTarget()
+
+
+class FakeDrone:
+    def __init__(self):
+        self.armed = False
+        self.took_off = False
+        self.takeoff_alt = 0.0
+        self.rtl = False
+        self.landed = False
+        self.disarmed = False
+
+    class _Action:
+        def __init__(self, d):
+            self._d = d
+
+        async def arm(self):
+            self._d.armed = True
+
+        async def set_takeoff_altitude(self, a):
+            self._d.takeoff_alt = a
+
+        async def takeoff(self):
+            self._d.took_off = True
+
+        async def return_to_launch(self):
+            self._d.rtl = True
+
+        async def land(self):
+            self._d.landed = True
+
+        async def disarm(self):
+            self._d.disarmed = True
+
+    @property
+    def action(self):
+        return FakeDrone._Action(self)
+
+
+def _state(now_ms, lat=0.0, lon=0.0, rel_alt=0.0):
+    return OwnState(latitude_deg=lat, longitude_deg=lon,
+                    relative_altitude_m=rel_alt, fix_type=3, num_satellites=12,
+                    has_position=True, has_attitude=True, position_valid=True,
+                    time_ms=now_ms, position_time_ms=now_ms)
+
+
+def _make_runner(px4=None, bridge=None, ground=None, drone=None,
+                 sm_cfg=None, vision_fn=None, pod_control=None):
+    cfg = BridgeConfig()
+    cfg.ground_inject.enabled = True
+    cfg.ground_inject.timeout_ms = 5000
+    return TaskRunner(
+        cfg, sm_cfg or TaskStateConfig(),
+        px4 or FakePx4(_state(1000)),
+        bridge or FakeBridge(),
+        TaskStateMachine(sm_cfg or TaskStateConfig()),
+        ground or FakeGround(),
+        drone or FakeDrone(),
+        vision_fn=vision_fn,
+        now_ms_fn=lambda: 1000,
+        pod_control=pod_control,
+    )
+
+
+# --- 输入采集 ---------------------------------------------------------------
+def test_collect_injected_fresh_sets_distance():
+    ground = FakeGround(GroundTarget(latitude_deg=0.001, longitude_deg=0.0,
+                                     time_ms=1000))
+    px4 = FakePx4(_state(1000))
+    r = _make_runner(px4=px4, ground=ground)
+
+    inp = r._collect_inputs(1000)
+    assert inp.target_injected is True
+    assert inp.distance_to_target_m == pytest.approx(111.19, rel=1e-2)  # ~111m
+    assert inp.takeoff_reached is False   # rel_alt=0 < 5
+
+
+def test_collect_injected_stale_is_invalid():
+    ground = FakeGround(GroundTarget(latitude_deg=0.001, longitude_deg=0.0,
+                                     time_ms=1000))
+    r = _make_runner(ground=ground)  # timeout=5000
+
+    inp = r._collect_inputs(7000)  # 6000ms 差值 > 5000 → 超龄
+    assert inp.target_injected is False
+    assert inp.distance_to_target_m == float("inf")
+
+
+def test_collect_takeoff_reached():
+    px4 = FakePx4(_state(1000, rel_alt=5.0))
+    sm_cfg = TaskStateConfig(takeoff_altitude_m=5.0)
+    r = _make_runner(px4=px4, sm_cfg=sm_cfg)
+
+    assert r._collect_inputs(1000).takeoff_reached is True
+
+
+def test_collect_locked_from_bridge():
+    bridge = FakeBridge()
+    bridge._locked = True
+    r = _make_runner(bridge=bridge)
+
+    assert r._collect_inputs(1000).target_locked is True
+
+
+def test_collect_vision_second_judgement():
+    r = _make_runner(vision_fn=lambda: True)
+    assert r._collect_inputs(1000).vision_has_target is True
+
+    r2 = _make_runner(vision_fn=lambda: False)
+    assert r2._collect_inputs(1000).vision_has_target is False
+
+
+# --- 居中累加 ---------------------------------------------------------------
+def test_centered_accumulates_duration():
+    bridge = FakeBridge()
+    bridge._telemetry = (Telemetry(off_target_x_deg=0.1, off_target_y_deg=0.1), 1000)
+    sm_cfg = TaskStateConfig(center_threshold_deg=1.0)
+    r = _make_runner(bridge=bridge, sm_cfg=sm_cfg)
+
+    assert r._update_centered(1000) == (True, 0.0)      # 开始居中
+    assert r._update_centered(3000) == (True, 2.0)      # 持续 2s
+    assert r._update_centered(3500) == (True, 2.5)      # 持续 2.5s
+
+
+def test_centered_resets_on_large_offset():
+    bridge = FakeBridge()
+    bridge._telemetry = (Telemetry(off_target_x_deg=0.1, off_target_y_deg=0.1), 1000)
+    r = _make_runner(bridge=bridge)
+
+    r._update_centered(1000)
+    bridge._telemetry = (Telemetry(off_target_x_deg=5.0, off_target_y_deg=0.1), 2000)
+    assert r._update_centered(2000) == (False, 0.0)     # 脱靶量大 → 清零
+
+
+def test_centered_no_telemetry_returns_false():
+    r = _make_runner()  # bridge.latest_telemetry is None
+    assert r._update_centered(1000) == (False, 0.0)
+
+
+# --- 状态进入动作 -----------------------------------------------------------
+def test_on_enter_arm_takeoff_arms_and_takes_off():
+    drone = FakeDrone()
+    r = _make_runner(drone=drone, sm_cfg=TaskStateConfig(takeoff_altitude_m=5.0))
+    asyncio.run(r._on_enter(TaskState.ARM_TAKEOFF, 1000))
+    assert drone.armed is True
+    assert drone.took_off is True
+    assert drone.takeoff_alt == pytest.approx(5.0)
+
+
+def test_on_enter_goto_switches_to_l2():
+    bridge = FakeBridge()
+    r = _make_runner(bridge=bridge)
+    asyncio.run(r._on_enter(TaskState.GOTO, 1000))
+    assert bridge.started is True
+    assert bridge.law is False  # L2 地理引导
+
+
+def test_on_enter_track_switches_to_l1():
+    bridge = FakeBridge()
+    r = _make_runner(bridge=bridge)
+    asyncio.run(r._on_enter(TaskState.TRACK, 1000))
+    assert bridge.law is True  # L1 航向伺服
+
+
+def test_on_enter_rtl_stops_bridge_and_returns():
+    bridge = FakeBridge()
+    drone = FakeDrone()
+    r = _make_runner(bridge=bridge, drone=drone)
+    asyncio.run(r._on_enter(TaskState.RTL, 1000))
+    assert bridge.stopped is True
+    assert drone.rtl is True
+
+
+# --- 持续动作 ---------------------------------------------------------------
+def test_tick_goto_steps_bridge():
+    bridge = FakeBridge()
+    r = _make_runner(bridge=bridge)
+    asyncio.run(r._tick(1000))
+    # WAIT 状态不 step；切到 GOTO 才 step
+    r._sm._state = TaskState.GOTO
+    asyncio.run(r._tick(1000))
+    assert bridge.steps == 1
+
+
+def test_tick_approach_brakes():
+    px4 = FakePx4(_state(1000))
+    r = _make_runner(px4=px4)
+    r._sm._state = TaskState.APPROACH
+    asyncio.run(r._tick(1000))
+    assert len(px4.brake_calls) == 1
+
+
+# --- 吊舱控制集成（真机）----------------------------------------------------
+class FakePodControl:
+    def __init__(self):
+        self.armed = False
+        self.stopped = False
+
+    def arm_detection(self):
+        self.armed = True
+
+    def stop_tracking(self):
+        self.stopped = True
+
+
+def test_on_enter_goto_arms_detection():
+    pod_control = FakePodControl()
+    r = _make_runner(pod_control=pod_control)
+    asyncio.run(r._on_enter(TaskState.GOTO, 1000))
+    assert pod_control.armed is True
+    assert pod_control.stopped is False
+
+
+def test_on_enter_rtl_stops_tracking_before_teardown():
+    pod_control = FakePodControl()
+    r = _make_runner(pod_control=pod_control)
+    asyncio.run(r._on_enter(TaskState.RTL, 1000))
+    assert pod_control.stopped is True
+
+
+def test_no_pod_control_is_safe():
+    """未注入 pod_control（SITL）→ GOTO/RTL 进入动作不触发吊舱控制、不抛异常。"""
+    r = _make_runner(pod_control=None)
+    asyncio.run(r._on_enter(TaskState.GOTO, 1000))
+    asyncio.run(r._on_enter(TaskState.RTL, 1000))

utils/tests/task_runner/state_machine_test.py

@@ -0,0 +1,233 @@
+"""state_machine_test.py —— 任务级状态机单元测试。
+
+覆盖：完整状态链 WAIT→ARM_TAKEOFF→GOTO→TRACK→APPROACH→RTL、各状态超时、
+GOTO→TRACK 的锁定/视觉守卫、TRACK 丢目标退回 GOTO、紧急/返航全局终止、
+RTL 终态与 reset。
+"""
+
+import pytest
+
+from task_runner.state_machine import (TaskInputs, TaskState, TaskStateConfig,
+                                       TaskStateMachine, TaskTransition)
+
+
+def _inp(**kw) -> TaskInputs:
+    return TaskInputs(**kw)
+
+
+def _sm(cfg=None) -> TaskStateMachine:
+    return TaskStateMachine(cfg or TaskStateConfig())
+
+
+# --- 初始态与 reset ---------------------------------------------------------
+def test_initial_state_is_wait():
+    assert _sm().state is TaskState.WAIT
+
+
+def test_reset_returns_to_wait():
+    sm = _sm()
+    sm.update(_inp(target_injected=True), 1000)
+    assert sm.state is TaskState.ARM_TAKEOFF
+    sm.reset()
+    assert sm.state is TaskState.WAIT
+
+
+# --- WAIT -------------------------------------------------------------------
+def test_wait_without_coords_stays():
+    tr = _sm().update(_inp(), 1000)
+    assert not tr.occurred
+    assert tr.to_state is TaskState.WAIT
+
+
+def test_wait_receives_coords_goes_arm_takeoff():
+    sm = _sm()
+    tr = sm.update(_inp(target_injected=True), 1000)
+    assert tr.occurred
+    assert tr.from_state is TaskState.WAIT
+    assert tr.to_state is TaskState.ARM_TAKEOFF
+    assert sm.state is TaskState.ARM_TAKEOFF
+
+
+# --- ARM_TAKEOFF ------------------------------------------------------------
+def test_arm_takeoff_reaches_altitude_goes_goto():
+    sm = _sm()
+    sm.update(_inp(target_injected=True), 1000)
+    tr = sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
+    assert tr.occurred and tr.to_state is TaskState.GOTO
+
+
+def test_arm_takeoff_timeout_goes_rtl():
+    cfg = TaskStateConfig(arm_takeoff_timeout_s=60.0)
+    sm = _sm(cfg)
+    sm.update(_inp(target_injected=True), 1000)
+    # 60s 未到高度 → 超时返航
+    tr = sm.update(_inp(target_injected=True), 1000 + 61_000)
+    assert tr.occurred and tr.to_state is TaskState.RTL
+
+
+# --- GOTO -------------------------------------------------------------------
+def test_goto_far_away_stays():
+    sm = _sm()
+    sm.update(_inp(target_injected=True), 1000)
+    sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
+    tr = sm.update(_inp(target_injected=True, distance_to_target_m=100.0,
+                        target_locked=True), 3000)
+    assert not tr.occurred
+    assert sm.state is TaskState.GOTO
+
+
+def test_goto_within_radius_and_locked_goes_track():
+    sm = _sm()
+    sm.update(_inp(target_injected=True), 1000)
+    sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
+    tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
+                        target_locked=True), 3000)
+    assert tr.occurred and tr.to_state is TaskState.TRACK
+
+
+def test_goto_within_radius_but_unlocked_stays():
+    sm = _sm()
+    sm.update(_inp(target_injected=True), 1000)
+    sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
+    tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
+                        target_locked=False), 3000)
+    assert not tr.occurred
+    assert sm.state is TaskState.GOTO
+
+
+def test_goto_requires_vision_when_enabled():
+    cfg = TaskStateConfig(require_vision=True)
+    sm = _sm(cfg)
+    sm.update(_inp(target_injected=True), 1000)
+    sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
+    # 有锁定但无视觉第二判据 → 不切换
+    tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
+                        target_locked=True, vision_has_target=False), 3000)
+    assert not tr.occurred
+    # 补上视觉 → 切换
+    tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
+                        target_locked=True, vision_has_target=True), 4000)
+    assert tr.occurred and tr.to_state is TaskState.TRACK
+
+
+def test_goto_loses_coords_goes_rtl():
+    sm = _sm()
+    sm.update(_inp(target_injected=True), 1000)
+    sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
+    tr = sm.update(_inp(target_injected=False, distance_to_target_m=10.0), 3000)
+    assert tr.occurred and tr.to_state is TaskState.RTL
+
+
+def test_goto_timeout_goes_rtl():
+    cfg = TaskStateConfig(goto_timeout_s=120.0)
+    sm = _sm(cfg)
+    sm.update(_inp(target_injected=True), 1000)
+    sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
+    tr = sm.update(_inp(target_injected=True, distance_to_target_m=100.0), 2000 + 121_000)
+    assert tr.occurred and tr.to_state is TaskState.RTL
+
+
+# --- TRACK ------------------------------------------------------------------
+def test_track_centered_stable_goes_approach():
+    sm = _sm(TaskStateConfig(center_hold_s=3.0))
+    sm.update(_inp(target_injected=True), 1000)
+    sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
+    sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
+                   target_locked=True), 3000)
+    assert sm.state is TaskState.TRACK
+    # 居中但未满时长 → 停留
+    tr = sm.update(_inp(target_injected=True, target_locked=True,
+                        target_centered=True, centered_duration_s=1.0), 4000)
+    assert not tr.occurred
+    # 居中稳定满 3s → APPROACH
+    tr = sm.update(_inp(target_injected=True, target_locked=True,
+                        target_centered=True, centered_duration_s=3.0), 5000)
+    assert tr.occurred and tr.to_state is TaskState.APPROACH
+
+
+def test_track_lost_with_coords_goes_back_to_goto():
+    sm = _sm()
+    sm.update(_inp(target_injected=True), 1000)
+    sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
+    sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
+                   target_locked=True), 3000)
+    tr = sm.update(_inp(target_injected=True, target_locked=False), 4000)
+    assert tr.occurred and tr.to_state is TaskState.GOTO
+
+
+def test_track_lost_without_coords_goes_rtl():
+    sm = _sm()
+    sm.update(_inp(target_injected=True), 1000)
+    sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
+    sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
+                   target_locked=True), 3000)
+    tr = sm.update(_inp(target_injected=False, target_locked=False), 4000)
+    assert tr.occurred and tr.to_state is TaskState.RTL
+
+
+def test_track_timeout_goes_rtl():
+    cfg = TaskStateConfig(track_timeout_s=120.0)
+    sm = _sm(cfg)
+    sm.update(_inp(target_injected=True), 1000)
+    sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
+    sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
+                   target_locked=True), 3000)
+    tr = sm.update(_inp(target_injected=True, target_locked=True), 3000 + 121_000)
+    assert tr.occurred and tr.to_state is TaskState.RTL
+
+
+# --- APPROACH ---------------------------------------------------------------
+def test_approach_hover_done_goes_rtl():
+    cfg = TaskStateConfig(center_hold_s=0.0, approach_hover_s=10.0)
+    sm = _sm(cfg)
+    sm.update(_inp(target_injected=True), 1000)
+    sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
+    sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
+                   target_locked=True), 3000)
+    sm.update(_inp(target_injected=True, target_locked=True, target_centered=True,
+                   centered_duration_s=0.0), 4000)
+    assert sm.state is TaskState.APPROACH
+    # 悬停未满 10s → 停留
+    tr = sm.update(_inp(target_injected=True, target_locked=True), 4000 + 5000)
+    assert not tr.occurred
+    # 满 10s → RTL
+    tr = sm.update(_inp(target_injected=True, target_locked=True), 4000 + 10_000)
+    assert tr.occurred and tr.to_state is TaskState.RTL
+
+
+# --- 全局终止（紧急 / 返航）-------------------------------------------------
+@pytest.mark.parametrize("state_kw", [
+    {},                                                    # WAIT
+    {"target_injected": True},                             # 会先转 ARM_TAKEOFF
+])
+def test_emergency_goes_rtl_from_any_state(state_kw):
+    sm = _sm()
+    sm.update(_inp(**state_kw), 1000)
+    tr = sm.update(_inp(emergency=True), 2000)
+    assert tr.occurred and tr.to_state is TaskState.RTL
+
+
+def test_rtl_requested_goes_rtl():
+    sm = _sm()
+    tr = sm.update(_inp(rtl_requested=True), 1000)
+    assert tr.occurred and tr.to_state is TaskState.RTL
+
+
+# --- RTL 终态 ---------------------------------------------------------------
+def test_rtl_is_terminal():
+    sm = _sm()
+    sm.update(_inp(rtl_requested=True), 1000)
+    assert sm.state is TaskState.RTL
+    # 终态不再转移（连紧急/坐标也不动）
+    tr = sm.update(_inp(target_injected=True, emergency=True), 2000)
+    assert not tr.occurred
+    assert sm.state is TaskState.RTL
+
+
+# --- 转移结果结构 -----------------------------------------------------------
+def test_transition_dataclass_defaults():
+    tr = TaskTransition()
+    assert tr.from_state is TaskState.WAIT
+    assert tr.to_state is TaskState.WAIT
+    assert tr.occurred is False
+    assert tr.reason == ""