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