实现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:
@@ -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"
|
||||
|
||||
|
||||
Reference in New Issue
Block a user