实现L1航向伺服(路线A1): 吊舱角度直入控制律 + 桥接胶水 + 干跑入口
- visual_track_control: ControlSnapshot 加可选 los_beta_deg/los_alpha_deg, 第④步角度直入优先否则回退像素路径(向后兼容); config 拆出 load_from_dict 供内嵌复用 - pod_bridge: BridgeConfig 内嵌 TrackingConfig + 新 HeadingServoConfig; bridge._build_snapshot 合成 β/α(姿态来自PX4/角度来自吊舱,信号双路适配无电机), step 按 heading_servo.enabled 切 L1/L2, DEGRADED_HOLD 补发刹车保 offboard 心跳; run() 循环频率按生效律取(修 L1 频率错配) - run_bridge.py: 闭环 CLI 入口, --dry-run(S2 只接吊舱不发offboard) 与实飞优雅停机 - 配置落 L1 首飞值(slow_approach/0.2mps、垂直压低、无电机仅用脱靶量) - 新增测试 29 条(vtc5+config9+bridgeL1 10+runner5); 全量 401 passed
This commit is contained in:
keita
@@ -26,7 +26,9 @@ from datetime import datetime, timezone
|
||||
from typing import Optional
|
||||
|
||||
from visual_track_control.staleness_checker import is_stale
|
||||
from visual_track_control.types import ControlMode
|
||||
from visual_track_control.tracking_control_law import TrackingControlLaw
|
||||
from visual_track_control.types import (ControlMode, ControlSnapshot,
|
||||
TrackState)
|
||||
|
||||
from .config import BridgeConfig
|
||||
from .guidance import GuidanceInput, GuidanceLaw
|
||||
@@ -46,12 +48,14 @@ class PodBridge:
|
||||
"""px4：Px4Gateway（或等价 mock）；pod：PodLink（PodSerial 或内存 FakePod）。"""
|
||||
|
||||
def __init__(self, cfg: BridgeConfig, px4: Px4Gateway, pod: PodLink,
|
||||
now_ms_fn=_monotonic_ms) -> None:
|
||||
now_ms_fn=_monotonic_ms, vision_target_fn=None) -> None:
|
||||
self._cfg = cfg
|
||||
self._px4 = px4
|
||||
self._pod = pod
|
||||
self._now_ms = now_ms_fn
|
||||
self._vision_target_fn = vision_target_fn # §2.3 第二判据（可选，S3+ 接入我方视觉）
|
||||
self._guidance = GuidanceLaw(cfg.guidance)
|
||||
self._tracking = TrackingControlLaw(cfg.tracking) # L1 航向伺服（方案 §0）
|
||||
self._attitude_period_ms = max(
|
||||
1, int(round(1000.0 / cfg.link.attitude_downlink_hz)))
|
||||
self._last_attitude_ms: Optional[int] = None
|
||||
@@ -80,6 +84,7 @@ class PodBridge:
|
||||
await self._px4.stop_telemetry()
|
||||
self._pod.close()
|
||||
self._guidance.reset()
|
||||
self._tracking.reset()
|
||||
logger.info("[Bridge] 已停止")
|
||||
|
||||
# --- 主循环 -------------------------------------------------------------
|
||||
@@ -91,7 +96,7 @@ class PodBridge:
|
||||
零速刹车保住 offboard 心跳——否则 PX4 数秒内触发 offboard 失联保护。
|
||||
CancelledError 继承 BaseException，不在此拦截。
|
||||
"""
|
||||
period_s = 1.0 / self._cfg.guidance.frequency_hz
|
||||
period_s = 1.0 / self._active_frequency_hz()
|
||||
while not stop_event.is_set():
|
||||
try:
|
||||
await self.step(self._now_ms())
|
||||
@@ -107,14 +112,28 @@ class PodBridge:
|
||||
pass # 补发失败不掩盖原异常，下周期重试
|
||||
await asyncio.sleep(period_s)
|
||||
|
||||
def _active_frequency_hz(self) -> float:
|
||||
"""当前生效控制律的循环频率：L1 用 tracking.control，L2 用 guidance。"""
|
||||
if self._cfg.heading_servo.enabled:
|
||||
return self._cfg.tracking.control.frequency_hz
|
||||
return self._cfg.guidance.frequency_hz
|
||||
|
||||
async def step(self, now_ms: int):
|
||||
"""单控制周期（显式传入时钟，便于确定性单测）。返回 ControlOutput。"""
|
||||
"""单控制周期（显式传入时钟，便于确定性单测）。返回 ControlOutput。
|
||||
|
||||
按 heading_servo.enabled 在 L1（TrackingControlLaw，吊舱角度直入）与
|
||||
L2（GuidanceLaw，表4 地理引导）间切换（方案 §7）。
|
||||
"""
|
||||
self._ingest_pod_frames(now_ms)
|
||||
|
||||
own = self._px4.own_state()
|
||||
self._downlink_attitude(own, now_ms)
|
||||
|
||||
out = self._guidance.update(self._build_guidance_input(own, now_ms), now_ms)
|
||||
if self._cfg.heading_servo.enabled:
|
||||
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)
|
||||
return out
|
||||
|
||||
@@ -191,11 +210,61 @@ class PodBridge:
|
||||
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)
|
||||
# §2.3：require_vision 时需我方视觉作第二判据（bit2 粘滞兜底）。未接入视觉源
|
||||
# 时 fail-safe 为 False（无法确认 → 宁可刹车）。
|
||||
if tracked and hs.require_vision:
|
||||
tracked = self._vision_target_fn() if self._vision_target_fn else False
|
||||
|
||||
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 无距离（L2 换 stereo_ranger.forward_distance_m）
|
||||
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
|
||||
|
||||
Reference in New Issue
Block a user