实现预定流程全链路: 任务状态机 + 编排器 + 地面站 + 真机/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结论与实现状态
2026-10-13 15:50:32 +08:00
parent 5a944af4f5
commit ded17d7995
30 changed files with 3552 additions and 27 deletions
@@ -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 == ""