实现预定流程全链路: 任务状态机 + 编排器 + 地面站 + 真机/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结论与实现状态
This commit is contained in:
keita
@@ -0,0 +1,9 @@
|
||||
"""task_runner —— 任务编排器（预定流程.md 的天空侧任务级状态机）。
|
||||
|
||||
把已通的 L1（航向伺服 TrackingControlLaw）与 L2（地理引导 GuidanceLaw）按任务阶段
|
||||
编排成一条可运行的全流程：WAIT → ARM_TAKEOFF → GOTO → TRACK → APPROACH → RTL。
|
||||
|
||||
模块划分：
|
||||
* state_machine.py 纯逻辑任务状态机（无 IO，可单测）
|
||||
* runner.py 编排器（MAVSDK 飞行编排 + 状态机驱动 + PodBridge 闭环，A3）
|
||||
"""
|
||||
@@ -0,0 +1,149 @@
|
||||
"""log.py —— 任务日志模块：结构化记录任务全流程（事件 + 周期遥测）。
|
||||
|
||||
预定流程 B 阶段：让任意一次 SITL / 实飞都能回溯出完整阶段轨迹与异常。
|
||||
|
||||
分层：
|
||||
* 事件日志（JSONL）：状态机转移、飞行编排事件（解锁/起飞/返航）、异常/紧急。
|
||||
低频、带语义、结构化，便于 grep / jq 定位「什么时候发生了什么」。
|
||||
* 周期遥测日志（CSV）：每控制周期的位姿 / GPS / 目标 / 控制输出，便于回放与画图。
|
||||
|
||||
时间戳：单调毫秒（now_ms，与运行时代码同源）+ 相对时间 t_s（自首个 now_ms 起）。
|
||||
文件 IO 集中在 open / close，热路径只追加一行，不做 flush 决策。
|
||||
"""
|
||||
|
||||
import csv
|
||||
import json
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Optional
|
||||
|
||||
_TELEMETRY_FIELDS = (
|
||||
"t_s", "state",
|
||||
"pos_valid", "fix_type", "num_satellites",
|
||||
"latitude_deg", "longitude_deg", "relative_altitude_m", "yaw_deg",
|
||||
"target_injected", "distance_to_target_m", "target_locked",
|
||||
"off_target_x_deg", "off_target_y_deg",
|
||||
"mode", "yaw_rate_dps", "vx_body_mps", "vz_mps",
|
||||
)
|
||||
|
||||
|
||||
@dataclass
|
||||
class TelemetryRecord:
|
||||
"""单周期遥测记录（字段顺序即 CSV 列序）。"""
|
||||
|
||||
t_s: float = 0.0
|
||||
state: str = ""
|
||||
pos_valid: bool = False
|
||||
fix_type: int = 0
|
||||
num_satellites: int = 0
|
||||
latitude_deg: float = 0.0
|
||||
longitude_deg: float = 0.0
|
||||
relative_altitude_m: float = 0.0
|
||||
yaw_deg: float = 0.0
|
||||
target_injected: bool = False
|
||||
distance_to_target_m: float = float("inf")
|
||||
target_locked: bool = False
|
||||
off_target_x_deg: float = 0.0
|
||||
off_target_y_deg: float = 0.0
|
||||
mode: str = ""
|
||||
yaw_rate_dps: float = 0.0
|
||||
vx_body_mps: float = 0.0
|
||||
vz_mps: float = 0.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class TaskLogConfig:
|
||||
"""任务日志配置（全外置）。"""
|
||||
|
||||
enabled: bool = True
|
||||
dir: str = "logs" # 输出目录（相对工作目录）
|
||||
event_file: str = "task_events.jsonl" # 事件日志
|
||||
telemetry_file: str = "task_telemetry.csv" # 周期遥测日志
|
||||
telemetry_every_n: int = 1 # 每 N 周期记一次遥测（1 = 每周期）
|
||||
|
||||
|
||||
def _monotonic_ms() -> int:
|
||||
return int(time.monotonic() * 1000)
|
||||
|
||||
|
||||
class TaskLogger:
|
||||
"""结构化任务日志。用法：
|
||||
|
||||
logger = TaskLogger(cfg, now_ms_fn)
|
||||
logger.open() # 建目录 + 写 CSV 表头
|
||||
logger.log_event(now_ms, "state", from_state="wait", ...)
|
||||
logger.log_telemetry(now_ms, record)
|
||||
logger.close()
|
||||
|
||||
enabled=False 或未 open 时，所有 log 方法为空操作（不抛异常、不写文件）。
|
||||
"""
|
||||
|
||||
def __init__(self, cfg: TaskLogConfig = TaskLogConfig(),
|
||||
now_ms_fn=_monotonic_ms) -> None:
|
||||
self._cfg = cfg
|
||||
self._now_ms = now_ms_fn
|
||||
self._event_fp = None
|
||||
self._tel_fp = None
|
||||
self._tel_writer = None
|
||||
self._t0_ms: Optional[int] = None
|
||||
self._tick = 0
|
||||
|
||||
# --- 生命周期 -----------------------------------------------------------
|
||||
def open(self) -> None:
|
||||
"""创建输出目录并打开两个日志文件；disabled 时跳过。"""
|
||||
if not self._cfg.enabled:
|
||||
return
|
||||
out_dir = Path(self._cfg.dir)
|
||||
out_dir.mkdir(parents=True, exist_ok=True)
|
||||
self._event_fp = (out_dir / self._cfg.event_file).open(
|
||||
"a", encoding="utf-8")
|
||||
self._tel_fp = (out_dir / self._cfg.telemetry_file).open(
|
||||
"a", encoding="utf-8", newline="")
|
||||
self._tel_writer = csv.writer(self._tel_fp)
|
||||
if (out_dir / self._cfg.telemetry_file).stat().st_size == 0:
|
||||
self._tel_writer.writerow(_TELEMETRY_FIELDS)
|
||||
self._event_fp.flush()
|
||||
self._tel_fp.flush()
|
||||
|
||||
def close(self) -> None:
|
||||
"""flush 并关闭两个日志文件。"""
|
||||
if self._event_fp is not None:
|
||||
self._event_fp.flush()
|
||||
self._event_fp.close()
|
||||
self._event_fp = None
|
||||
if self._tel_fp is not None:
|
||||
self._tel_fp.flush()
|
||||
self._tel_fp.close()
|
||||
self._tel_fp = None
|
||||
self._tel_writer = None
|
||||
|
||||
# --- 记录 ---------------------------------------------------------------
|
||||
def log_event(self, now_ms: int, event_type: str, **fields) -> None:
|
||||
"""追加一条 JSONL 事件。"""
|
||||
if self._event_fp is None:
|
||||
return
|
||||
record = {"type": event_type, "ts_ms": now_ms,
|
||||
"t_s": self._rel_s(now_ms)}
|
||||
record.update(fields)
|
||||
self._event_fp.write(json.dumps(record, ensure_ascii=False) + "\n")
|
||||
|
||||
def log_telemetry(self, now_ms: int, rec: TelemetryRecord) -> None:
|
||||
"""追加一行遥测（按 telemetry_every_n 节流）。"""
|
||||
if self._tel_writer is None:
|
||||
return
|
||||
self._tick += 1
|
||||
if self._tick % max(1, self._cfg.telemetry_every_n) != 0:
|
||||
return
|
||||
rec.t_s = self._rel_s(now_ms)
|
||||
self._tel_writer.writerow([getattr(rec, f) for f in _TELEMETRY_FIELDS])
|
||||
|
||||
# --- 内部 ---------------------------------------------------------------
|
||||
def _rel_s(self, now_ms: int) -> float:
|
||||
if self._t0_ms is None:
|
||||
self._t0_ms = now_ms
|
||||
return (now_ms - self._t0_ms) / 1000.0
|
||||
|
||||
@property
|
||||
def fieldnames(self) -> tuple:
|
||||
return _TELEMETRY_FIELDS
|
||||
@@ -0,0 +1,39 @@
|
||||
"""pod_control.py —— 吊舱目标识别/锁定控制（真机，任务编排的一部分）。
|
||||
|
||||
真机吊舱不会自动输出"锁定"——需要先发控制指令让它进入目标识别 + 自动锁定模式：
|
||||
* 0x07 目标识别开（param_x = 目标类型：0=人/车，1=无人机）
|
||||
* 0x0F 发现目标自动锁定（常驻模式：目标进画面自动锁定，非一次性动作）
|
||||
结束（任务完成/返航）时：
|
||||
* 0x0E 停止跟踪（只停当前这一次跟踪）
|
||||
* 0x08 目标识别关（真正退出识别）
|
||||
|
||||
本模块把这些指令序列封装成 PodController，供 TaskRunner 在 GOTO / RTL 阶段调用。
|
||||
SITL 用 VirtualPod（直接合成锁定态），不注入 PodController。
|
||||
"""
|
||||
|
||||
from pod_bridge.protocol import ControlCode, ControlCommand
|
||||
|
||||
|
||||
class PodController:
|
||||
"""吊舱目标识别/锁定控制器。
|
||||
|
||||
arm_detection / stop_tracking 为同步方法，经 bridge.send_control 下发 表2-1
|
||||
控制帧（编码由 PodBridge 完成，本类只关心"发什么指令序列"）。
|
||||
"""
|
||||
|
||||
def __init__(self, bridge, target_type: int = 0) -> None:
|
||||
"""bridge：PodBridge（需有 send_control）；target_type：0=人/车，1=无人机。"""
|
||||
self._bridge = bridge
|
||||
self._target_type = int(target_type)
|
||||
|
||||
def arm_detection(self) -> None:
|
||||
"""开启目标识别 + 发现自动锁定（GOTO 阶段进入时调用）。"""
|
||||
self._bridge.send_control(ControlCommand(
|
||||
code=ControlCode.TARGET_DETECT_ON, param_x=self._target_type))
|
||||
self._bridge.send_control(ControlCommand(
|
||||
code=ControlCode.AUTO_LOCK_ON_FOUND))
|
||||
|
||||
def stop_tracking(self) -> None:
|
||||
"""停止跟踪 + 关闭目标识别（RTL 前调用）。"""
|
||||
self._bridge.send_control(ControlCommand(code=ControlCode.STOP_TRACK))
|
||||
self._bridge.send_control(ControlCommand(code=ControlCode.TARGET_DETECT_OFF))
|
||||
@@ -0,0 +1,296 @@
|
||||
"""runner.py —— 任务编排器：驱动任务状态机跑通 WAIT→…→RTL 全流程。
|
||||
|
||||
「预定流程.md」天空侧的落地：把已通的 L1（航向伺服 TrackingControlLaw）与
|
||||
L2（地理引导 GuidanceLaw）按任务阶段编排，配合地面坐标注入（GroundLink）与
|
||||
状态机（TaskStateMachine），串成一条可运行的全流程。
|
||||
|
||||
职责边界：
|
||||
* 状态机（TaskStateMachine）只做「状态 + 转移判据」决策；
|
||||
* 本编排器负责「采集快照 → 喂状态机 → 按状态执行动作」；
|
||||
* 闭环控制复用 PodBridge（L1/L2 运行时切换走 set_heading_servo_enabled）；
|
||||
* 飞行编排（解锁/起飞/返航）通过 duck-typed `drone`（MAVSDK System）完成，
|
||||
便于 mock 单测；真实 SITL 装配见仓库根 task_runner_sitl.py。
|
||||
|
||||
生命周期：run() 开头先订阅 PX4 遥测（幂等），用于起飞到位 / 距离计算；
|
||||
GOTO 进入时 start offboard；RTL 进入时安全停机 + 返航。
|
||||
"""
|
||||
|
||||
import asyncio
|
||||
import logging
|
||||
import math
|
||||
import time
|
||||
from typing import Callable, Optional
|
||||
|
||||
from pod_bridge.guidance import haversine_distance_m
|
||||
from visual_track_control.staleness_checker import is_stale
|
||||
|
||||
from .log import TaskLogger, TelemetryRecord
|
||||
from .state_machine import (TaskInputs, TaskState, TaskStateConfig,
|
||||
TaskStateMachine)
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
def _monotonic_ms() -> int:
|
||||
return int(time.monotonic() * 1000)
|
||||
|
||||
|
||||
class TaskRunner:
|
||||
"""任务编排器。依赖（除 sm 外均可注入 mock）：
|
||||
|
||||
cfg BridgeConfig —— ground_inject.timeout_ms 等
|
||||
sm_cfg TaskStateConfig —— 切换半径/超时/起飞高度/居中阈值/循环频率
|
||||
px4 Px4Gateway —— own_state()（姿态/位置/相对高度）
|
||||
bridge PodBridge —— step / set_heading_servo_enabled / pod_locked
|
||||
sm TaskStateMachine —— update()
|
||||
ground GroundLink —— latest()（地面注入坐标）
|
||||
drone MAVSDK System —— action.arm/takeoff/return_to_launch/land/disarm
|
||||
vision_fn callable->bool —— 我方视觉第二判据（可选，require_vision 时用）
|
||||
"""
|
||||
|
||||
def __init__(self, cfg, sm_cfg: TaskStateConfig, px4, bridge, sm: TaskStateMachine,
|
||||
ground, drone, vision_fn: Optional[Callable[[], bool]] = None,
|
||||
now_ms_fn: Callable[[], int] = _monotonic_ms,
|
||||
logger: Optional[TaskLogger] = None,
|
||||
pod_control=None) -> None:
|
||||
self._cfg = cfg
|
||||
self._sm_cfg = sm_cfg
|
||||
self._px4 = px4
|
||||
self._bridge = bridge
|
||||
self._sm = sm
|
||||
self._ground = ground
|
||||
self._drone = drone
|
||||
self._vision_fn = vision_fn
|
||||
self._now = now_ms_fn
|
||||
self._logger = logger # 结构化任务日志（可选，B 阶段）
|
||||
self._pod_control = pod_control # 吊舱识别/锁定控制（可选，真机）
|
||||
self._centered_since_ms: Optional[int] = None
|
||||
self._rtl_requested = False
|
||||
self._emergency = False
|
||||
self._rtl_done = False
|
||||
|
||||
# --- 外部控制 -----------------------------------------------------------
|
||||
def request_rtl(self) -> None:
|
||||
"""外部请求返航（如收到地面站返航命令 / 人工按键）。"""
|
||||
self._rtl_requested = True
|
||||
|
||||
def trigger_emergency(self) -> None:
|
||||
"""外部触发紧急终止（丢控 / RC 接管 / 失控）。"""
|
||||
self._emergency = True
|
||||
|
||||
@property
|
||||
def state(self) -> TaskState:
|
||||
return self._sm.state
|
||||
|
||||
@property
|
||||
def rtl_done(self) -> bool:
|
||||
return self._rtl_done
|
||||
|
||||
# --- 主循环 -------------------------------------------------------------
|
||||
async def run(self, stop_event: asyncio.Event) -> None:
|
||||
"""主循环：采集快照 → 状态机 → 进入动作/持续动作，直到 RTL 完成或停止。"""
|
||||
await self._px4.start_telemetry() # 幂等：先拿遥测，供起飞到位/距离计算
|
||||
if self._logger is not None:
|
||||
self._logger.open()
|
||||
period_s = 1.0 / self._sm_cfg.loop_frequency_hz
|
||||
try:
|
||||
while not stop_event.is_set():
|
||||
now_ms = self._now()
|
||||
inputs = self._collect_inputs(now_ms)
|
||||
tr = self._sm.update(inputs, now_ms)
|
||||
if tr.occurred:
|
||||
logger.info("[Task] %s -> %s (%s)",
|
||||
tr.from_state.value, tr.to_state.value, tr.reason)
|
||||
if self._logger is not None:
|
||||
self._logger.log_event(
|
||||
now_ms, "state", from_state=tr.from_state.value,
|
||||
to_state=tr.to_state.value, reason=tr.reason)
|
||||
await self._on_enter(tr.to_state, now_ms)
|
||||
out = await self._tick(now_ms)
|
||||
if self._logger is not None:
|
||||
self._logger.log_telemetry(
|
||||
now_ms, self._build_telemetry_record(now_ms, inputs, out))
|
||||
if self._finished():
|
||||
break
|
||||
await asyncio.sleep(period_s)
|
||||
finally:
|
||||
await self._teardown()
|
||||
if self._logger is not None:
|
||||
self._logger.close()
|
||||
|
||||
# --- 输入采集 -----------------------------------------------------------
|
||||
def _collect_inputs(self, now_ms: int) -> TaskInputs:
|
||||
"""聚合 PX4 / 注入坐标 / 吊舱 / 视觉为一份 TaskInputs 快照。"""
|
||||
own = self._px4.own_state()
|
||||
|
||||
injected = self._ground.latest()
|
||||
target_injected = bool(
|
||||
injected.time_ms > 0
|
||||
and not is_stale(injected.time_ms, now_ms,
|
||||
self._cfg.ground_inject.timeout_ms))
|
||||
|
||||
distance = math.inf
|
||||
if target_injected and own.position_valid:
|
||||
distance = haversine_distance_m(
|
||||
own.latitude_deg, own.longitude_deg,
|
||||
injected.latitude_deg, injected.longitude_deg)
|
||||
|
||||
takeoff_reached = (own.relative_altitude_m
|
||||
>= self._sm_cfg.takeoff_altitude_m)
|
||||
target_locked = self._bridge.pod_locked(now_ms)
|
||||
vision = bool(self._vision_fn and self._vision_fn())
|
||||
centered, centered_s = self._update_centered(now_ms)
|
||||
|
||||
return TaskInputs(
|
||||
target_injected=target_injected,
|
||||
distance_to_target_m=distance,
|
||||
takeoff_reached=takeoff_reached,
|
||||
target_locked=target_locked,
|
||||
vision_has_target=vision,
|
||||
target_centered=centered,
|
||||
centered_duration_s=centered_s,
|
||||
rtl_requested=self._rtl_requested,
|
||||
emergency=self._emergency,
|
||||
)
|
||||
|
||||
def _update_centered(self, now_ms: int) -> tuple[bool, float]:
|
||||
"""脱靶量居中判定 + 持续时长累加（TRACK→APPROACH 判据）。
|
||||
|
||||
无遥测 / 任一轴脱靶量超出 center_threshold_deg → 清零累加。
|
||||
"""
|
||||
tel = self._bridge.latest_telemetry
|
||||
if tel is None:
|
||||
self._centered_since_ms = None
|
||||
return False, 0.0
|
||||
frame, _ = tel
|
||||
centered = (abs(frame.off_target_x_deg) < self._sm_cfg.center_threshold_deg
|
||||
and abs(frame.off_target_y_deg) < self._sm_cfg.center_threshold_deg)
|
||||
if not centered:
|
||||
self._centered_since_ms = None
|
||||
return False, 0.0
|
||||
if self._centered_since_ms is None:
|
||||
self._centered_since_ms = now_ms
|
||||
return True, (now_ms - self._centered_since_ms) / 1000.0
|
||||
|
||||
# --- 状态进入动作 -------------------------------------------------------
|
||||
async def _on_enter(self, state: TaskState, now_ms: int) -> None:
|
||||
if state is TaskState.ARM_TAKEOFF:
|
||||
await self._arm_takeoff()
|
||||
elif state is TaskState.GOTO:
|
||||
if not await self._bridge.start():
|
||||
logger.error("[Task] offboard 启动失败，紧急终止")
|
||||
self._emergency = True
|
||||
return
|
||||
self._bridge.set_heading_servo_enabled(False) # L2 地理引导
|
||||
if self._pod_control is not None:
|
||||
self._pod_control.arm_detection() # 真机：开启目标识别 + 自动锁定
|
||||
elif state is TaskState.TRACK:
|
||||
self._bridge.set_heading_servo_enabled(True) # L1 航向伺服
|
||||
elif state is TaskState.APPROACH:
|
||||
pass # 悬停由 _tick 持续发零速
|
||||
elif state is TaskState.RTL:
|
||||
if self._pod_control is not None:
|
||||
self._pod_control.stop_tracking() # 真机：停止跟踪 + 识别关（bridge 停机前）
|
||||
await self._start_rtl()
|
||||
|
||||
# --- 每周期持续动作 -----------------------------------------------------
|
||||
async def _tick(self, now_ms: int):
|
||||
"""每周期持续动作。返回本拍控制输出（GOTO/TRACK 有，其余 None）。"""
|
||||
state = self._sm.state
|
||||
if state in (TaskState.GOTO, TaskState.TRACK):
|
||||
return await self._bridge.step(now_ms)
|
||||
elif state is TaskState.APPROACH:
|
||||
await self._px4.send_brake_hover(now_ms) # 悬停目标上空（保 offboard 心跳）
|
||||
elif state is TaskState.RTL:
|
||||
await self._check_landed()
|
||||
return None
|
||||
# WAIT / ARM_TAKEOFF：无持续动作（待命 / 等起飞高度）
|
||||
|
||||
def _build_telemetry_record(self, now_ms: int, inputs: TaskInputs,
|
||||
out) -> TelemetryRecord:
|
||||
"""聚合本拍遥测为一帧 TelemetryRecord（供日志 / 页面复用）。"""
|
||||
own = self._px4.own_state()
|
||||
tel = self._bridge.latest_telemetry
|
||||
off_x = off_y = 0.0
|
||||
if tel is not None:
|
||||
frame, _ = tel
|
||||
off_x = frame.off_target_x_deg
|
||||
off_y = frame.off_target_y_deg
|
||||
return TelemetryRecord(
|
||||
state=self._sm.state.value,
|
||||
pos_valid=own.position_valid,
|
||||
fix_type=own.fix_type,
|
||||
num_satellites=own.num_satellites,
|
||||
latitude_deg=own.latitude_deg,
|
||||
longitude_deg=own.longitude_deg,
|
||||
relative_altitude_m=own.relative_altitude_m,
|
||||
yaw_deg=math.degrees(own.yaw_rad),
|
||||
target_injected=inputs.target_injected,
|
||||
distance_to_target_m=inputs.distance_to_target_m,
|
||||
target_locked=inputs.target_locked,
|
||||
off_target_x_deg=off_x,
|
||||
off_target_y_deg=off_y,
|
||||
mode=out.mode.name if out is not None else "",
|
||||
yaw_rate_dps=out.yaw_rate_dps if out is not None else 0.0,
|
||||
vx_body_mps=out.vx_body_mps if out is not None else 0.0,
|
||||
vz_mps=out.vz_mps if out is not None else 0.0,
|
||||
)
|
||||
|
||||
# --- 飞行编排 -----------------------------------------------------------
|
||||
async def _arm_takeoff(self) -> None:
|
||||
try:
|
||||
await self._drone.action.arm()
|
||||
await self._drone.action.set_takeoff_altitude(
|
||||
self._sm_cfg.takeoff_altitude_m)
|
||||
await self._drone.action.takeoff()
|
||||
if self._logger is not None:
|
||||
self._logger.log_event(
|
||||
self._now(), "takeoff",
|
||||
altitude_m=self._sm_cfg.takeoff_altitude_m)
|
||||
except Exception as exc:
|
||||
logger.error("[Task] 起飞失败: %s", exc)
|
||||
self._emergency = True
|
||||
if self._logger is not None:
|
||||
self._logger.log_event(self._now(), "takeoff_failed",
|
||||
error=str(exc))
|
||||
|
||||
async def _start_rtl(self) -> None:
|
||||
try:
|
||||
await self._bridge.stop() # 先零速刹车 + 停 offboard/遥测/串口
|
||||
except Exception as exc:
|
||||
logger.warning("[Task] 停机异常: %s", exc)
|
||||
try:
|
||||
await self._drone.action.return_to_launch()
|
||||
except Exception as exc:
|
||||
logger.warning("[Task] RTL 失败，尝试降落: %s", exc)
|
||||
try:
|
||||
await self._drone.action.land()
|
||||
except Exception:
|
||||
pass
|
||||
if self._logger is not None:
|
||||
self._logger.log_event(self._now(), "rtl")
|
||||
|
||||
async def _check_landed(self) -> None:
|
||||
"""RTL 状态每周期取一帧 in_air 遥测；落地后 disarm 并标记完成。"""
|
||||
try:
|
||||
async for is_in_air in self._drone.telemetry.in_air():
|
||||
if not is_in_air and not self._rtl_done:
|
||||
self._rtl_done = True
|
||||
try:
|
||||
await self._drone.action.disarm()
|
||||
except Exception:
|
||||
pass
|
||||
break # 只取最新一帧
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# --- 收尾 ---------------------------------------------------------------
|
||||
def _finished(self) -> bool:
|
||||
return self._sm.state is TaskState.RTL and self._rtl_done
|
||||
|
||||
async def _teardown(self) -> None:
|
||||
"""退出主循环（含异常/中途停止）时的安全收尾。"""
|
||||
try:
|
||||
await self._bridge.stop()
|
||||
except Exception:
|
||||
pass
|
||||
@@ -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)
|
||||
Reference in New Issue
Block a user