From 5a944af4f561ee2c463dc435adefa4cc76e918a1 Mon Sep 17 00:00:00 2001 From: keita <16924591+keita_sakura@user.noreply.gitee.com> Date: Fri, 9 Oct 2026 12:52:14 +0800 Subject: [PATCH] =?UTF-8?q?=E5=AE=9E=E7=8E=B0L1=E8=88=AA=E5=90=91=E4=BC=BA?= =?UTF-8?q?=E6=9C=8D(=E8=B7=AF=E7=BA=BFA1):=20=E5=90=8A=E8=88=B1=E8=A7=92?= =?UTF-8?q?=E5=BA=A6=E7=9B=B4=E5=85=A5=E6=8E=A7=E5=88=B6=E5=BE=8B=20+=20?= =?UTF-8?q?=E6=A1=A5=E6=8E=A5=E8=83=B6=E6=B0=B4=20+=20=E5=B9=B2=E8=B7=91?= =?UTF-8?q?=E5=85=A5=E5=8F=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 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 --- docs/L1航向伺服实现方案.md | 73 ++++-- utils/config/pod_bridge.json | 57 +++++ utils/pod_bridge/bridge.py | 79 ++++++- utils/pod_bridge/config.py | 68 ++++++ utils/pod_bridge/run_bridge.py | 209 ++++++++++++++++++ utils/tests/pod_bridge/bridge_test.py | 132 +++++++++++ utils/tests/pod_bridge/config_test.py | 61 ++++- utils/tests/pod_bridge/run_bridge_test.py | 82 +++++++ .../tracking_control_law_test.py | 50 +++++ utils/visual_track_control/config.py | 30 ++- .../tracking_control_law.py | 23 +- utils/visual_track_control/types.py | 6 + 12 files changed, 827 insertions(+), 43 deletions(-) create mode 100644 utils/pod_bridge/run_bridge.py create mode 100644 utils/tests/pod_bridge/run_bridge_test.py diff --git a/docs/L1航向伺服实现方案.md b/docs/L1航向伺服实现方案.md index ec3edd1..758cb8f 100644 --- a/docs/L1航向伺服实现方案.md +++ b/docs/L1航向伺服实现方案.md @@ -2,7 +2,7 @@ > 目标：**先把"只修正飞行方向、不需要距离"的部分落地**，速度-距离闭环（L2）滞后。 > 基于结论：本吊舱无激光 → 表4 永不出，且不允许二次开发（部署文档 §10.0 / §10）。 -> 日期：2026-10-09　状态：**方案待确认，未动代码** +> 日期：2026-10-09　状态：**S0 已实现（路线 A1 + L1 胶水层），全量 396 passed；符号待台架 S1 标定** --- @@ -68,6 +68,16 @@ distance_available = snapshot.distance_valid and distance>0 and 未超龄 **额外好处**：走角度输入就**与视场 FOV 无关**——吊舱会变焦，若走像素换算 （`PixelToAngle` 反推），变焦后 FOV 变化会直接失真。这是不用像素路径的硬理由。 +> ⚠ **本版吊舱未装电机（决策 2，2026-10-09）**：云台刚性固连机体、朝前不动， +> `frame_yaw_deg` 恒 0（"框架角无用"），锁定后也**不会**自 servo 回中，故脱靶量 +> 长期**不**≈0。此时 `off_target_x_deg` 恰好 = 目标相对机头的视线角 β，是**唯一** +> 有效信号。因此合成做成**配置双路**（`beta_from_frame` / `beta_from_off_target`）： +> - 现在（无电机）：`beta_from_frame=false`、`beta_from_off_target=true` → β = off_target_x +> - 将来（装电机）：两者皆 `true` → β = frame_yaw + off_target_x（即上文 §2.1） +> +> "装不装电机"只是翻配置，架构（姿态来自 PX4、角度来自吊舱）不变。**装电机后 §2.1 +> 的"脱靶量长期≈0、必须靠框架角"警告才重新生效。** + ### 2.2 如何接入 TrackingControlLaw（三选一，需拍板） 控制律第 ④ 步现在是**硬编码从像素算角度**，没有"直接喂角度"的入口： @@ -105,6 +115,8 @@ L1 的 `tracked` 判据按阶段递进： ## 3. 改动清单（文件级） +> 计划视图。**实际落地状态与逐文件勾选见 §9**（含计划外新增的 `config.py:load_from_dict`）。 + | # | 文件 | 改动 | |---|---|---| | 1 | `utils/visual_track_control/types.py` | `ControlSnapshot` 加 `los_beta_deg` / `los_alpha_deg: Optional[float] = None`（路线 A1） | @@ -122,27 +134,31 @@ L1 的 `tracked` 判据按阶段递进： ## 4. 配置外置（要点，非最终字段名） ```jsonc -// utils/config/pod_bridge.json —— L1 段 +// utils/config/pod_bridge.json —— L1 段（已实现，字段名与 HeadingServoConfig 一致） "heading_servo": { - "enabled": true, - "beta_sign": 1, // 台架标定后填 ±1（框架角 + 脱靶量合成的符号） + "enabled": true, // true=走 L1(TrackingControlLaw)，false=L2(GuidanceLaw) + "beta_sign": 1, // 台架 S1 标定后填 ±1（符号错了会反向转） "alpha_sign": 1, - "use_off_target": true, // β 是否叠加脱靶量（false = 只用框架角） - "vertical_enabled": false, // L1 先禁垂直，避免撞地；vz=0 - "require_vision": false // true = 需我方视觉作为第二判据（S3 起开） + "beta_from_frame": false, // 无电机 → 框架角无用；装电机后置 true + "beta_from_off_target": true,// 无电机 → 脱靶量即 β 主信号 + "alpha_from_frame": false, + "alpha_from_off_target": true, + "vertical_enabled": true, // 决策3：无人机 L1 开垂直（vz 由 tracking.vertical 压低） + "require_vision": false // true = 需我方视觉第二判据（S3 起开，§2.3） } ``` ```jsonc -// visual_track_control 侧（沿用既有结构，只改值） -"heading": { "mode": "rate", "gain": ..., "yaw_rate_limit_dps": 20.0, "yaw_slew_limit_dps2": 40.0 }, -"distance": { "no_distance_action": "hold", // 首次上机 = hold - "no_distance_approach_limit_mps": 0.3 }, -"vertical": { "vz_gain_mps_per_deg": 0.0 } // L1 禁用 +// tracking 段（内嵌整份 visual_track_control 配置，各分组须齐全；L1 首飞值） +"heading": { "mode": "rate", "gain": 1.0, "yaw_rate_limit_dps": 20.0, "yaw_slew_limit_dps2": 40.0 }, +"distance": { "no_distance_action": "slow_approach", // 决策3：低速接近（非 hold） + "no_distance_approach_limit_mps": 0.2 }, // 极低限速起步 +"vertical": { "vz_gain_mps_per_deg": 0.05, "vz_limit_mps": 0.5 } // 开垂直但压低 ``` -> 首次上机一律 `no_distance_action = "hold"`：**只转向、不前近**。 -> 航向验证通过后再切 `slow_approach` 并把限速压到 0.3 m/s 起步。 +> 决策 3：首次上机 `no_distance_action = "slow_approach"`、限速 **0.2 m/s**（很低）， +> 而非原推荐的 `hold`。安全不受影响——**S2 干跑不下发 offboard**，符号先验证过再通电。 +> `beta_sign`/`alpha_sign`/增益均为占位，须 S1/S2 标定后回填，标定前不得实飞。 --- @@ -185,8 +201,29 @@ L1 落地时就留好口子，将来 L2 只是"换个输入源"，不动结构 --- -## 8. 待你拍板 +## 8. 已拍板（2026-10-09） -1. **接入路线**：A1（改 `ControlSnapshot` 加可选角度字段，推荐）/ A2（pod_bridge 侧薄封装）/ B（像素反推，不推荐）； -2. **目标类型**：人车（地面）/ 无人机（空中）—— 影响 L2 的几何反推法能否用，L1 不受影响； -3. **首次上机无距离动作**：`hold`（只转不前，我建议）/ `slow_approach`（0.3 m/s）。 +1. **接入路线 = A1**：`ControlSnapshot` 加可选 `los_beta_deg`/`los_alpha_deg`，第④步角度直入优先、否则回退像素路径。已实现（`types.py` / `tracking_control_law.py`，补 5 条兼容性测试）。 +2. **目标类型 = 无人机（空中）**：影响 L2 几何反推法，L1 无关。 +3. **首次上机 = `slow_approach` + 极低限速 0.2 m/s**（非 `hold`）；**L1 开垂直通道**，但 `vz` 限速压低（`vz_limit_mps=0.5`、`vz_gain=0.05`）防撞。 + +补充决策（BridgeConfig 结构）：**BridgeConfig 直接内嵌 `TrackingConfig` 字段 + `heading_servo` 段**；运行时按 `heading_servo.enabled` 在 L1(`TrackingControlLaw`)/L2(`GuidanceLaw`) 间切换，L2 接口天然保留（对齐 §7）。姿态来自 PX4、β/α 来自吊舱（决策 2 见 §2.1 无电机说明）。 + +--- + +## 9. 实现状态（S0 已完成） + +| # | 文件 | 状态 | +|---|---|---| +| 1 | `visual_track_control/types.py` | ✅ `ControlSnapshot` 加 `los_beta_deg`/`los_alpha_deg: Optional[float]=None` | +| 2 | `visual_track_control/tracking_control_law.py` | ✅ 第④步角度直入优先，否则回退像素 | +| 3 | `visual_track_control/config.py` | ✅ 拆出 `load_from_dict()` 供 bridge 内嵌复用 | +| 4 | `pod_bridge/config.py` | ✅ 加 `HeadingServoConfig` + 内嵌 `TrackingConfig`，严格校验符号(±1) | +| 5 | `pod_bridge/bridge.py` | ✅ `_build_snapshot()`；`step()` 按 enabled 切 L1/L2；`_dispatch` 对 DEGRADED_HOLD 补发刹车保心跳；`vision_target_fn` 注入点(§2.3 第二判据) | +| 6 | `config/pod_bridge.json` | ✅ 落 L1 首飞值（slow_approach/0.2、垂直压低、无电机 → 仅脱靶量） | +| 7 | `tests/` | ✅ 新增 24 条（vtc 5 + config 9 + bridge L1 10）；全量 **396 passed** | +| 8 | `pod_bridge/run_bridge.py` | ✅ 闭环 CLI 入口：`--dry-run`(S2 只接吊舱/不发 offboard/打印 β→yaw_rate) 与实飞(S3+，`PodBridge.connect`+信号优雅停机)；配套 `run_bridge_test.py` 5 条，全量 **401 passed** | +| 9 | `pod_bridge/bridge.py:run()` | ✅ 循环周期改按生效律取频率（L1=tracking.control，L2=guidance），修 L1 live 频率错配 | + +**待办（非代码，台架）**：S1 标定 `beta_sign`/`alpha_sign` 与框架角定义；S2 干跑 +`run_bridge.py --dry-run` 验符号（目标偏左→yaw_rate 负、居中→趋 0），符号错就翻配置里的 sign。 diff --git a/utils/config/pod_bridge.json b/utils/config/pod_bridge.json index 6244698..8f24841 100644 --- a/utils/config/pod_bridge.json +++ b/utils/config/pod_bridge.json @@ -1,4 +1,6 @@ { + "_comment": "吊舱↔PX4 桥接配置。heading_servo.enabled=true → 走 L1 航向伺服(TrackingControlLaw)；tracking 段为 L1 控制律参数(与 visual_track_control 同结构，各分组须齐全)。全部数值为台架标定前占位，符号(beta_sign/alpha_sign)与增益须经 S1/S2 标定后回填，标定前不得实飞。", + "pod_serial": { "port": "", "baudrate": 115200 @@ -26,5 +28,60 @@ "max_yaw_rate_dps": 30.0, "yaw_kp": 1.0, "yaw_slew_dps2": 60.0 + }, + + "heading_servo": { + "enabled": true, + "beta_sign": 1, + "alpha_sign": 1, + "beta_from_frame": false, + "beta_from_off_target": true, + "alpha_from_frame": false, + "alpha_from_off_target": true, + "vertical_enabled": true, + "require_vision": false + }, + + "tracking": { + "control": { + "frequency_hz": 20.0, + "visual_stale_ms": 200, + "distance_stale_ms": 500, + "attitude_stale_ms": 300 + }, + "camera": { + "image_width": 1920, + "image_height": 1080, + "fov_h_deg": 60.0, + "fov_v_deg": 35.0 + }, + "heading": { + "mode": "rate", + "gain": 1.0, + "yaw_rate_limit_dps": 20.0, + "yaw_slew_limit_dps2": 40.0 + }, + "vertical": { + "vz_gain_mps_per_deg": 0.05, + "vz_limit_mps": 0.5 + }, + "distance": { + "d_exp_m": 10.0, + "kp": 0.5, + "ki": 0.0, + "kd": 0.0, + "integral_limit": 0.0, + "derivative_filter_coef": 0.0, + "approach_velocity_limit_mps": 0.5, + "retreat_velocity_limit_mps": 0.3, + "no_distance_action": "slow_approach", + "no_distance_approach_limit_mps": 0.2, + "no_distance_exit_grace_ms": 3000 + }, + "accel_limit": { + "ax_mps2": 1.0, + "ay_mps2": 1.0, + "az_mps2": 0.5 + } } } diff --git a/utils/pod_bridge/bridge.py b/utils/pod_bridge/bridge.py index 3c3fbe8..751e5af 100644 --- a/utils/pod_bridge/bridge.py +++ b/utils/pod_bridge/bridge.py @@ -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 diff --git a/utils/pod_bridge/config.py b/utils/pod_bridge/config.py index 8126b39..01b54cc 100644 --- a/utils/pod_bridge/config.py +++ b/utils/pod_bridge/config.py @@ -16,6 +16,9 @@ import json from dataclasses import dataclass, field from typing import Any +from visual_track_control.config import TrackingConfig +from visual_track_control.config import load_from_dict as _load_tracking_dict + from .guidance import GuidanceConfig @@ -46,12 +49,34 @@ class LinkConfig: # 激光无效即不解算，以此兜底；无激光的吊舱置 False） +@dataclass +class HeadingServoConfig: + """L1 航向伺服（方案 §2/§4）：把吊舱表2-2 角度喂进 TrackingControlLaw。 + + 信号合成（§2.1）：β = beta_sign·(beta_from_frame·frame_yaw + beta_from_off_target·off_target_x)， + α 同理。无电机吊舱 frame_yaw 恒 0（"框架角无用"）→ 仅用脱靶量；装电机后两者叠加。 + 符号（±1）台架 S1 标定后填，配置化而非硬编码，防接反反向转。 + """ + + enabled: bool = False # True = bridge 走 L1(TrackingControlLaw)，False = L2(GuidanceLaw) + beta_sign: int = 1 # 水平合成符号（台架标定 ±1） + alpha_sign: int = 1 # 垂直合成符号 + beta_from_frame: bool = False # β 是否叠加框架角 frame_yaw_deg（装电机后开） + beta_from_off_target: bool = True # β 是否叠加脱靶量 off_target_x_deg + alpha_from_frame: bool = False # α 是否叠加框架角 frame_pitch_deg + alpha_from_off_target: bool = True # α 是否叠加脱靶量 off_target_y_deg + vertical_enabled: bool = True # 无人机 L1 开垂直（决策3）；false → α 强制 0 + require_vision: bool = False # true = 需我方视觉作第二判据（S3 起，§2.3） + + @dataclass class BridgeConfig: pod_serial: PodSerialConfig = field(default_factory=PodSerialConfig) px4: Px4Config = field(default_factory=Px4Config) link: LinkConfig = field(default_factory=LinkConfig) guidance: GuidanceConfig = field(default_factory=GuidanceConfig) + heading_servo: HeadingServoConfig = field(default_factory=HeadingServoConfig) + tracking: TrackingConfig = field(default_factory=TrackingConfig) # --------------------------------------------------------------------------- @@ -228,6 +253,47 @@ def _load_guidance(root: dict) -> GuidanceConfig: return g +def _get_sign(section: dict, section_name: str, key: str, default: int) -> int: + """符号位：仅允许 -1 / +1（配置化防接反，方案 §2.1/§6）。""" + if key not in section: + return int(default) + value = section[key] + if not _is_integer(value) or value not in (-1, 1): + raise ValueError(f"配置非法(仅允许 -1 或 1): {section_name}.{key}") + return int(value) + + +def _load_heading_servo(root: dict) -> HeadingServoConfig: + s = _require_section(root, "heading_servo") + allowed = {f.name for f in dataclasses.fields(HeadingServoConfig)} + _reject_unknown(s, allowed, "heading_servo") + c = HeadingServoConfig() + c.enabled = _get_bool(s, "heading_servo", "enabled", c.enabled) + c.beta_sign = _get_sign(s, "heading_servo", "beta_sign", c.beta_sign) + c.alpha_sign = _get_sign(s, "heading_servo", "alpha_sign", c.alpha_sign) + c.beta_from_frame = _get_bool( + s, "heading_servo", "beta_from_frame", c.beta_from_frame) + c.beta_from_off_target = _get_bool( + s, "heading_servo", "beta_from_off_target", c.beta_from_off_target) + c.alpha_from_frame = _get_bool( + s, "heading_servo", "alpha_from_frame", c.alpha_from_frame) + c.alpha_from_off_target = _get_bool( + s, "heading_servo", "alpha_from_off_target", c.alpha_from_off_target) + c.vertical_enabled = _get_bool( + s, "heading_servo", "vertical_enabled", c.vertical_enabled) + c.require_vision = _get_bool( + s, "heading_servo", "require_vision", c.require_vision) + return c + + +def _load_tracking(root: dict) -> TrackingConfig: + """L1 控制律配置：缺省 → TrackingConfig() 默认；提供 → 按 vtc 严格解析（各分组齐全）。""" + s = _require_section(root, "tracking") + if not s: + return TrackingConfig() + return _load_tracking_dict(s) + + def load_from_json(path: str) -> BridgeConfig: """从 JSON 文件一次性加载桥接配置（启动期调用，调参重启生效）。""" try: @@ -246,4 +312,6 @@ def load_from_json(path: str) -> BridgeConfig: cfg.px4 = _load_px4(root) cfg.link = _load_link(root) cfg.guidance = _load_guidance(root) + cfg.heading_servo = _load_heading_servo(root) + cfg.tracking = _load_tracking(root) return cfg diff --git a/utils/pod_bridge/run_bridge.py b/utils/pod_bridge/run_bridge.py new file mode 100644 index 0000000..f494dff --- /dev/null +++ b/utils/pod_bridge/run_bridge.py @@ -0,0 +1,209 @@ +"""run_bridge.py —— 吊舱↔PX4 桥接闭环可执行入口（L1 航向伺服 / L2 地理引导）。 + +把 PodBridge 库串成能跑的进程：读配置 → 连接吊舱串口(+PX4) → 按控制频率闭环 → +优雅停机（先零速刹车再拆链路）。控制律选择由配置 heading_servo.enabled 决定 +（true=L1 TrackingControlLaw 角度伺服；false=L2 GuidanceLaw 地理引导）。 + +两种模式： + * --dry-run（S2 台架干跑）：**只接吊舱、不接飞控、不下发 offboard**。PX4 侧用 + 合成姿态桩（yaw=0、恒新鲜），每周期打印 mode / yaw_rate / vx_body / vz 与吊舱 + 脱靶量、框架角，用于验证 β→yaw_rate 的符号与量级。**电机不转、飞机不动。** + * 实飞（S3+，无 --dry-run）：PodBridge.connect() 连真 PX4(MAVSDK) + 真吊舱串口， + start() 起 offboard，run() 闭环，SIGINT/SIGTERM 或 --duration 到点后安全停机。 + ⚠ 实飞前必须先在 --dry-run 下确认符号正确；不装桨 / 空旷场地 / RC 随时可切手动。 + +用法： + # S2 干跑（先在 utils/config/pod_bridge.json 填 pod_serial.port，或用 --port 覆盖） + python3 utils/pod_bridge/run_bridge.py --config utils/config/pod_bridge.json \\ + --port /dev/ttyAMA0 --dry-run + python3 utils/pod_bridge/run_bridge.py --dry-run --duration 30 # 定时结束 + + # S3+ 实飞（连真 PX4；连接串取配置 px4.connection_string） + python3 utils/pod_bridge/run_bridge.py --config utils/config/pod_bridge.json +""" + +import argparse +import asyncio +import logging +import signal +import sys +import time +from pathlib import Path +from typing import Optional + +sys.path.insert(0, str(Path(__file__).resolve().parents[1])) # utils/ 入 sys.path + +from pod_bridge.bridge import PodBridge, _monotonic_ms # noqa: E402 +from pod_bridge.config import load_from_json # noqa: E402 +from pod_bridge.pod_serial import PodSerial # noqa: E402 +from pod_bridge.px4_gateway import OwnState # noqa: E402 + +logger = logging.getLogger("run_bridge") + +_DEFAULT_CONFIG = str(Path(__file__).resolve().parents[1] / "config" / "pod_bridge.json") + + +class DryRunPx4: + """S2 干跑用的 PX4 桩：合成新鲜姿态，offboard 指令一律不下发（仅计数）。 + + 接口对齐 Px4Gateway，故可直接注入 PodBridge。own_state() 每次调用都用当前 + 单调时钟打戳，保证控制律第①步姿态超龄校验恒通过（台架无真飞控也能跑闭环）。 + """ + + def __init__(self, now_ms_fn=_monotonic_ms, yaw_rad: float = 0.0) -> None: + self._now = now_ms_fn + self._yaw = yaw_rad + self.velocity_calls = 0 + self.brake_calls = 0 + + def own_state(self) -> OwnState: + t = self._now() + return OwnState(has_attitude=True, has_position=True, position_valid=True, + fix_type=3, num_satellites=12, yaw_rad=self._yaw, + time_ms=t, position_time_ms=t) + + async def start_telemetry(self) -> None: + pass + + async def stop_telemetry(self) -> None: + pass + + async def start_offboard(self) -> bool: + return True + + async def stop_offboard(self) -> None: + pass + + async def send_velocity_heading(self, out, time_boot_ms=None) -> bool: + self.velocity_calls += 1 # 干跑：只计数，绝不发 offboard + return True + + async def send_brake_hover(self, time_boot_ms=None) -> bool: + self.brake_calls += 1 + return True + + +def _format_status(now_s: float, out, tel_pair, locked: bool) -> str: + """把控制输出 + 吊舱源角度拼成一行（S2 符号/量级判读用）。""" + if tel_pair is not None: + frame, _ = tel_pair + off = f"off=({frame.off_target_x_deg:+.2f},{frame.off_target_y_deg:+.2f})°" + fyaw = f"frame_yaw={frame.frame_yaw_deg:+.2f}°" + lock = f"locked={int(frame.is_target_locked)}" + else: + off, fyaw, lock = "off=(无遥测)", "frame_yaw=-", "locked=-" + return (f"[dry] t={now_s:6.1f}s {out.mode.name:<16} " + f"yaw_rate={out.yaw_rate_dps:+7.2f}dps vx_body={out.vx_body_mps:+.2f} " + f"vz={out.vz_mps:+.2f} | {off} {fyaw} {lock}") + + +async def _run_dry(cfg, args) -> int: + port = args.port or cfg.pod_serial.port + if not port: + logger.error("干跑需要吊舱串口：在配置填 pod_serial.port 或用 --port 指定") + return 2 + pod = PodSerial(port, cfg.pod_serial.baudrate) + px4 = DryRunPx4(yaw_rad=0.0) + bridge = PodBridge(cfg, px4, pod, now_ms_fn=_monotonic_ms) + + period_s = 1.0 / bridge._active_frequency_hz() + print_period_s = 1.0 / args.print_hz if args.print_hz > 0 else 0.0 + law = "L1 航向伺服(TrackingControlLaw)" if cfg.heading_servo.enabled \ + else "L2 地理引导(GuidanceLaw)" + logger.info("[干跑] %s @ %.1fHz | 吊舱 %s | offboard 不下发（飞机不会动）", + law, 1.0 / period_s, port) + + t0 = time.monotonic() + last_print = -1.0 + failures = 0 + try: + while True: + now_s = time.monotonic() - t0 + if args.duration > 0 and now_s >= args.duration: + break + now_ms = _monotonic_ms() + try: + out = await bridge.step(now_ms) + failures = 0 + if now_s - last_print >= print_period_s: + last_print = now_s + print(_format_status(now_s, out, bridge.latest_telemetry, + bridge._pod_locked(now_ms))) + except OSError as exc: + failures += 1 + if failures == 1 or failures % 100 == 0: + logger.warning("[干跑] 吊舱串口异常(第 %d 次): %s", failures, exc) + await asyncio.sleep(period_s) + except KeyboardInterrupt: + pass + finally: + try: + pod.close() + except OSError: + pass + logger.info("[干跑] 结束：velocity 拍 %d，brake 拍 %d", + px4.velocity_calls, px4.brake_calls) + return 0 + + +async def _run_live(cfg, args) -> int: + bridge = await PodBridge.connect(cfg) # 连真 PX4(MAVSDK) + 真吊舱串口 + stop = asyncio.Event() + + loop = asyncio.get_running_loop() + for sig in (signal.SIGINT, signal.SIGTERM): + try: + loop.add_signal_handler(sig, stop.set) + except (NotImplementedError, RuntimeError): + pass # 平台不支持（如 Windows）→ 靠 KeyboardInterrupt 兜底 + + if not await bridge.start(): + logger.error("offboard 启动失败，未进入闭环") + await bridge.stop() + return 1 + logger.info("[实飞] 闭环运行中；Ctrl+C 或 --duration 到点后安全停机") + runner = asyncio.ensure_future(bridge.run(stop)) + try: + if args.duration > 0: + await asyncio.sleep(args.duration) + stop.set() + await runner + except KeyboardInterrupt: + stop.set() + runner.cancel() + finally: + stop.set() + await bridge.stop() # 先零速刹车，再拆 offboard/遥测/串口 + return 0 + + +async def _amain(args) -> int: + cfg = load_from_json(args.config) + return await (_run_dry(cfg, args) if args.dry_run else _run_live(cfg, args)) + + +def main(argv: Optional[list] = None) -> int: + ap = argparse.ArgumentParser(description="吊舱↔PX4 桥接闭环入口（L1/L2）") + ap.add_argument("--config", default=_DEFAULT_CONFIG, help="桥接配置 JSON 路径") + ap.add_argument("--port", help="覆盖配置里的吊舱串口设备号（如 /dev/ttyAMA0）") + ap.add_argument("--dry-run", action="store_true", + help="S2 干跑：只接吊舱、不接飞控、不下发 offboard，仅打印") + ap.add_argument("--duration", type=float, default=0.0, + help="运行时长(秒)；0=一直跑到 Ctrl+C") + ap.add_argument("--print-hz", type=float, default=5.0, + help="干跑打印频率(秒级节流)；0=每拍都打") + ap.add_argument("--verbose", action="store_true", help="打印 DEBUG 日志") + args = ap.parse_args(argv) + + logging.basicConfig( + level=logging.DEBUG if args.verbose else logging.INFO, + format="%(asctime)s %(levelname)s %(message)s", datefmt="%H:%M:%S") + + try: + return asyncio.run(_amain(args)) + except KeyboardInterrupt: + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/utils/tests/pod_bridge/bridge_test.py b/utils/tests/pod_bridge/bridge_test.py index 9089406..9775db4 100644 --- a/utils/tests/pod_bridge/bridge_test.py +++ b/utils/tests/pod_bridge/bridge_test.py @@ -9,6 +9,8 @@ import asyncio import time from dataclasses import replace +import pytest + from pod_bridge.bridge import PodBridge from pod_bridge.config import BridgeConfig, load_from_json from pod_bridge.protocol import (ControlCode, ControlCommand, TargetGeo, @@ -391,3 +393,133 @@ def test_run_recovers_after_transient_failure(): assert len(px4.brake_calls) >= 3 # 3 次异常各补一发刹车 assert len(px4.velocity_calls) >= 1 # 恢复后进入正常引导 assert b._step_failures == 0 # 计数已复位 + + +# --- L1 航向伺服（方案 §2，heading_servo.enabled=True）----------------------- +def _l1_config(**hs_overrides): + """L1 配置：控制律限幅近似透明，前向置 0，隔离验证 β→yaw_rate / α→vz 数学。""" + cfg = BridgeConfig() + cfg.heading_servo.enabled = True + for k, v in hs_overrides.items(): + setattr(cfg.heading_servo, k, v) + t = cfg.tracking + t.heading.gain = 1.0 + t.heading.yaw_rate_limit_dps = 30.0 + t.heading.yaw_slew_limit_dps2 = 100000.0 + t.vertical.vz_gain_mps_per_deg = 0.1 + t.vertical.vz_limit_mps = 2.0 + t.accel_limit.ax_mps2 = 1000.0 + t.accel_limit.ay_mps2 = 1000.0 + t.accel_limit.az_mps2 = 1000.0 + t.distance.no_distance_approach_limit_mps = 0.0 # L1 前向隔离为 0 + return cfg + + +def _locked_off_target(x=0.0, y=0.0, frame_yaw=0.0, frame_pitch=0.0, laser=50.0): + """bit2=锁定 + 有效激光，带指定脱靶量/框架角的遥测帧。""" + return Telemetry(status1=0b00000100, laser_range_m=laser, + off_target_x_deg=x, off_target_y_deg=y, + frame_yaw_deg=frame_yaw, frame_pitch_deg=frame_pitch) + + +def test_l1_off_target_beta_drives_yaw_rate(): + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + pod.queue(_locked_off_target(x=10.0)) # 目标偏右 10° → 右转正 + b = _bridge(px4, pod, _l1_config()) + out = asyncio.run(b.step(1000)) + assert out.mode.name == "VELOCITY_HEADING" + assert out.yaw_rate_dps == pytest.approx(10.0, abs=1e-9) + assert len(px4.velocity_calls) == 1 + + +def test_l1_beta_sign_flips_direction(): + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + pod.queue(_locked_off_target(x=10.0)) + b = _bridge(px4, pod, _l1_config(beta_sign=-1)) # 台架标定若接反，配置翻符号 + out = asyncio.run(b.step(1000)) + assert out.yaw_rate_dps == pytest.approx(-10.0, abs=1e-9) + + +def test_l1_frame_angle_ignored_when_disabled(): + # 无电机：beta_from_frame=False → frame_yaw 不贡献，仅脱靶量生效 + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + pod.queue(_locked_off_target(x=5.0, frame_yaw=99.0)) + b = _bridge(px4, pod, _l1_config()) + out = asyncio.run(b.step(1000)) + assert out.yaw_rate_dps == pytest.approx(5.0, abs=1e-9) + + +def test_l1_frame_angle_added_when_enabled(): + # 装电机后：beta_from_frame=True → β = frame_yaw + off_target_x + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + pod.queue(_locked_off_target(x=5.0, frame_yaw=3.0)) + b = _bridge(px4, pod, _l1_config(beta_from_frame=True)) + out = asyncio.run(b.step(1000)) + assert out.yaw_rate_dps == pytest.approx(8.0, abs=1e-9) + + +def test_l1_vertical_enabled_drives_vz(): + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + pod.queue(_locked_off_target(y=20.0)) # α=20° × gain 0.1 = 2.0（上限 2.0） + b = _bridge(px4, pod, _l1_config()) + out = asyncio.run(b.step(1000)) + assert out.vz_mps == pytest.approx(2.0, abs=1e-9) + + +def test_l1_vertical_disabled_zeroes_vz(): + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + pod.queue(_locked_off_target(y=20.0)) + b = _bridge(px4, pod, _l1_config(vertical_enabled=False)) + out = asyncio.run(b.step(1000)) + assert out.vz_mps == 0.0 + + +def test_l1_not_locked_brakes(): + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + pod.queue(Telemetry(status1=0, off_target_x_deg=10.0)) # 搜索态 + b = _bridge(px4, pod, _l1_config()) + out = asyncio.run(b.step(1000)) + assert out.mode.name == "BRAKE_HOVER" + assert px4.velocity_calls == [] + assert len(px4.brake_calls) == 1 + + +def test_l1_no_telemetry_degrades_and_keeps_heartbeat(): + # 无表2-2 → track_time_ms=0 → 控制律判视觉超龄 → DEGRADED_HOLD；仍补发刹车保心跳 + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + b = _bridge(px4, pod, _l1_config()) + out = asyncio.run(b.step(1000)) + assert out.mode.name == "DEGRADED_HOLD" + assert px4.velocity_calls == [] + assert len(px4.brake_calls) == 1 + + +def test_l1_require_vision_blocks_without_source(): + # §2.3 第二判据：require_vision=True 但未接视觉源 → fail-safe 视为无目标 → 刹车 + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + pod.queue(_locked_off_target(x=10.0)) + b = _bridge(px4, pod, _l1_config(require_vision=True)) + out = asyncio.run(b.step(1000)) + assert out.mode.name == "BRAKE_HOVER" + assert px4.velocity_calls == [] + + +def test_l1_require_vision_passes_with_source(): + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + pod.queue(_locked_off_target(x=10.0)) + b = PodBridge(_l1_config(require_vision=True), px4, pod, + now_ms_fn=lambda: 0, vision_target_fn=lambda: True) + out = asyncio.run(b.step(1000)) + assert out.mode.name == "VELOCITY_HEADING" + assert out.yaw_rate_dps == pytest.approx(10.0, abs=1e-9) + diff --git a/utils/tests/pod_bridge/config_test.py b/utils/tests/pod_bridge/config_test.py index 13c2e25..7f6c01c 100644 --- a/utils/tests/pod_bridge/config_test.py +++ b/utils/tests/pod_bridge/config_test.py @@ -5,8 +5,9 @@ from pathlib import Path import pytest -from pod_bridge.config import (BridgeConfig, GuidanceConfig, PodSerialConfig, - load_from_json) +from pod_bridge.config import (BridgeConfig, GuidanceConfig, HeadingServoConfig, + PodSerialConfig, load_from_json) +from visual_track_control.config import NoDistanceAction, TrackingConfig _DEFAULT_CONFIG = Path(__file__).resolve().parents[2] / "config" / "pod_bridge.json" @@ -113,3 +114,59 @@ def test_out_of_range_raises(tmp_path, obj, field): def test_pod_serial_default_port_is_placeholder(): assert PodSerialConfig().port == "" + + +# --- L1 航向伺服配置（方案 §4，决策 1/2/3）---------------------------------- +def test_empty_json_heading_servo_and_tracking_default(tmp_path): + cfg = load_from_json(_write(tmp_path, {})) + assert cfg.heading_servo == HeadingServoConfig() + assert cfg.heading_servo.enabled is False # 默认关，保持 L2 行为 + assert cfg.tracking == TrackingConfig() # 缺 tracking 段 → 默认 + + +def test_heading_servo_loads(): + cfg = load_from_json(str(_DEFAULT_CONFIG)) + hs = cfg.heading_servo + assert hs.enabled is True # 部署配置走 L1 + assert hs.beta_from_off_target is True # 无电机：脱靶量为主信号 + assert hs.beta_from_frame is False # 框架角当前无用 + assert hs.vertical_enabled is True # 决策3：无人机 L1 开垂直 + + +def test_heading_servo_partial_override(tmp_path): + cfg = load_from_json(_write(tmp_path, { + "heading_servo": {"beta_sign": -1, "beta_from_frame": True}, + })) + assert cfg.heading_servo.beta_sign == -1 + assert cfg.heading_servo.beta_from_frame is True + assert cfg.heading_servo.alpha_sign == 1 # 默认回退 + + +@pytest.mark.parametrize("obj,field", [ + ({"heading_servo": {"beta_sign": 0}}, "heading_servo.beta_sign"), # 非 ±1 + ({"heading_servo": {"alpha_sign": 2}}, "heading_servo.alpha_sign"), + ({"heading_servo": {"beta_sign": 1.0}}, "heading_servo.beta_sign"), # 非整数 + ({"heading_servo": {"unknown": 1}}, "未知字段"), +]) +def test_heading_servo_bad_sign_rejected(tmp_path, obj, field): + with pytest.raises(ValueError, match=field.replace(".", r"\.")): + load_from_json(_write(tmp_path, obj)) + + +def test_tracking_incomplete_section_raises(tmp_path): + # tracking 段一旦提供即按 vtc 严格解析：缺分组报错 + with pytest.raises(ValueError, match="camera"): + load_from_json(_write(tmp_path, {"tracking": { + "control": {"frequency_hz": 20.0, "visual_stale_ms": 200, + "distance_stale_ms": 500, "attitude_stale_ms": 300}, + }})) + + +def test_shipped_tracking_l1_values(): + cfg = load_from_json(str(_DEFAULT_CONFIG)) + d = cfg.tracking.distance + assert d.no_distance_action is NoDistanceAction.SLOW_APPROACH # 决策3：低速接近 + assert d.no_distance_approach_limit_mps == pytest.approx(0.2) # 极低限速 + assert cfg.tracking.vertical.vz_limit_mps == pytest.approx(0.5) # vz 压低 + assert cfg.tracking.heading.mode.value == "rate" + diff --git a/utils/tests/pod_bridge/run_bridge_test.py b/utils/tests/pod_bridge/run_bridge_test.py new file mode 100644 index 0000000..98c2970 --- /dev/null +++ b/utils/tests/pod_bridge/run_bridge_test.py @@ -0,0 +1,82 @@ +"""run_bridge_test.py —— 桥接闭环入口单元测试（干跑桩 + 打印 + 干跑循环，无硬件）。 + +重点守住 S2 干跑的安全属性：DryRunPx4 合成新鲜姿态让闭环能跑，但 send_* 只计数、 +**绝不下发 offboard**；串口未指定时报错退出而非静默。 +""" + +import asyncio +from types import SimpleNamespace + +import pytest + +from pod_bridge.config import BridgeConfig +from pod_bridge.protocol import Telemetry +from pod_bridge.run_bridge import DryRunPx4, _format_status, _run_dry + + +class FakePod: + def __init__(self, *frames): + self.inbox = list(frames) + self.sent = [] + self.closed = False + + def poll(self): + out, self.inbox = self.inbox, [] + return out + + def send(self, data): + self.sent.append(data) + + def close(self): + self.closed = True + + +# --- DryRunPx4：合成姿态 + 不发 offboard ------------------------------------ +def test_dry_run_px4_state_is_fresh(): + px4 = DryRunPx4(now_ms_fn=lambda: 12345) + st = px4.own_state() + assert st.has_attitude and st.position_valid + assert st.time_ms == 12345 and st.position_time_ms == 12345 + + +def test_dry_run_px4_never_sends_offboard(): + px4 = DryRunPx4() + assert asyncio.run(px4.start_offboard()) is True + assert asyncio.run(px4.send_velocity_heading(object())) is True + assert asyncio.run(px4.send_brake_hover()) is True + # 只计数，无任何硬件副作用 + assert px4.velocity_calls == 1 and px4.brake_calls == 1 + + +def test_format_status_shows_mode_and_angles(): + out = SimpleNamespace(mode=SimpleNamespace(name="VELOCITY_HEADING"), + yaw_rate_dps=10.0, vx_body_mps=0.2, vz_mps=0.0) + tel = (Telemetry(status1=0b00000100, off_target_x_deg=10.0, + off_target_y_deg=-3.0, frame_yaw_deg=0.0), 1000) + line = _format_status(1.0, out, tel, locked=True) + assert "VELOCITY_HEADING" in line + assert "+10.00" in line # off_target_x + assert "locked=1" in line + + +# --- 干跑循环 --------------------------------------------------------------- +def test_run_dry_completes_and_downlinks_only(monkeypatch): + # 注入假串口：不碰真硬件，验证干跑循环能跑通并正常收尾 + pod = FakePod(Telemetry(status1=0b00000100, off_target_x_deg=10.0)) + monkeypatch.setattr("pod_bridge.run_bridge.PodSerial", lambda *a, **k: pod) + cfg = BridgeConfig() + cfg.heading_servo.enabled = True + cfg.tracking.control.frequency_hz = 200.0 # 加快节拍，缩短测试 + args = SimpleNamespace(port="/dev/fake", duration=0.05, print_hz=0.0) + + rc = asyncio.run(_run_dry(cfg, args)) + assert rc == 0 + assert pod.closed # 收尾关闭串口 + + +def test_run_dry_requires_port(monkeypatch): + monkeypatch.setattr("pod_bridge.run_bridge.PodSerial", + lambda *a, **k: pytest.fail("无端口时不应打开串口")) + cfg = BridgeConfig() # pod_serial.port 默认空 + args = SimpleNamespace(port=None, duration=0.0, print_hz=5.0) + assert asyncio.run(_run_dry(cfg, args)) == 2 diff --git a/utils/tests/visual_track_control/tracking_control_law_test.py b/utils/tests/visual_track_control/tracking_control_law_test.py index a4b1531..3febf65 100644 --- a/utils/tests/visual_track_control/tracking_control_law_test.py +++ b/utils/tests/visual_track_control/tracking_control_law_test.py @@ -387,3 +387,53 @@ def test_reset_restores_slew_from_zero(): out = law.update(snap, 3000) # Reset 后限幅器归零：又从 0.1 开始渐变 assert out.vx_mps == pytest.approx(0.1, abs=1e-9) + + +# ---- L1 角度直入（方案 §2.1/§2.2 路线 A1） ---- +# make_snapshot 目标恒在画面中心（像素路径 β/α=0），故以下用例中任何非零 +# 航向/垂直输出都只能来自直入角度，可隔离验证"直入优先、绕过像素换算"。 + +def test_los_beta_direct_drives_yaw_rate(): + law = TrackingControlLaw(make_config()) + snap = make_snapshot(1000) + snap.los_beta_deg = 12.0 # 目标居中，仅提供框架角合成的 β + out = law.update(snap, 1000) + assert out.yaw_rate_dps == pytest.approx(12.0, abs=1e-9) # gain=1，右转正 + + +def test_los_beta_sign_negative_turns_left(): + law = TrackingControlLaw(make_config()) + snap = make_snapshot(1000) + snap.los_beta_deg = -8.0 # 目标偏左 → 负偏航角速度（左转） + out = law.update(snap, 1000) + assert out.yaw_rate_dps == pytest.approx(-8.0, abs=1e-9) + + +def test_los_alpha_direct_drives_vz(): + law = TrackingControlLaw(make_config()) + snap = make_snapshot(1000) + snap.los_alpha_deg = 5.0 # 目标偏低 → α>0 → 下降 + out = law.update(snap, 1000) + assert out.vz_mps == pytest.approx(0.1 * 5.0, abs=1e-9) + + +def test_los_beta_overrides_pixel_path(): + # 同时给像素偏移与直入角：直入优先，像素路径被绕过 + law = TrackingControlLaw(make_config()) + snap = make_snapshot(1000) + snap.track.center = (600.0, 500.0) # 像素路径会给 β≈11.31° + snap.los_beta_deg = 3.0 # 直入应覆盖为 3° + out = law.update(snap, 1000) + assert out.yaw_rate_dps == pytest.approx(3.0, abs=1e-9) + + +def test_los_none_falls_back_to_pixel(): + # 兼容性：默认 None 仍走既有像素路径（81 条既有测试的前提） + law = TrackingControlLaw(make_config()) + snap = make_snapshot(1000) + assert snap.los_beta_deg is None and snap.los_alpha_deg is None + snap.track.center = (600.0, 500.0) + out = law.update(snap, 1000) + beta_deg = math.degrees(math.atan(0.2)) + assert out.yaw_rate_dps == pytest.approx(beta_deg, abs=1e-6) + diff --git a/utils/visual_track_control/config.py b/utils/visual_track_control/config.py index ff7694a..9a73628 100644 --- a/utils/visual_track_control/config.py +++ b/utils/visual_track_control/config.py @@ -263,6 +263,24 @@ def _load_accel_limit(root: dict) -> AccelLimitConfig: return a +def load_from_dict(root: dict) -> TrackingConfig: + """从已解析的 JSON 对象构造 TrackingConfig（各分组必须齐全，缺字段/非法即抛）。 + + 供 pod_bridge 内嵌复用：桥接配置把本律的整份配置作为一个子对象加载。 + """ + if not isinstance(root, dict): + raise ValueError("配置解析失败: 根节点必须为对象") + + cfg = TrackingConfig() + cfg.control = _load_control(root) + cfg.camera = _load_camera(root) + cfg.heading = _load_heading(root) + cfg.vertical = _load_vertical(root) + cfg.distance = _load_distance(root) + cfg.accel_limit = _load_accel_limit(root) + return cfg + + def load_from_json(path: str) -> TrackingConfig: """从 JSON 文件一次性加载配置（启动期调用，调参重启生效）。""" try: @@ -273,14 +291,4 @@ def load_from_json(path: str) -> TrackingConfig: except json.JSONDecodeError as exc: raise ValueError(f"配置文件解析失败: {path} ({exc})") from None - if not isinstance(root, dict): - raise ValueError(f"配置文件解析失败: {path} (根节点必须为对象)") - - cfg = TrackingConfig() - cfg.control = _load_control(root) - cfg.camera = _load_camera(root) - cfg.heading = _load_heading(root) - cfg.vertical = _load_vertical(root) - cfg.distance = _load_distance(root) - cfg.accel_limit = _load_accel_limit(root) - return cfg + return load_from_dict(root) diff --git a/utils/visual_track_control/tracking_control_law.py b/utils/visual_track_control/tracking_control_law.py index cab5c48..a378327 100644 --- a/utils/visual_track_control/tracking_control_law.py +++ b/utils/visual_track_control/tracking_control_law.py @@ -7,7 +7,8 @@ ① 超龄判定（姿态优先于视觉，超龄 → 降级保持） ② 判丢 → 零速刹车（进入时清积分与限幅器历史） ③ Coast（is_predicted）→ 清距离积分防饱和 - ④ 像素偏移 → 视线角 β/α（全程针孔模型） + ④ 视线角 β/α：默认像素偏移→针孔模型换算；若 snapshot 直入角度（los_beta_deg/ + los_alpha_deg，L1 航向伺服路线 A1）则优先采用，绕过 FOV 换算 ⑤ 水平航向（速率式/位置式，A2：位置式合成绝对 NED 航向） ⑥ 垂直统一速度支路（A1：不切姿态微调模式） ⑦ 距离通道 PID → 前向速度；无距离按配置降级（hold/slow_approach/exit， @@ -130,12 +131,20 @@ class TrackingControlLaw: # 航向通道为纯比例增益映射（§4.2），无积分项，无需清理 out.coast_active = True - # ---- ④ 像素偏移 → 视线角（§4.1 水平 β / §5.1 垂直 α） ---- - # x_offset = cx − W/2（右正），y_offset = cy − H/2（下正） - x_offset = snapshot.track.center[0] - self._cfg.camera.image_width * 0.5 - y_offset = snapshot.track.center[1] - self._cfg.camera.image_height * 0.5 - beta_deg = self._pixel_to_beta.convert(x_offset) * _K_RAD_TO_DEG - alpha_deg = self._pixel_to_alpha.convert(y_offset) * _K_RAD_TO_DEG + # ---- ④ 视线角 β/α（§4.1 水平 / §5.1 垂直） ---- + # 优先角度直入（L1 航向伺服，路线 A1）：信源已给角度量则跳过像素换算， + # 避免吊舱变焦后 FOV 变化导致失真；未提供（None）则回退既有像素路径。 + # 像素路径：x_offset = cx − W/2（右正），y_offset = cy − H/2（下正） + if snapshot.los_beta_deg is not None: + beta_deg = snapshot.los_beta_deg + else: + x_offset = snapshot.track.center[0] - self._cfg.camera.image_width * 0.5 + beta_deg = self._pixel_to_beta.convert(x_offset) * _K_RAD_TO_DEG + if snapshot.los_alpha_deg is not None: + alpha_deg = snapshot.los_alpha_deg + else: + y_offset = snapshot.track.center[1] - self._cfg.camera.image_height * 0.5 + alpha_deg = self._pixel_to_alpha.convert(y_offset) * _K_RAD_TO_DEG # ---- ⑤ 水平航向通道（§4.2：增益 → slew → 角速度限幅） ---- if self._cfg.heading.mode is HeadingMode.RATE: diff --git a/utils/visual_track_control/types.py b/utils/visual_track_control/types.py index a6a7a0d..ebb17a5 100644 --- a/utils/visual_track_control/types.py +++ b/utils/visual_track_control/types.py @@ -55,6 +55,12 @@ class ControlSnapshot: track: TrackState = field(default_factory=TrackState) track_time_ms: int = 0 # 取样单调时钟（毫秒），<=0 视为从未取样 + # 视线角直入（L1 航向伺服，方案 §2.1/§2.2 路线 A1）： + # 吊舱等已直接给出角度量的信源，跳过第④步像素→角度换算（避免变焦后 FOV 失真）。 + # 约定 β 右正、α 下正，与控制律像素路径一致。默认 None = 未提供，回退像素路径。 + los_beta_deg: Optional[float] = None # 水平视线角（度，右正） + los_alpha_deg: Optional[float] = None # 垂直视线角（度，下正） + # 目标距离（双目立体测距，§6.3：distance_valid 为 False 时不得当作目标距离） target_distance_m: float = 0.0 # 目标距离（米，双目前向分量 Z） distance_valid: bool = False # 距离有效标志（测距来自检测框自身，无跨模态关联）