实现预定流程全链路: 任务状态机 + 编排器 + 地面站 + 真机/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,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
|
||||
Reference in New Issue
Block a user