"""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)