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。
682 lines
33 KiB
Python
682 lines
33 KiB
Python
"""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
|