实现预定流程全链路: 任务状态机 + 编排器 + 地面站 + 真机/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,178 @@
+"""state_machine.py —— 任务级状态机（纯逻辑，无 IO，可单测）。
+
+「预定流程.md」天空侧的任务编排：把已通的 L1（航向伺服 TrackingControlLaw）与
+L2（地理引导 GuidanceLaw）按任务阶段串成一条全流程。本模块只做「状态 + 转移判据
+ 守卫」，不驱动 MAVSDK/offboard —— 那些由 runner.py（编排器）负责。每周期吃一份
+TaskInputs 快照，返回是否转移 + 原因。
+
+状态链：
+  WAIT ──收到有效坐标──▶ ARM_TAKEOFF ──到起飞高度──▶ GOTO（L2 坐标引导）
+                                                      │ 距目标 < switch_radius
+                                                      │ 且锁定(+可选视觉)
+                                                      ▼
+                   APPROACH ◀──居中稳定── TRACK（L1 航向伺服）
+                       │
+  任意状态 ──超时 / 返航 / 紧急──▶ RTL
+
+设计要点（铁律对齐）：
+  * 纯逻辑：输入/输出都是 dataclass，时间用外部传入的 now_ms（单调毫秒），
+    不碰时钟/IO/网络，可确定性单测。
+  * 全局最高优先级：emergency（丢控/RC 接管/失控）与 rtl_requested（人工返航）
+    → 任何非 RTL 状态立即转 RTL（安全第一）。
+  * GOTO→TRACK 守卫复用 L1 方案 §2.3：目标锁定 bit2 +（可选）我方视觉第二判据，
+    避免 bit2 粘滞导致误入追踪。
+  * TRACK 丢目标：若注入坐标仍有效 → 退回 GOTO 重新引导；否则 → RTL。
+  * 阈值/超时全外置（TaskStateConfig），代码不写死数值。
+"""
+
+import math
+from dataclasses import dataclass
+from enum import Enum
+
+
+class TaskState(Enum):
+    """任务状态（.value 为小写字符串，供日志/序列化）。"""
+
+    WAIT = "wait"                # 待命：等地面坐标
+    ARM_TAKEOFF = "arm_takeoff"  # 解锁 + 起飞到悬停高度
+    GOTO = "goto"                # L2 坐标引导（飞向注入坐标）
+    TRACK = "track"              # L1 航向伺服（吊舱角度追踪）
+    APPROACH = "approach"        # 接近/悬停目标上空
+    RTL = "rtl"                  # 返航 + 落地 + 上锁（终态，reset() 才回 WAIT）
+
+
+@dataclass
+class TaskStateConfig:
+    """状态机配置（全外置；数值均为占位，须台架/实飞标定后回填）。"""
+
+    switch_radius_m: float = 30.0        # GOTO→TRACK 切换半径（米）
+    require_vision: bool = False         # GOTO→TRACK 是否需视觉第二判据（S3 起开）
+    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）
+    takeoff_altitude_m: float = 5.0      # 起飞目标高度（runner 起飞 + takeoff_reached 判据）
+    center_threshold_deg: float = 1.0    # 脱靶量居中阈值（|off_target| < 此值判居中）
+    loop_frequency_hz: float = 20.0      # runner 主循环频率（bridge.step 节拍）
+
+
+@dataclass
+class TaskInputs:
+    """单周期任务快照（调用方在同一控制周期采集并打戳）。
+
+    各布尔量为「已合成好的判据」，由 runner 从 PX4 遥测 / 吊舱遥测 / 地面注入 /
+    视觉源聚合后传入；状态机不做聚合，只做转移决策。
+    """
+
+    target_injected: bool = False       # 地面坐标有效注入（新鲜、未超龄）
+    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    # 居中持续时长（外部累加，秒）
+    rtl_requested: bool = False         # 人工返航命令
+    emergency: bool = False             # 紧急（丢控 / RC 接管 / 失控）
+
+
+@dataclass
+class TaskTransition:
+    """状态机单周期转移结果。occurred=False 表示维持原状态。"""
+
+    from_state: TaskState = TaskState.WAIT
+    to_state: TaskState = TaskState.WAIT
+    occurred: bool = False
+    reason: str = ""
+
+
+class TaskStateMachine:
+    """任务级状态机。update() 单周期返回 TaskTransition，维护内部 current 状态。"""
+
+    def __init__(self, cfg: TaskStateConfig) -> None:
+        self._cfg = cfg
+        self._state = TaskState.WAIT
+        self._entered_ms = 0  # 进入当前状态的单调毫秒（超时/时长判定用）
+
+    @property
+    def state(self) -> TaskState:
+        return self._state
+
+    def reset(self) -> None:
+        """回到 WAIT 并清零计时（新任务 / 任务终止后复用）。"""
+        self._state = TaskState.WAIT
+        self._entered_ms = 0
+
+    def update(self, inputs: TaskInputs, now_ms: int) -> TaskTransition:
+        """吃一份快照，返回转移结果；无转移时 occurred=False、to_state 保持原状态。"""
+        state = self._state
+
+        # 全局最高优先级：紧急 / 人工返航 → 任何非 RTL 状态立即终止。
+        if state is not TaskState.RTL:
+            if inputs.emergency:
+                return self._goto(TaskState.RTL, "紧急（丢控/RC 接管）", now_ms)
+            if inputs.rtl_requested:
+                return self._goto(TaskState.RTL, "人工返航命令", now_ms)
+
+        if state is TaskState.WAIT:
+            if inputs.target_injected:
+                return self._goto(TaskState.ARM_TAKEOFF, "收到地面坐标", now_ms)
+
+        elif state is TaskState.ARM_TAKEOFF:
+            if inputs.takeoff_reached:
+                return self._goto(TaskState.GOTO, "已到起飞高度", now_ms)
+            if self._elapsed_s(now_ms) >= self._cfg.arm_takeoff_timeout_s:
+                return self._goto(TaskState.RTL, "起飞超时", now_ms)
+
+        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 self._elapsed_s(now_ms) >= self._cfg.goto_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.RTL, "目标丢失且坐标失效", now_ms)
+            if (inputs.target_centered
+                    and inputs.centered_duration_s >= self._cfg.center_hold_s):
+                return self._goto(TaskState.APPROACH, "目标居中稳定", now_ms)
+            if self._elapsed_s(now_ms) >= self._cfg.track_timeout_s:
+                return self._goto(TaskState.RTL, "追踪超时", now_ms)
+
+        elif state is TaskState.APPROACH:
+            if self._elapsed_s(now_ms) >= self._cfg.approach_hover_s:
+                return self._goto(TaskState.RTL, "悬停完成", now_ms)
+
+        # RTL 为终态；无任何转移条件命中 → 维持原状态。
+        return TaskTransition(from_state=state, to_state=state, occurred=False)
+
+    # --- 内部辅助 -----------------------------------------------------------
+    def _lock_ok(self, inputs: TaskInputs) -> bool:
+        """GOTO→TRACK 的目标锁定判据（复用 L1 方案 §2.3）。
+
+        需吊舱锁定 bit2；若 require_vision 还需我方视觉第二判据（bit2 粘滞兜底）。
+        """
+        if not inputs.target_locked:
+            return False
+        if self._cfg.require_vision and not inputs.vision_has_target:
+            return False
+        return True
+
+    def _elapsed_s(self, now_ms: int) -> float:
+        """进入当前状态的时长（秒）。_entered_ms=0（未打戳）时返回 0。"""
+        if self._entered_ms <= 0:
+            return 0.0
+        return max(0.0, (now_ms - self._entered_ms) / 1000.0)
+
+    def _goto(self, target: TaskState, reason: str, now_ms: int) -> TaskTransition:
+        prev = self._state
+        self._state = target
+        self._entered_ms = now_ms
+        return TaskTransition(from_state=prev, to_state=target,
+                              occurred=True, reason=reason)