实现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
This commit is contained in:
keita
2026-10-09 12:52:14 +08:00
parent b147925d58
commit 5a944af4f5
12 changed files with 827 additions and 43 deletions
+59 -2
View File
@@ -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"