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

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