实现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

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,
+  "enabled": true,             // true=走 L1(TrackingControlLaw)，false=L2(GuidanceLaw)
-  "beta_sign": 1,            // 台架标定后填 ±1（框架角 + 脱靶量合成的符号）
+  "beta_sign": 1,              // 台架 S1 标定后填 ±1（符号错了会反向转）
   "alpha_sign": 1,
-  "use_off_target": true,    // β 是否叠加脱靶量（false = 只用框架角）
+  "beta_from_frame": false,    // 无电机 → 框架角无用；装电机后置 true
-  "vertical_enabled": false, // L1 先禁垂直，避免撞地；vz=0
+  "beta_from_off_target": true,// 无电机 → 脱靶量即 β 主信号
-  "require_vision": false    // true = 需我方视觉作为第二判据（S3 起开）
+  "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 侧（沿用既有结构，只改值）
+// tracking 段（内嵌整份 visual_track_control 配置，各分组须齐全；L1 首飞值）
-"heading":  { "mode": "rate", "gain": ..., "yaw_rate_limit_dps": 20.0, "yaw_slew_limit_dps2": 40.0 },
+"heading":  { "mode": "rate", "gain": 1.0, "yaw_rate_limit_dps": 20.0, "yaw_slew_limit_dps2": 40.0 },
-"distance": { "no_distance_action": "hold",          // 首次上机 = hold
+"distance": { "no_distance_action": "slow_approach",       // 决策3：低速接近（非 hold）
-              "no_distance_approach_limit_mps": 0.3 },
+              "no_distance_approach_limit_mps": 0.2 },     // 极低限速起步
-"vertical": { "vz_gain_mps_per_deg": 0.0 }          // L1 禁用
+"vertical": { "vz_gain_mps_per_deg": 0.05, "vz_limit_mps": 0.5 }  // 开垂直但压低
 ```
 
-> 首次上机一律 `no_distance_action = "hold"`：**只转向、不前近**。
+> 决策 3：首次上机 `no_distance_action = "slow_approach"`、限速 **0.2 m/s**（很低），
-> 航向验证通过后再切 `slow_approach` 并把限速压到 0.3 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（像素反推，不推荐）；
+1. **接入路线 = A1**：`ControlSnapshot` 加可选 `los_beta_deg`/`los_alpha_deg`，第④步角度直入优先、否则回退像素路径。已实现（`types.py` / `tracking_control_law.py`，补 5 条兼容性测试）。
-2. **目标类型**：人车（地面）/ 无人机（空中）—— 影响 L2 的几何反推法能否用，L1 不受影响；
+2. **目标类型 = 无人机（空中）**：影响 L2 几何反推法，L1 无关。
-3. **首次上机无距离动作**：`hold`（只转不前，我建议）/ `slow_approach`（0.3 m/s）。
+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。

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
+    }
   }
 }

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

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

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())

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)
+

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,
+from pod_bridge.config import (BridgeConfig, GuidanceConfig, HeadingServoConfig,
-                               load_from_json)
+                               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"
+

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

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)
+

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):
+    return load_from_dict(root)
-        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

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 垂直 α） ----
+        # ---- ④ 视线角 β/α（§4.1 水平 / §5.1 垂直） ----
-        # x_offset = cx − W/2（右正），y_offset = cy − H/2（下正）
+        # 优先角度直入（L1 航向伺服，路线 A1）：信源已给角度量则跳过像素换算，
-        x_offset = snapshot.track.center[0] - self._cfg.camera.image_width * 0.5
+        # 避免吊舱变焦后 FOV 变化导致失真；未提供（None）则回退既有像素路径。
-        y_offset = snapshot.track.center[1] - self._cfg.camera.image_height * 0.5
+        # 像素路径：x_offset = cx − W/2（右正），y_offset = cy − H/2（下正）
-        beta_deg = self._pixel_to_beta.convert(x_offset) * _K_RAD_TO_DEG
+        if snapshot.los_beta_deg is not None:
-        alpha_deg = self._pixel_to_alpha.convert(y_offset) * _K_RAD_TO_DEG
+            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:

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      # 距离有效标志（测距来自检测框自身，无跨模态关联）