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。
441 lines
22 KiB
Python
441 lines
22 KiB
Python
"""bridge.py —— 吊舱 ↔ PX4 桥接主循环（asyncio 胶水层）。
|
||
|
||
闭环（每控制周期，guidance.frequency_hz）：
|
||
① poll 吊舱串口 → 缓存最新 表4 目标经纬度 / 表2-2 遥测（带单调时间戳）
|
||
② 按 link.attitude_downlink_hz 节流，读 PX4 遥测 → 打包 表3 载机姿态下发吊舱
|
||
（吊舱据此解算目标经纬度，文档要求 1~60Hz）
|
||
③ 目标门控：表2-2 状态字节1 bit2=锁定 且遥测在 link.telemetry_timeout_ms 内，
|
||
表4 坐标才视为有效（规格：表4 周期发送，搜索状态下坐标无意义；激光无效时
|
||
吊舱不解算、表4 自然中断，由 guidance.target_stale_ms 兜底）
|
||
④ 自身状态 + 最新目标 → GuidanceInput → GuidanceLaw.update → ControlOutput
|
||
⑤ 按模式分发：VELOCITY_HEADING → offboard 速度；BRAKE_HOVER → 零速刹车
|
||
（引导律对一切不安全情形输出 BRAKE_HOVER，故 offboard 始终有心跳，无失联悬停）
|
||
|
||
run() 主循环带异常护栏：单周期异常（如串口拔线）按节流节奏告警并补发零速刹车
|
||
保 offboard 心跳，任务不终止。
|
||
|
||
时钟一致性：Px4Gateway 与 PodBridge 必须共用同一单调毫秒时钟（OwnState.time_ms
|
||
与 step(now_ms) 同源），否则 position/target 超龄判定失效。connect() 工厂已统一。
|
||
"""
|
||
|
||
import asyncio
|
||
import logging
|
||
import math
|
||
import time
|
||
from datetime import datetime, timezone
|
||
from typing import Optional
|
||
|
||
from visual_track_control.staleness_checker import is_stale
|
||
from visual_track_control.tracking_control_law import TrackingControlLaw
|
||
from visual_track_control.types import (ControlMode, ControlSnapshot,
|
||
TrackState)
|
||
|
||
from .config import BridgeConfig
|
||
from .ground_link import GroundTarget
|
||
from .guidance import GuidanceInput, GuidanceLaw
|
||
from .los_consistency import (LosConsistencyConfig, LosConsistencyMonitor,
|
||
LosStatus)
|
||
from .pod_serial import PodLink, PodSerial
|
||
from .protocol import (CarrierAttitude, ControlCommand, TargetGeo, Telemetry,
|
||
encode_attitude, encode_control)
|
||
from .px4_gateway import Px4Gateway
|
||
|
||
logger = logging.getLogger(__name__)
|
||
|
||
|
||
def _monotonic_ms() -> int:
|
||
return int(time.monotonic() * 1000)
|
||
|
||
|
||
class PodBridge:
|
||
"""px4：Px4Gateway（或等价 mock）；pod：PodLink（PodSerial 或内存 FakePod）。"""
|
||
|
||
def __init__(self, cfg: BridgeConfig, px4: Px4Gateway, pod: PodLink,
|
||
now_ms_fn=_monotonic_ms,
|
||
ground_target_fn=None) -> None:
|
||
self._cfg = cfg
|
||
self._px4 = px4
|
||
self._pod = pod
|
||
self._now_ms = now_ms_fn
|
||
self._ground_target_fn = ground_target_fn # 地面注入坐标访问器（A2，无激光替代表4）
|
||
self._use_heading_servo = cfg.heading_servo.enabled # 运行时控制律选择（A3 状态机切换）
|
||
self._guidance = GuidanceLaw(cfg.guidance)
|
||
self._tracking = TrackingControlLaw(cfg.tracking) # L1 航向伺服（方案 §0）
|
||
# 目标有效性第二判据：LOS 运动学一致性（取代已废弃的 require_vision）
|
||
hs = cfg.heading_servo
|
||
self._los_monitor = LosConsistencyMonitor(LosConsistencyConfig(
|
||
enabled=hs.los_consistency_enabled,
|
||
window_ms=hs.los_window_ms,
|
||
min_yaw_change_deg=hs.los_min_yaw_change_deg,
|
||
max_residual_deg=hs.los_max_residual_deg))
|
||
self._los_consistent = True # 最近一次一致性判定（公共门控用）
|
||
self._los_logged_ok = True # 日志：只在判定翻转时打印
|
||
self._attitude_period_ms = max(
|
||
1, int(round(1000.0 / cfg.link.attitude_downlink_hz)))
|
||
self._last_attitude_ms: Optional[int] = None
|
||
# 生效律周期：step() 被外部以更高频率调用时（任务编排器统一 20Hz，而
|
||
# L2 引导律配置为 10Hz），按此节流，避免 dt 相关项/限幅被以双倍速率推进。
|
||
self._law_period_ms = max(
|
||
1, int(round(1000.0 / max(1e-6, self._active_frequency_hz()))))
|
||
self._last_law_ms: Optional[int] = None
|
||
self._last_law_output = None # 节流期内重发的上一拍输出
|
||
self._latest_target = None # (TargetGeo, time_ms)
|
||
self._latest_telemetry = None # (Telemetry, time_ms)
|
||
self._step_failures = 0 # run() 连续异常计数（告警节流用）
|
||
self._stopped = False # stop() 幂等标志（真机实测：RTL 与收尾会各调一次）
|
||
self._landing_stopped = False # 是否已为降落退出 offboard（遥测仍保留）
|
||
|
||
# --- 生命周期 -----------------------------------------------------------
|
||
async def start(self) -> bool:
|
||
"""启动 PX4 遥测订阅与 offboard；offboard 失败返回 False（不应继续闭环）。"""
|
||
await self._px4.start_telemetry()
|
||
if not await self._px4.start_offboard():
|
||
logger.error("[Bridge] offboard 启动失败，桥接未运行")
|
||
return False
|
||
self._stopped = False
|
||
logger.info("[Bridge] 启动：引导 %.1fHz，姿态下发 %.1fHz",
|
||
self._cfg.guidance.frequency_hz,
|
||
self._cfg.link.attitude_downlink_hz)
|
||
return True
|
||
|
||
async def stop(self) -> None:
|
||
"""安全停机：先零速刹车，再停 offboard / 遥测 / 串口。
|
||
|
||
幂等：任务编排器在进入 RTL 时停一次，收尾 `_teardown` 还会再停一次
|
||
（真机在环实测确认）。重复执行会二次关闭串口、二次停 offboard，
|
||
在 MAVSDK/串口驱动上产生无意义告警，故第二次直接返回。
|
||
"""
|
||
if self._stopped:
|
||
return
|
||
self._stopped = True
|
||
try:
|
||
await self._px4.send_brake_hover(self._now_ms())
|
||
finally:
|
||
await self._px4.stop_offboard()
|
||
await self._px4.stop_telemetry()
|
||
self._pod.close()
|
||
self._guidance.reset()
|
||
self._tracking.reset()
|
||
logger.info("[Bridge] 已停止")
|
||
|
||
async def landing_stop(self) -> None:
|
||
"""降落前停机：零速刹车 + 退出 offboard，但**保留遥测与串口**。
|
||
|
||
与 `stop()` 的差别：交给飞控 `land()` 之后仍需遥测来判定落地、记录高度
|
||
曲线。若此时就 `stop_telemetry()`，落地段的遥测会冻结在最后高度（SITL
|
||
实测：日志停在 5m，看不到下降）。完整清理留给收尾 `stop()`。
|
||
"""
|
||
try:
|
||
await self._px4.send_brake_hover(self._now_ms())
|
||
finally:
|
||
await self._px4.stop_offboard()
|
||
self._landing_stopped = True
|
||
logger.info("[Bridge] 已退出 offboard（保留遥测用于落地判定）")
|
||
|
||
# --- 主循环 -------------------------------------------------------------
|
||
async def run(self, stop_event: asyncio.Event) -> None:
|
||
"""按引导频率循环 step()，直到 stop_event 置位。
|
||
|
||
异常护栏：单周期抛错（典型：吊舱串口拔线 SerialException/OSError）不终止
|
||
任务，按「第 1 次 + 每满 100 次」节奏告警（对齐 WarnThrottle 语义），并补发
|
||
零速刹车保住 offboard 心跳——否则 PX4 数秒内触发 offboard 失联保护。
|
||
CancelledError 继承 BaseException，不在此拦截。
|
||
"""
|
||
period_s = 1.0 / self._active_frequency_hz()
|
||
while not stop_event.is_set():
|
||
try:
|
||
await self.step(self._now_ms())
|
||
self._step_failures = 0
|
||
except Exception as exc:
|
||
self._step_failures += 1
|
||
if self._step_failures == 1 or self._step_failures % 100 == 0:
|
||
logger.warning("[Bridge] 控制周期异常(第 %d 次): %s",
|
||
self._step_failures, exc)
|
||
try:
|
||
await self._px4.send_brake_hover(self._now_ms())
|
||
except Exception:
|
||
pass # 补发失败不掩盖原异常，下周期重试
|
||
await asyncio.sleep(period_s)
|
||
|
||
def _active_frequency_hz(self) -> float:
|
||
"""当前生效控制律的循环频率：L1 用 tracking.control，L2 用 guidance。"""
|
||
if self._use_heading_servo:
|
||
return self._cfg.tracking.control.frequency_hz
|
||
return self._cfg.guidance.frequency_hz
|
||
|
||
def set_heading_servo_enabled(self, enabled: bool) -> None:
|
||
"""运行时切换控制律（任务状态机 GOTO(L2) ↔ TRACK(L1) 切换，A3）。
|
||
|
||
切换时 reset 即将启用的律（清积分/限幅器历史），使指令从零渐变，
|
||
避免切换瞬间输出跳变。L1 用吊舱角度直入，L2 用地理引导（注入坐标/表4）。
|
||
"""
|
||
if enabled == self._use_heading_servo:
|
||
return
|
||
self._use_heading_servo = enabled
|
||
if enabled:
|
||
self._tracking.reset()
|
||
# 进入 L1：LOS 一致性历史从零开始（避免上一段跟踪的样本污染本次判定）
|
||
self._los_monitor.reset()
|
||
self._los_consistent = True
|
||
self._los_logged_ok = True
|
||
else:
|
||
self._guidance.reset()
|
||
# 切律后按新律频率重算节流周期，并允许立即出第一拍
|
||
self._law_period_ms = max(
|
||
1, int(round(1000.0 / max(1e-6, self._active_frequency_hz()))))
|
||
self._last_law_ms = None
|
||
|
||
async def step(self, now_ms: int):
|
||
"""单控制周期（显式传入时钟，便于确定性单测）。返回 ControlOutput。
|
||
|
||
按运行时选择的控制律在 L1（TrackingControlLaw，吊舱角度直入）与
|
||
L2（GuidanceLaw，表4/注入坐标地理引导）间切换（方案 §7）。
|
||
"""
|
||
# 链路维持先做：poll 吊舱 + 按 link.attitude_downlink_hz 下发表3 姿态。
|
||
# 这两件事与生效律频率无关（吊舱解算依赖 表3，目标门控依赖遥测新鲜度）。
|
||
self._ingest_pod_frames(now_ms)
|
||
own = self._px4.own_state()
|
||
self._downlink_attitude(own, now_ms)
|
||
|
||
# 生效律节流：外部（任务编排器）统一 20Hz 调用，而 L2 引导律配置为 10Hz。
|
||
# 未到下一拍时**重发上一拍输出**（而非刹车），否则会与本拍的 GOTO 指令打架。
|
||
if (self._last_law_ms is not None
|
||
and now_ms - self._last_law_ms < self._law_period_ms
|
||
and self._last_law_output is not None):
|
||
await self._dispatch(self._last_law_output, now_ms)
|
||
return self._last_law_output
|
||
|
||
self._last_law_ms = now_ms
|
||
if self._use_heading_servo:
|
||
out = self._tracking.update(self._build_snapshot(own, now_ms), now_ms)
|
||
else:
|
||
out = self._guidance.update(
|
||
self._build_guidance_input(own, now_ms), now_ms)
|
||
await self._dispatch(out, now_ms)
|
||
self._last_law_output = out
|
||
return out
|
||
|
||
async def search_step(self, now_ms: int, yaw_rate_dps: float) -> None:
|
||
"""搜索周期（SEARCH 态）：只维持链路与心跳 + 原地偏航扫视，不出控制律。
|
||
|
||
之所以不让 SEARCH 走 step()：该态既不用 L2 地理引导（已在参考点，再发
|
||
速度会漂离），也不用 L1（还没锁定目标，控制律会判丢输出刹车）。此处
|
||
**仍要 poll 吊舱并下发表3 姿态**——否则搜索期间吊舱遥测超龄、锁定位永远
|
||
不新鲜，搜到目标也切不进 TRACK。
|
||
"""
|
||
self._ingest_pod_frames(now_ms)
|
||
own = self._px4.own_state()
|
||
self._downlink_attitude(own, now_ms)
|
||
await self._px4.send_yaw_rate(yaw_rate_dps)
|
||
|
||
# --- 吊舱侧 -------------------------------------------------------------
|
||
def _ingest_pod_frames(self, now_ms: int) -> None:
|
||
for frame in self._pod.poll():
|
||
if isinstance(frame, TargetGeo):
|
||
self._latest_target = (frame, now_ms)
|
||
elif isinstance(frame, Telemetry):
|
||
self._latest_telemetry = (frame, now_ms)
|
||
|
||
def _downlink_attitude(self, own, now_ms: int) -> None:
|
||
if (self._last_attitude_ms is not None
|
||
and now_ms - self._last_attitude_ms < self._attitude_period_ms):
|
||
return
|
||
self._pod.send(encode_attitude(self._build_attitude(own)))
|
||
self._last_attitude_ms = now_ms
|
||
|
||
def _build_attitude(self, own) -> CarrierAttitude:
|
||
t = datetime.now(timezone.utc)
|
||
return CarrierAttitude(
|
||
roll_deg=math.degrees(own.roll_rad),
|
||
pitch_deg=math.degrees(own.pitch_rad),
|
||
yaw_deg=math.degrees(own.yaw_rad),
|
||
latitude_deg=own.latitude_deg,
|
||
longitude_deg=own.longitude_deg,
|
||
abs_alt_m=own.absolute_altitude_m,
|
||
rel_alt_m=own.relative_altitude_m,
|
||
year=t.year, month=t.month, day=t.day,
|
||
hour=t.hour, minute=t.minute, second=t.second,
|
||
centisecond=t.microsecond // 10000,
|
||
airspeed_mps=0.0, # 无空速管，置 0（文档允许）
|
||
ground_speed_mps=own.ground_speed_mps,
|
||
)
|
||
|
||
def send_control(self, cmd: ControlCommand) -> None:
|
||
"""触发式下发 表2-1 控制指令（如开目标识别 / 框选跟踪）。"""
|
||
self._pod.send(encode_control(cmd))
|
||
|
||
# --- 飞控侧 -------------------------------------------------------------
|
||
def _build_guidance_input(self, own, now_ms: int) -> GuidanceInput:
|
||
injected = self._injected_target(now_ms)
|
||
if injected is not None:
|
||
# 地面注入坐标优先（本吊舱无激光 → 表4 永不出，目标坐标改走地面站）
|
||
return GuidanceInput(
|
||
own_lat_deg=own.latitude_deg,
|
||
own_lon_deg=own.longitude_deg,
|
||
own_yaw_deg=math.degrees(own.yaw_rad),
|
||
own_position_valid=own.position_valid,
|
||
own_position_time_ms=own.position_time_ms, # position 流专用时间戳
|
||
target_lat_deg=injected.latitude_deg,
|
||
target_lon_deg=injected.longitude_deg,
|
||
target_valid=True, # 注入坐标已验证新鲜
|
||
target_time_ms=injected.time_ms,
|
||
)
|
||
# 回退表4（有激光吊舱 / 未接入地面站）
|
||
tgt = self._latest_target
|
||
locked = self._pod_locked(now_ms)
|
||
return GuidanceInput(
|
||
own_lat_deg=own.latitude_deg,
|
||
own_lon_deg=own.longitude_deg,
|
||
own_yaw_deg=math.degrees(own.yaw_rad),
|
||
own_position_valid=own.position_valid,
|
||
own_position_time_ms=own.position_time_ms, # position 流专用时间戳
|
||
target_lat_deg=tgt[0].latitude_deg if tgt else 0.0,
|
||
target_lon_deg=tgt[0].longitude_deg if tgt else 0.0,
|
||
target_valid=tgt is not None and locked, # 未锁定=目标无效
|
||
target_time_ms=tgt[1] if tgt else 0,
|
||
)
|
||
|
||
def _injected_target(self, now_ms: int) -> Optional[GroundTarget]:
|
||
"""地面注入坐标（新鲜有效则返回，否则 None → 回退表4 / 无目标）。
|
||
|
||
本吊舱无激光 → 表4 永不出，目标坐标改由地面站注入（预定流程「地面给坐标」）。
|
||
未启用 / 未注入 / 超龄 → None（下游按无目标刹车）。"""
|
||
if not self._cfg.ground_inject.enabled:
|
||
return None
|
||
if self._ground_target_fn is None:
|
||
return None
|
||
target = self._ground_target_fn()
|
||
if target is None or target.time_ms <= 0:
|
||
return None
|
||
if is_stale(target.time_ms, now_ms, self._cfg.ground_inject.timeout_ms):
|
||
return None
|
||
return target
|
||
|
||
def pod_locked(self, now_ms: int) -> bool:
|
||
"""目标门控（公共接口，供编排层/状态机复用，A3）。
|
||
|
||
= 吊舱 bit2 门控 **AND** LOS 运动学一致性判定。
|
||
这样"bit2 粘滞但角度冻结"的幽灵目标会让状态机退出 TRACK 重新搜索，
|
||
而不只是让控制律单方面刹车（否则状态机会一直以为还锁着）。
|
||
"""
|
||
return self._pod_locked(now_ms) and self._los_consistent
|
||
|
||
def _pod_locked(self, now_ms: int) -> bool:
|
||
"""吊舱 bit2 门控（**不含** LOS 一致性，供内部合成本拍快照用）。
|
||
|
||
表2-2 遥测在 link.telemetry_timeout_ms 内、状态字节1 bit2=锁定、
|
||
且（若 link.require_laser_valid）激光测距有效，才算"吊舱自报锁定"。
|
||
|
||
实测依据：部分吊舱目标丢失后**锁定位置位不复位**（脱靶量也没有无效标志位，
|
||
恒为 0），仅凭 bit2 会朝过期坐标继续飞；激光无效时吊舱不解算目标经纬度，
|
||
故此兜底。无激光或激光恒无效的吊舱把 require_laser_valid 置 False。
|
||
更进一步的"角度是否活体"由 LOS 一致性判据在 _build_snapshot 里叠加。
|
||
|
||
从未收到遥测 / 遥测超龄 / 搜索状态 / 激光无效 一律视为目标无效（宁可刹车）。"""
|
||
tel = self._latest_telemetry
|
||
if tel is None:
|
||
return False
|
||
frame, t_ms = tel
|
||
if is_stale(t_ms, now_ms, self._cfg.link.telemetry_timeout_ms):
|
||
return False
|
||
if not frame.is_target_locked:
|
||
return False
|
||
if self._cfg.link.require_laser_valid and frame.laser_range_m is None:
|
||
return False
|
||
return True
|
||
|
||
def _build_snapshot(self, own, now_ms: int) -> ControlSnapshot:
|
||
"""L1：PX4 姿态 + 吊舱表2-2 角度 → ControlSnapshot（角度直入，方案 §2.1/§2.3）。
|
||
|
||
姿态（yaw / attitude_present / attitude_time_ms）来自 PX4；视线角 β/α 来自
|
||
吊舱脱靶量与框架角，信源与符号由 heading_servo 配置合成：
|
||
β = beta_sign·(beta_from_frame·frame_yaw + beta_from_off_target·off_target_x)
|
||
无电机吊舱 frame_yaw 恒 0（框架角无用），仅脱靶量生效；装电机后开启框架角项。
|
||
L1 无距离：distance_valid=False，控制律按 no_distance_action 降级（只修正方向）。
|
||
"""
|
||
hs = self._cfg.heading_servo
|
||
tel = self._latest_telemetry
|
||
frame, t_ms = tel if tel is not None else (None, 0)
|
||
|
||
beta_deg = 0.0
|
||
alpha_deg = 0.0
|
||
if frame is not None:
|
||
if hs.beta_from_frame:
|
||
beta_deg += frame.frame_yaw_deg
|
||
if hs.beta_from_off_target:
|
||
beta_deg += frame.off_target_x_deg
|
||
if hs.alpha_from_frame:
|
||
alpha_deg += frame.frame_pitch_deg
|
||
if hs.alpha_from_off_target:
|
||
alpha_deg += frame.off_target_y_deg
|
||
beta_deg *= hs.beta_sign
|
||
alpha_deg *= hs.alpha_sign
|
||
if not hs.vertical_enabled:
|
||
alpha_deg = 0.0
|
||
|
||
tracked = self._pod_locked(now_ms)
|
||
# 第二判据：LOS 运动学一致性（独立于 bit2）。
|
||
# 本吊舱无电机、相机刚性固连机体 → 机头转 Δψ 时真实目标的脱靶量必变 ≈−Δψ；
|
||
# 机头已明显转动而 β 不动 → 角度冻结/幽灵目标 → 当丢目标处理（fail-safe）。
|
||
# 仅在"吊舱自报锁定"期间喂样本；解锁即清历史，避免跨段污染。
|
||
if tracked:
|
||
status = self._los_monitor.update(
|
||
now_ms, math.degrees(own.yaw_rad), beta_deg)
|
||
self._los_consistent = status.consistent
|
||
if status.frozen:
|
||
tracked = False
|
||
if self._los_logged_ok:
|
||
self._los_logged_ok = False
|
||
logger.warning(
|
||
"[Bridge] 吊舱角度与机头转动不一致（Δψ=%.1f° Δβ=%.1f° "
|
||
"残差=%.1f°，超 %.1f°）→ 判为丢目标",
|
||
status.delta_yaw_deg, status.delta_beta_deg,
|
||
status.residual_deg, hs.los_max_residual_deg)
|
||
elif self._los_consistent and not self._los_logged_ok:
|
||
self._los_logged_ok = True
|
||
logger.info("[Bridge] 吊舱角度一致性恢复（残差 %.1f°）",
|
||
status.residual_deg)
|
||
else:
|
||
self._los_monitor.reset()
|
||
self._los_consistent = True
|
||
self._los_logged_ok = True
|
||
|
||
return ControlSnapshot(
|
||
track=TrackState(tracked=tracked),
|
||
track_time_ms=t_ms, # 无遥测(t_ms=0) → 控制律判视觉超龄 → 降级
|
||
los_beta_deg=beta_deg,
|
||
los_alpha_deg=alpha_deg,
|
||
distance_valid=False, # L1 无距离（距离源接入后改为可用，见 prompt 文档）
|
||
attitude_present=own.has_attitude,
|
||
attitude_time_ms=own.time_ms,
|
||
yaw_rad=own.yaw_rad,
|
||
)
|
||
|
||
async def _dispatch(self, out, now_ms: int) -> None:
|
||
if out.mode is ControlMode.VELOCITY_HEADING and out.valid:
|
||
await self._px4.send_velocity_heading(out, now_ms)
|
||
elif out.mode is ControlMode.BRAKE_HOVER and out.valid:
|
||
await self._px4.send_brake_hover(now_ms)
|
||
elif out.mode is ControlMode.DEGRADED_HOLD:
|
||
# L1 控制律超龄/退出会降级（valid=False）：仍补发零速刹车，
|
||
# 保住 offboard 心跳（否则 PX4 数秒内触发失联保护）。
|
||
await self._px4.send_brake_hover(now_ms)
|
||
|
||
# --- 诊断访问 -----------------------------------------------------------
|
||
@property
|
||
def latest_target(self):
|
||
return self._latest_target
|
||
|
||
@property
|
||
def latest_telemetry(self):
|
||
return self._latest_telemetry
|
||
|
||
# --- 实机工厂 -----------------------------------------------------------
|
||
@classmethod
|
||
async def connect(cls, cfg: BridgeConfig) -> "PodBridge":
|
||
"""按配置连接 PX4(MAVSDK) 与吊舱(串口)，共用单一单调时钟。"""
|
||
px4 = await Px4Gateway.connect(cfg.px4.connection_string,
|
||
now_ms_fn=_monotonic_ms)
|
||
pod = PodSerial(cfg.pod_serial.port, cfg.pod_serial.baudrate)
|
||
return cls(cfg, px4, pod, now_ms_fn=_monotonic_ms)
|