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