keita
df91b1110e
归档 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。
204 lines
11 KiB
Python
204 lines
11 KiB
Python
"""state_machine.py —— 任务级状态机（纯逻辑，无 IO，可单测）。
|
||
|
||
「预定流程.md」天空侧的任务编排：把已通的 L1（航向伺服 TrackingControlLaw）与
|
||
L2（地理引导 GuidanceLaw）按任务阶段串成一条全流程。本模块只做「状态 + 转移判据
|
||
+ 守卫」，不驱动 MAVSDK/offboard —— 那些由 runner.py（编排器）负责。每周期吃一份
|
||
TaskInputs 快照，返回是否转移 + 原因。
|
||
|
||
状态链（2026-10-15 按实机作业预期重构）：
|
||
WAIT ──收到有效坐标──▶ ARM_TAKEOFF ──到起飞高度──▶ GOTO（L2 飞到参考点）
|
||
│ 距参考点 < arrival_radius
|
||
▼
|
||
SEARCH（原地偏航扫视找目标）──锁定(+可选视觉)──▶ TRACK
|
||
│ 搜索超时 （L1 视觉追踪 +
|
||
│ slow_approach 缓慢靠近）
|
||
▼ │ 追踪超时
|
||
RTL ◀────────────────────────────────┘
|
||
|
||
设计要点（铁律对齐）：
|
||
* 纯逻辑：输入/输出都是 dataclass，时间用外部传入的 now_ms（单调毫秒），
|
||
不碰时钟/IO/网络，可确定性单测。
|
||
* 全局最高优先级：emergency（丢控/RC 接管/失控）与 rtl_requested（人工返航）
|
||
→ 任何非 RTL 状态立即转 RTL（安全第一）。
|
||
* 「参考点」语义：地面给的坐标是**搜索起点**，不是终点——单点坐标精度不足以
|
||
直接覆盖目标，故进入到达半径即转 SEARCH 扫视，由吊舱/视觉去发现目标。
|
||
* GOTO/SEARCH→TRACK 守卫复用 L1 方案 §2.3：目标锁定 bit2 +（可选）我方视觉
|
||
第二判据，避免 bit2 粘滞导致误入追踪。
|
||
* TRACK 丢目标 → 回 SEARCH 重新扫视（**不回 GOTO**：飞机本就在参考点附近，
|
||
回 GOTO 只会反复重跑进入动作，而参考点坐标本身不精确）；坐标同时失效 → RTL。
|
||
* TRACK 不因「目标居中」退出：居中正是吊舱对准目标、应当继续缓慢靠近的时刻。
|
||
接近的收尾只由超时 / 人工返航 / 紧急终止（无距离输入，判不出"已到目标"）。
|
||
* 阈值/超时全外置（TaskStateConfig），代码不写死数值。
|
||
"""
|
||
|
||
import math
|
||
from dataclasses import dataclass
|
||
from enum import Enum
|
||
|
||
|
||
class TaskState(Enum):
|
||
"""任务状态（.value 为小写字符串，供日志/序列化）。"""
|
||
|
||
WAIT = "wait" # 待命：等地面坐标
|
||
ARM_TAKEOFF = "arm_takeoff" # 解锁 + 起飞到悬停高度
|
||
GOTO = "goto" # L2 地理引导：飞向参考点（地面给的搜索起点）
|
||
SEARCH = "search" # 到点后原地偏航扫视搜索目标（吊舱/视觉）
|
||
TRACK = "track" # L1 视觉追踪 + 缓慢靠近（slow_approach 保守速度）
|
||
APPROACH = "approach" # 到点悬停（真机可选收尾；SITL/常规流程不用）
|
||
RTL = "rtl" # 返航 + 落地 + 上锁（终态，reset() 才回 WAIT）
|
||
|
||
|
||
@dataclass
|
||
class TaskStateConfig:
|
||
"""状态机配置（全外置；数值均为占位，须台架/实飞标定后回填）。"""
|
||
|
||
arrival_radius_m: float = 30.0 # GOTO→SEARCH 到达参考点半径（米）
|
||
arm_takeoff_timeout_s: float = 60.0 # 起飞阶段超时
|
||
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 节拍）
|
||
|
||
|
||
@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 + 遥测新鲜 + LOS 一致性，见 bridge）
|
||
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 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
|
||
|
||
# 全局最高优先级：紧急 / 人工返航 → 任何非 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.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)
|
||
|
||
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:
|
||
# 丢目标：坐标仍有效 → 回参考点附近重新扫视；否则终止返航。
|
||
if inputs.target_injected:
|
||
return self._goto(TaskState.SEARCH, "目标丢失，重新搜索", now_ms)
|
||
return self._goto(TaskState.RTL, "目标丢失且坐标失效", now_ms)
|
||
# 注意：不再有「居中稳定 → APPROACH」分支。居中表示吊舱已对准目标，
|
||
# 正是应当继续按 slow_approach 缓慢靠近的时刻，不是终止条件。
|
||
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:
|
||
"""进 TRACK 的目标判据（GOTO/SEARCH→TRACK）。
|
||
|
||
`target_locked` 已由编排层合成：吊舱 bit2 + 遥测新鲜 + **LOS 运动学一致性**
|
||
（见 bridge.pod_locked / los_consistency）。状态机不再重复做视觉判据 ——
|
||
吊舱已取代双目+YOLO 成为唯一目标源，"我方视觉"不复存在。
|
||
"""
|
||
return inputs.target_locked
|
||
|
||
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)
|