"""runner.py —— 任务编排器：驱动任务状态机跑通 WAIT→…→RTL 全流程。 「预定流程.md」天空侧的落地：把已通的 L1（航向伺服 TrackingControlLaw）与 L2（地理引导 GuidanceLaw）按任务阶段编排，配合地面坐标注入（GroundLink）与 状态机（TaskStateMachine），串成一条可运行的全流程。 职责边界： * 状态机（TaskStateMachine）只做「状态 + 转移判据」决策； * 本编排器负责「采集快照 → 喂状态机 → 按状态执行动作」； * 地面站命令（RTL/紧急/吊舱手动指令，经 GroundLink 队列）主循环逐拍分派； * 飞控状态文本（STATUSTEXT，警告/异常）逐拍落任务日志（fc_status 事件）； * 闭环控制复用 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 visual_track_control.types import ControlMode, ControlOutput from .log import TaskLogger, TelemetryRecord from .return_home import HomingController, ReturnConfig from .state_machine import (TaskInputs, TaskState, TaskStateConfig, TaskStateMachine) logger = logging.getLogger(__name__) # 自定义返航的阶段 _RTL_RETURN = "return" # 转场：飞回起飞点，保持转场高度 _RTL_YAW = "yaw" # 到位：把机头转回初始航向 _RTL_LAND = "land" # 降落：交给飞控垂直降落 _RTL_FALLBACK = "fallback" # 兜底：退回 PX4 原生 RTL（会爬升，仅异常时使用） _RTL_IDLE = "idle" # 地面站吊舱命令 → PodController 方法（与 ground_station/app.py 白名单对齐） _POD_COMMAND_METHODS = { "pod_detect_on": "arm_detection", "pod_auto_lock": "auto_lock", "pod_stop_track": "stop_track", "pod_detect_off": "stop_tracking", "pod_center": "center", } # 搜索扫视换向周期计数中「向右」的半周期序号（0 → 右，1 → 左，交替） _SWEEP_RIGHT, _SWEEP_LEFT = 0, 1 # RTL 落地判定：单次等待 in_air 遥测的上限（秒）。MAVSDK 遥测流挂起时 # `async for` 会永久阻塞并卡死主循环，故必须有兜底（超时 → 下一拍重试）。 _IN_AIR_TIMEOUT_S = 5.0 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 """ def __init__(self, cfg, sm_cfg: TaskStateConfig, px4, bridge, sm: TaskStateMachine, ground, drone, now_ms_fn: Callable[[], int] = _monotonic_ms, logger: Optional[TaskLogger] = None, pod_control=None, land_instead_of_rtl: bool = False, return_cfg: Optional[ReturnConfig] = 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._now = now_ms_fn self._logger = logger # 结构化任务日志（可选，B 阶段） self._pod_control = pod_control # 吊舱识别/锁定控制（可选，真机） self._land_instead_of_rtl = land_instead_of_rtl # 收尾改用就地降落 self._return_cfg = return_cfg or ReturnConfig() self._centered_since_ms: Optional[int] = None self._rtl_requested = False self._emergency = False self._rtl_done = False # SEARCH 扫视状态：当前扫向（右/左）与本次扫向的起始时刻 self._sweep_dir = _SWEEP_RIGHT self._sweep_since_ms: Optional[int] = None self._pod_detection_on = False # 吊舱识别/自动锁定只开一次（防重发 0x07） self._takeoff_done = False # 解锁/起飞只执行一次（防空中重复起飞） self._in_air_stream = None # in_air 遥测流复用（避免每拍重新订阅） self._rtl_entered_ms: Optional[int] = None # 进入 RTL 时刻（落地等待上限用） # --- 自定义返航（回起飞点 → 恢复初始航向 → 降落）--- self._home_lat: Optional[float] = None # 起飞点纬度（解锁时刻记录） self._home_lon: Optional[float] = None self._initial_yaw_rad: Optional[float] = None # 起飞时刻航向 self._bridge_started = False # 是否已进入 offboard（未起飞则不能自主返航） self._rtl_phase = _RTL_IDLE self._return_ctrl: Optional[HomingController] = None self._return_started_ms: Optional[int] = None self._yaw_started_ms: Optional[int] = None self._land_cmd_sent = False # --- 外部控制 ----------------------------------------------------------- def request_rtl(self) -> None: """外部请求返航（如收到地面站返航命令 / 人工按键）。""" self._rtl_requested = True def trigger_emergency(self) -> None: """外部触发紧急终止（丢控 / RC 接管 / 失控）。""" self._emergency = True # --- 地面站命令分派 ----------------------------------------------------- def _apply_ground_commands(self) -> None: """排空地面站命令队列并逐条分派（主循环每拍调用）。""" for name in self._ground.drain_commands(): self._apply_command(name) def _apply_command(self, name: str) -> None: """分派一条地面站命令；未知命令名忽略并警告。""" if name == "rtl": self.request_rtl() elif name == "emergency": self.trigger_emergency() elif name in _POD_COMMAND_METHODS: if self._pod_control is None: logger.warning("[Task] 地面站命令 %s 被忽略：未配置吊舱控制（SITL）", name) return try: getattr(self._pod_control, _POD_COMMAND_METHODS[name])() except Exception as exc: logger.warning("[Task] 吊舱命令 %s 执行失败: %s", name, exc) return else: logger.warning("[Task] 未知地面站命令: %s", name) return logger.info("[Task] 地面站命令: %s", name) if self._logger is not None: self._logger.log_event(self._now(), "command", name=name) def _log_fc_messages(self) -> None: """把飞控状态文本（STATUSTEXT）落任务日志并输出控制台（每拍调用）。""" for sev, text in self._px4.take_fc_messages(): logger.info("[FC] %s: %s", sev, text) if self._logger is not None: self._logger.log_event(self._now(), "fc_status", severity=sev, text=text) @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() # 记初始状态：页面「任务状态」取日志最后一个 state 事件，缺此行会显示上次旧状态 self._logger.log_event(self._now(), "state", from_state=self._sm.state.value, to_state=self._sm.state.value, reason="任务启动") period_s = 1.0 / self._sm_cfg.loop_frequency_hz try: while not stop_event.is_set(): now_ms = self._now() try: await self._iteration(now_ms) except asyncio.CancelledError: raise except Exception as exc: # 单周期异常不得让任务带着 offboard 指令静默停摆：转紧急 → 下一拍 # 状态机转 RTL（与 bridge.run() 的按周期护栏同思路，但这里必须 # 升级为返航，因为再没有别的循环会接管飞机）。 logger.error("[Task] 任务周期异常，转紧急返航: %s", exc) self._emergency = True if self._logger is not None: self._logger.log_event(self._now(), "iteration_error", error=f"{type(exc).__name__}: {exc}") await self._safe_abort() if self._finished(): break await asyncio.sleep(period_s) finally: await self._teardown() if self._logger is not None: self._logger.close() async def _iteration(self, now_ms: int) -> None: """单周期：命令 → 快照 → 状态机 → 进入动作 → 持续动作 → 日志。""" self._apply_ground_commands() # 地面站命令先于快照采集生效 self._log_fc_messages() 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)) async def _safe_abort(self) -> None: """异常兜底：直接进入 RTL 流程（不依赖状态机下一拍转移）。 异常可能来自采集/控制律/日志任意环节，下一拍未必能正常跑完转移；因此在 兜底里显式走一次 RTL 进入动作（停机 + 返航），保证飞机不会停在 offboard 速度指令上。 """ if self._sm.state is TaskState.RTL: return self._sm.force_state(TaskState.RTL, self._now()) self._rtl_entered_ms = self._now() if self._pod_control is not None: try: self._pod_control.stop_tracking() except Exception: pass try: # 兜底不走自定义返航：异常后不再信任自身状态/定位，直接交给 PX4 原生 RTL await self._begin_rtl(self._now(), force_fallback=True) except Exception as exc: logger.error("[Task] 异常兜底返航失败: %s", exc) # --- 输入采集 ----------------------------------------------------------- def _collect_inputs(self, now_ms: int) -> TaskInputs: """聚合 PX4 / 注入坐标 / 吊舱 / 视觉为一份 TaskInputs 快照。""" own = self._px4.own_state() self._maybe_record_home(own) # 起飞阶段定位可能晚到：每拍补记一次 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) # 起飞到达判据带容差：悬停/高度估计通常略低于设定目标（实测 SITL 4.99m vs 5.0m）， # 无容差会差厘米误判"起飞超时"→ 真机上更明显 takeoff_reached = (own.relative_altitude_m >= self._sm_cfg.takeoff_altitude_m - self._sm_cfg.takeoff_reached_tolerance_m) target_locked = self._bridge.pod_locked(now_ms) 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, 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: """进入状态的**一次性**动作。 注意：这些动作只在「首次」进入时执行——任务中途可能需要多次经过 GOTO/ SEARCH（丢目标回搜），重复 `offboard.start()` / `arm_detection()` （0x07 目标识别开会打断正在进行的跟踪）会造成真机自激，故以标志位守卫。 """ 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_started = True self._bridge.set_heading_servo_enabled(False) # L2 地理引导 self._start_pod_detection() # 首次：开识别 + 自动锁定 elif state is TaskState.SEARCH: self._bridge.set_heading_servo_enabled(False) # 搜索不用控制律 self._sweep_dir = _SWEEP_RIGHT # 每次进入从右向起扫 self._sweep_since_ms = now_ms 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: self._rtl_entered_ms = now_ms if self._pod_control is not None: self._pod_control.stop_tracking() # 真机：停止跟踪 + 识别关 await self._begin_rtl(now_ms) def _start_pod_detection(self) -> None: """首次到达引导/搜索阶段时开启吊舱目标识别 + 自动锁定（只发一次）。""" if self._pod_control is None or self._pod_detection_on: return self._pod_detection_on = True self._pod_control.arm_detection() # --- SEARCH 扫视 -------------------------------------------------------- def _search_yaw_rate(self, now_ms: int) -> float: """SEARCH 扫视角速度：按 sweep_reverse_s 交替左右，0 = 单向扫视。 吊舱相机刚性固连机体（无电机、不独立转动），故只能靠机头旋转改变观察 方向；扫视期间零线速度，飞机保持悬停在参考点附近。 """ rate = self._sm_cfg.sweep_rate_dps if rate == 0.0: return 0.0 period_s = self._sm_cfg.sweep_reverse_s if period_s > 0.0: if self._sweep_since_ms is None: self._sweep_since_ms = now_ms elif (now_ms - self._sweep_since_ms) >= period_s * 1000.0: self._sweep_since_ms = now_ms self._sweep_dir = (_SWEEP_LEFT if self._sweep_dir == _SWEEP_RIGHT else _SWEEP_RIGHT) sign = 1.0 if self._sweep_dir == _SWEEP_RIGHT else -1.0 return sign * rate # --- 每周期持续动作 ----------------------------------------------------- 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.SEARCH: # 原地偏航扫视（同时 poll 吊舱，保遥测新鲜，否则搜到也切不进 TRACK） await self._bridge.search_step(now_ms, self._search_yaw_rate(now_ms)) elif state is TaskState.APPROACH: await self._px4.send_brake_hover(now_ms) # 悬停目标上空（保 offboard 心跳） elif state is TaskState.RTL: await self._tick_rtl(now_ms) 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: """解锁 + 起飞到悬停高度（每次任务只执行一次）。 重复执行会导致：机已在空中时再次 arm/takeoff → PX4 拒绝甚至触发保护， 且日志看起来像"重新起飞"。故用标志位守卫，重复进入只告警不改动飞机。 """ if self._takeoff_done: logger.warning("[Task] 已在飞行中，忽略重复的解锁/起飞请求") return self._takeoff_done = True # 解锁前记录起飞点与初始航向（自定义返航要用；PX4 的 home 此时已固定） self._record_home() 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)) def _record_home(self) -> bool: """尝试记录起飞点与初始航向；成功返回 True（可重复调用，只记一次）。""" if self._home_lat is not None: return True try: own = self._px4.own_state() except Exception as exc: # 遥测不可用时不阻塞起飞 logger.warning("[Task] 读取起飞点失败: %s", exc) return False return self._maybe_record_home(own) def _maybe_record_home(self, own) -> bool: """在**起飞前/起飞中**用首个有效定位记下起飞点与初始航向。 ⚠ SITL 实测踩到的坑：解锁发生在地面站坐标到达后约 0.3s，此时 `gps_info` （2Hz）可能还没到，`position_valid` 仍为 False——若只在解锁那一刻记一次， 起飞点就会丢失、自定义返航只能退回 PX4 RTL。故起飞阶段每拍补记。 只接受 WAIT / ARM_TAKEOFF 阶段的定位：进入 GOTO 后飞机已在空中， 那时的位置不该再当作"起飞点"。 """ if self._home_lat is not None: return True if not own.position_valid: return False if self._sm.state not in (TaskState.WAIT, TaskState.ARM_TAKEOFF): return False self._home_lat = own.latitude_deg self._home_lon = own.longitude_deg self._initial_yaw_rad = own.yaw_rad logger.info("[Task] 起飞点已记录: (%.7f, %.7f) 初始航向 %.1f°", self._home_lat, self._home_lon, math.degrees(self._initial_yaw_rad)) return True # --- 返航（自定义：回起飞点 → 恢复初始航向 → 降落）--------------------- def _custom_return_possible(self) -> bool: """能否走自定义返航（已起飞 + 有起飞点 + 位置有效）。""" if not self._return_cfg.enabled: return False if not self._bridge_started: return False # 没进过 offboard：还在待命/起飞段 if self._home_lat is None or self._home_lon is None: return False if self._initial_yaw_rad is None: return False try: return bool(self._px4.own_state().position_valid) except Exception: return False async def _begin_rtl(self, now_ms: int, force_fallback: bool = False) -> None: """进入返航：选择「自定义返航」或「就地降落」或「PX4 RTL 兜底」。""" if self._land_instead_of_rtl: self._rtl_phase = _RTL_LAND await self._start_land() return if force_fallback or not self._custom_return_possible(): reason = "异常兜底" if force_fallback else "未起飞/无起飞点" logger.info("[Task] 使用 PX4 原生 RTL（%s）", reason) await self._fallback_rtl() return own = self._px4.own_state() # 转场高度：取"当前高度"与"起飞高度"的较大者 —— 不低于任务高度， # 也不会为了返航额外爬高（这正是原生 RTL 会做的事）。 transit_alt = max(own.relative_altitude_m, self._sm_cfg.takeoff_altitude_m) self._return_ctrl = HomingController( self._return_cfg, self._home_lat, self._home_lon, self._initial_yaw_rad, transit_alt) self._return_started_ms = now_ms self._rtl_phase = _RTL_RETURN logger.info("[Task] 自定义返航：回起飞点 (%.7f, %.7f)，转场高度 %.1fm，" "到位后恢复航向 %.1f° 并降落", self._home_lat, self._home_lon, transit_alt, math.degrees(self._initial_yaw_rad)) if self._logger is not None: self._logger.log_event( now_ms, "return_home", home_lat=self._home_lat, home_lon=self._home_lon, transit_alt_m=transit_alt, initial_yaw_deg=math.degrees(self._initial_yaw_rad)) async def _return_step(self, now_ms: int) -> None: """转场阶段：朝起飞点飞，保持转场高度。""" cfg = self._return_cfg if (self._return_started_ms is not None and now_ms - self._return_started_ms >= cfg.timeout_s * 1000.0): logger.warning("[Task] 返航转场超时 %.0fs，退回 PX4 RTL", cfg.timeout_s) if self._logger is not None: self._logger.log_event(now_ms, "return_timeout", timeout_s=cfg.timeout_s) await self._fallback_rtl() return own = self._px4.own_state() if not own.position_valid: # 定位丢失：不能自主返航，先刹停等恢复（不盲目飞） logger.warning("[Task] 返航中定位失效，刹停等待") await self._px4.send_brake_hover(now_ms) return cmd = self._return_ctrl.update(own.latitude_deg, own.longitude_deg, own.yaw_rad, own.relative_altitude_m) out = ControlOutput(mode=ControlMode.VELOCITY_HEADING, valid=True, vx_body_mps=cmd.vx_mps, vz_mps=cmd.vz_mps, yaw_rate_dps=cmd.yaw_rate_dps) await self._px4.send_velocity_heading(out, now_ms) if cmd.arrived: logger.info("[Task] 已回到起飞点（距 %.1fm），开始恢复初始航向", cmd.distance_m) self._rtl_phase = _RTL_YAW self._yaw_started_ms = now_ms async def _yaw_align_step(self, now_ms: int) -> None: """恢复初始航向阶段：原地转回起飞时刻的航向。""" cfg = self._return_cfg own = self._px4.own_state() err, rate = self._return_ctrl.yaw_align(own.yaw_rad) # 保持转场高度（不额外爬升） out = ControlOutput(mode=ControlMode.VELOCITY_HEADING, valid=True, vx_body_mps=0.0, vz_mps=self._return_ctrl.alt_hold_vz( own.relative_altitude_m), yaw_rate_dps=rate) await self._px4.send_velocity_heading(out, now_ms) timed_out = (self._yaw_started_ms is not None and now_ms - self._yaw_started_ms >= cfg.yaw_align_timeout_s * 1000.0) if abs(err) <= cfg.yaw_align_tolerance_deg: logger.info("[Task] 航向已恢复（误差 %.1f°），开始降落", err) elif timed_out: logger.warning("[Task] 航向对齐超时（误差 %.1f°），仍进入降落", err) else: return if self._logger is not None: self._logger.log_event(now_ms, "yaw_restored", err_deg=err, timed_out=timed_out) self._rtl_phase = _RTL_LAND await self._start_land() async def _start_land(self) -> None: """退出 offboard 并垂直降落（不爬升）。""" if self._land_cmd_sent: return self._land_cmd_sent = True # 只退 offboard、保留遥测：落地判定与高度曲线都依赖遥测（SITL 实测过： # 若在这里就全停，日志会冻结在降落前的高度，看不出下降）。 stop_for_landing = getattr(self._bridge, "landing_stop", None) try: if stop_for_landing is not None: await stop_for_landing() else: await self._bridge.stop() except Exception as exc: logger.warning("[Task] 停机异常: %s", exc) try: await self._drone.action.land() except Exception as exc: logger.warning("[Task] 降落指令失败，尝试 PX4 RTL 兜底: %s", exc) try: await self._drone.action.return_to_launch() except Exception: pass if self._logger is not None: self._logger.log_event(self._now(), "rtl", action="land") async def _fallback_rtl(self) -> None: """兜底：停机后交给 PX4 原生 RTL（会先爬升到 RTL_RETURN_ALT）。""" self._rtl_phase = _RTL_FALLBACK try: await self._bridge.stop() 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", action="rtl") async def _tick_rtl(self, now_ms: int) -> None: """RTL 状态机：按阶段推进（转场 → 恢复航向 → 降落 / 兜底）。""" phase = self._rtl_phase if phase == _RTL_RETURN: await self._return_step(now_ms) elif phase == _RTL_YAW: await self._yaw_align_step(now_ms) elif phase == _RTL_LAND: await self._check_landed() elif phase == _RTL_FALLBACK: await self._check_landed() # 整体落地等待上限：任何阶段都不得无限等 if (not self._rtl_done and self._rtl_entered_ms is not None and now_ms - self._rtl_entered_ms >= self._sm_cfg.rtl_timeout_s * 1000.0): logger.warning("[Task] RTL 等待落地超过 %.0fs，结束任务（无法确认落地）", self._sm_cfg.rtl_timeout_s) self._rtl_done = True if self._logger is not None: self._logger.log_event(now_ms, "rtl_timeout", timeout_s=self._sm_cfg.rtl_timeout_s) async def _check_landed(self) -> None: """RTL 状态每周期取一帧 in_air 遥测；落地后 disarm 并标记完成。 两个要点： * `in_air()` 是 MAVSDK 遥测流：流挂起时 `async for` 会**永久阻塞**， 主循环随之卡死（rtl_done 永不置位）。故加超时兜底，超时下拍重试。 * 生成器**复用**（`_in_air_stream`）：每拍新建一个 `in_air()` 会在 mavsdk_server 侧反复订阅/退订同一条流（20Hz × 整个 RTL 段），是 回调队列的无谓负担；复用后只订阅一次。 """ try: if self._in_air_stream is None: self._in_air_stream = self._drone.telemetry.in_air() async with asyncio.timeout(_IN_AIR_TIMEOUT_S): async for is_in_air in self._in_air_stream: 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 (TimeoutError, asyncio.TimeoutError): logger.debug("[Task] in_air 遥测超时 %.1fs，下拍重试", _IN_AIR_TIMEOUT_S) except Exception: # 建流或消费失败都算流不可用：丢弃缓存，下拍重建（不永久卡在坏流上） self._in_air_stream = None # --- 收尾 --------------------------------------------------------------- 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