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