实现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
2026-10-09 12:52:14 +08:00
parent b147925d58
commit 5a944af4f5
12 changed files with 827 additions and 43 deletions
@@ -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)
+
@@ -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"
+
@@ -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
@@ -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)
+