keita
ded17d7995
- utils/task_runner: 6态状态机(WAIT→ARM_TAKEOFF→GOTO(L2)→TRACK(L1)→APPROACH→RTL) + 编排器(快照采集/状态动作) + 双层日志(事件JSONL/遥测CSV) + 吊舱控制指令(0x07/0x0F/0x0E/0x08) - pod_bridge: ground_link 地面坐标UDP注入(超龄判无效,无激光替代表4); set_heading_servo_enabled() 运行时L1/L2切换(切换即reset清积分防输出跳变); pod_locked 提为公共门控; config 新增 ground_inject 段 - virtual_pod 模拟吊舱(无电机语义) + verify_px4 增量④仅飞控验证 + pod_sitl_test L1 SITL全流程(飞行编排不塞进 run_bridge, 保持纯offboard闭环入口) - 入口: task_runner_sitl(VirtualPod+虚拟目标, SITL端到端跑通约1m24s) / task_runner_real(真串口+UDP注入+吊舱控制) - 地面站 ground_station: 坐标注入 + 状态可视化(Flask) - S1标定回填(2026-10-09): beta_sign=+1 / alpha_sign=-1, 加标定回归测试守护 - 新增测试: task_runner x4 + ground_link/virtual_pod*/bridge/px4_gateway 增量; 全量 483 passed - 文档: 预定流程(需求) + 预定流程-操作指南(操作/待办) + L1方案补S1结论与实现状态
599 lines
21 KiB
Python
599 lines
21 KiB
Python
"""bridge_test.py —— PodBridge 闭环主循环单元测试（FakePx4 + FakePod，无硬件）。
|
||
|
||
覆盖：单周期 step（有/无目标、位置无效、位置时间戳超龄）、姿态下发节流与帧内容、
|
||
吊舱帧入站缓存、锁定门控（表2-2 bit2 + 遥测超龄）、触发式控制指令编码、
|
||
start/stop 生命周期与停机刹车、run() 主循环异常护栏与故障恢复。
|
||
"""
|
||
|
||
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.ground_link import GroundTarget
|
||
from pod_bridge.protocol import (ControlCode, ControlCommand, TargetGeo,
|
||
Telemetry, decode_attitude, decode_control)
|
||
from pod_bridge.px4_gateway import OwnState
|
||
|
||
|
||
class FakePx4:
|
||
def __init__(self, state=None):
|
||
self._state = state or OwnState()
|
||
self.telemetry_started = False
|
||
self.telemetry_stopped = False
|
||
self.offboard_started = False
|
||
self.offboard_stopped = False
|
||
self.offboard_start_ok = True
|
||
self.velocity_calls = []
|
||
self.brake_calls = []
|
||
|
||
def own_state(self):
|
||
return replace(self._state)
|
||
|
||
async def start_telemetry(self):
|
||
self.telemetry_started = True
|
||
|
||
async def stop_telemetry(self):
|
||
self.telemetry_stopped = True
|
||
|
||
async def start_offboard(self):
|
||
self.offboard_started = True
|
||
return self.offboard_start_ok
|
||
|
||
async def stop_offboard(self):
|
||
self.offboard_stopped = True
|
||
|
||
async def send_velocity_heading(self, out, time_boot_ms=None):
|
||
self.velocity_calls.append((out, time_boot_ms))
|
||
return True
|
||
|
||
async def send_brake_hover(self, time_boot_ms=None):
|
||
self.brake_calls.append(time_boot_ms)
|
||
return True
|
||
|
||
|
||
class FakePod:
|
||
def __init__(self):
|
||
self.inbox = []
|
||
self.sent = []
|
||
self.closed = False
|
||
|
||
def queue(self, *frames):
|
||
self.inbox.extend(frames)
|
||
|
||
def poll(self):
|
||
out, self.inbox = self.inbox, []
|
||
return out
|
||
|
||
def send(self, data):
|
||
self.sent.append(data)
|
||
|
||
def close(self):
|
||
self.closed = True
|
||
|
||
|
||
def _valid_state(now_ms, lat=0.0, lon=0.0, yaw_rad=0.0, gs=0.0):
|
||
return OwnState(latitude_deg=lat, longitude_deg=lon, yaw_rad=yaw_rad,
|
||
ground_speed_mps=gs, fix_type=3, num_satellites=12,
|
||
has_position=True, has_attitude=True, position_valid=True,
|
||
time_ms=now_ms, position_time_ms=now_ms)
|
||
|
||
|
||
def _locked_telemetry(laser=50.0):
|
||
"""状态字节1 bit2=1(锁定)的遥测帧;默认带有效激光(默认闸门要求激光有效)。"""
|
||
return Telemetry(status1=0b00000100, laser_range_m=laser)
|
||
|
||
|
||
def _bridge(px4, pod, cfg=None):
|
||
return PodBridge(cfg or BridgeConfig(), px4, pod, now_ms_fn=lambda: 0)
|
||
|
||
|
||
# --- step：有目标 → 速度指令 ------------------------------------------------
|
||
def test_step_with_target_commands_velocity():
|
||
px4 = FakePx4(_valid_state(1000))
|
||
pod = FakePod()
|
||
pod.queue(TargetGeo(latitude_deg=0.001, longitude_deg=0.0), # 正北 ~111m
|
||
_locked_telemetry())
|
||
b = _bridge(px4, pod)
|
||
|
||
out = asyncio.run(b.step(1000))
|
||
assert out.mode.name == "VELOCITY_HEADING"
|
||
assert len(px4.velocity_calls) == 1
|
||
assert px4.brake_calls == []
|
||
|
||
|
||
def test_step_without_target_brakes():
|
||
px4 = FakePx4(_valid_state(1000))
|
||
pod = FakePod()
|
||
b = _bridge(px4, pod)
|
||
|
||
out = asyncio.run(b.step(1000))
|
||
assert out.mode.name == "BRAKE_HOVER"
|
||
assert len(px4.brake_calls) == 1
|
||
assert px4.velocity_calls == []
|
||
|
||
|
||
def test_step_invalid_position_brakes_even_with_target():
|
||
state = replace(_valid_state(1000), position_valid=False, fix_type=0)
|
||
px4 = FakePx4(state)
|
||
pod = FakePod()
|
||
pod.queue(TargetGeo(latitude_deg=0.001))
|
||
b = _bridge(px4, pod)
|
||
|
||
out = asyncio.run(b.step(1000))
|
||
assert out.mode.name == "BRAKE_HOVER"
|
||
|
||
|
||
# --- 姿态下发：首拍即发 + 节流 + 帧内容 ------------------------------------
|
||
def test_attitude_downlinked_on_first_step_with_own_state():
|
||
px4 = FakePx4(_valid_state(1000, lat=31.5, lon=121.5, yaw_rad=1.57, gs=4.0))
|
||
pod = FakePod()
|
||
b = _bridge(px4, pod)
|
||
|
||
asyncio.run(b.step(1000))
|
||
assert len(pod.sent) == 1
|
||
att = decode_attitude(pod.sent[0])
|
||
assert abs(att.latitude_deg - 31.5) < 1e-6
|
||
assert abs(att.longitude_deg - 121.5) < 1e-6
|
||
assert abs(att.yaw_deg - 89.95) < 0.02 # 1.57 rad ≈ 89.95°
|
||
assert att.ground_speed_mps == 4.0
|
||
assert att.year >= 2026 and 1 <= att.month <= 12
|
||
|
||
|
||
def test_attitude_downlink_throttled():
|
||
px4 = FakePx4(_valid_state(1000))
|
||
pod = FakePod()
|
||
cfg = BridgeConfig()
|
||
cfg.link.attitude_downlink_hz = 20.0 # 周期 50ms
|
||
b = _bridge(px4, pod, cfg)
|
||
|
||
async def scenario():
|
||
await b.step(1000) # 发（首拍）
|
||
await b.step(1010) # 10ms < 50ms，跳过
|
||
await b.step(1040) # 40ms < 50ms，跳过
|
||
await b.step(1060) # 60ms ≥ 50ms，发
|
||
asyncio.run(scenario())
|
||
assert len(pod.sent) == 2
|
||
|
||
|
||
# --- 吊舱帧入站缓存 --------------------------------------------------------
|
||
def test_target_frame_updates_latest_and_flips_to_velocity():
|
||
px4 = FakePx4(_valid_state(1000))
|
||
pod = FakePod()
|
||
b = _bridge(px4, pod)
|
||
|
||
asyncio.run(b.step(1000)) # 无目标 → 刹车
|
||
assert b.latest_target is None
|
||
pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry())
|
||
asyncio.run(b.step(1100)) # 收到目标且锁定 → 速度
|
||
assert b.latest_target is not None
|
||
assert len(px4.velocity_calls) == 1
|
||
|
||
|
||
def test_telemetry_frame_cached():
|
||
px4 = FakePx4(_valid_state(1000))
|
||
pod = FakePod()
|
||
pod.queue(Telemetry(laser_range_m=88.8))
|
||
b = _bridge(px4, pod)
|
||
asyncio.run(b.step(1000))
|
||
assert b.latest_telemetry is not None
|
||
assert b.latest_telemetry[0].laser_range_m == 88.8
|
||
|
||
|
||
# --- 触发式控制指令 --------------------------------------------------------
|
||
def test_send_control_encodes_frame():
|
||
px4 = FakePx4(_valid_state(1000))
|
||
pod = FakePod()
|
||
b = _bridge(px4, pod)
|
||
b.send_control(ControlCommand(code=ControlCode.CENTER))
|
||
assert len(pod.sent) == 1
|
||
assert decode_control(pod.sent[0]).code == ControlCode.CENTER
|
||
|
||
|
||
# --- 生命周期 --------------------------------------------------------------
|
||
def test_start_wires_telemetry_and_offboard():
|
||
px4 = FakePx4(_valid_state(0))
|
||
b = _bridge(px4, FakePod())
|
||
assert asyncio.run(b.start()) is True
|
||
assert px4.telemetry_started and px4.offboard_started
|
||
|
||
|
||
def test_start_returns_false_when_offboard_fails():
|
||
px4 = FakePx4(_valid_state(0))
|
||
px4.offboard_start_ok = False
|
||
b = _bridge(px4, FakePod())
|
||
assert asyncio.run(b.start()) is False
|
||
|
||
|
||
def test_stop_brakes_and_tears_down():
|
||
px4 = FakePx4(_valid_state(0))
|
||
pod = FakePod()
|
||
b = _bridge(px4, pod)
|
||
|
||
async def scenario():
|
||
await b.start()
|
||
await b.stop()
|
||
asyncio.run(scenario())
|
||
assert len(px4.brake_calls) >= 1 # 停机前刹车
|
||
assert px4.offboard_stopped and px4.telemetry_stopped
|
||
assert pod.closed
|
||
|
||
|
||
|
||
# --- 缺陷①回归:position 时间戳超龄 -------------------------------------------
|
||
def test_step_brakes_when_position_timestamp_stale():
|
||
"""time_ms 被姿态流刷得很新,但 position_time_ms 超龄 → 必须刹车。"""
|
||
state = replace(_valid_state(1000), time_ms=100000, position_time_ms=1000)
|
||
px4 = FakePx4(state)
|
||
pod = FakePod()
|
||
pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry())
|
||
b = _bridge(px4, pod)
|
||
|
||
out = asyncio.run(b.step(100000)) # position 已 99s 未更新
|
||
assert out.mode.name == "BRAKE_HOVER"
|
||
assert px4.velocity_calls == []
|
||
|
||
|
||
# --- 缺陷②:锁定门控 -----------------------------------------------------------
|
||
def test_target_without_lock_bit_brakes():
|
||
"""吊舱在搜索状态(bit2=0),即使收到表4 坐标也必须刹车。"""
|
||
px4 = FakePx4(_valid_state(1000))
|
||
pod = FakePod()
|
||
pod.queue(TargetGeo(latitude_deg=0.001), Telemetry(status1=0)) # 未锁定
|
||
b = _bridge(px4, pod)
|
||
|
||
out = asyncio.run(b.step(1000))
|
||
assert out.mode.name == "BRAKE_HOVER"
|
||
assert px4.velocity_calls == []
|
||
|
||
|
||
def test_target_without_any_telemetry_brakes():
|
||
"""只收到表4、从未收到表2-2 → 锁定状态未知,按未锁定处理。"""
|
||
px4 = FakePx4(_valid_state(1000))
|
||
pod = FakePod()
|
||
pod.queue(TargetGeo(latitude_deg=0.001))
|
||
b = _bridge(px4, pod)
|
||
|
||
out = asyncio.run(b.step(1000))
|
||
assert out.mode.name == "BRAKE_HOVER"
|
||
|
||
|
||
def test_target_with_stale_telemetry_brakes():
|
||
"""锁定遥测超龄(> link.telemetry_timeout_ms)→ 目标视为无效。"""
|
||
px4 = FakePx4(_valid_state(1000))
|
||
pod = FakePod()
|
||
cfg = BridgeConfig()
|
||
cfg.link.telemetry_timeout_ms = 500
|
||
b = PodBridge(cfg, px4, pod, now_ms_fn=lambda: 0)
|
||
|
||
async def scenario():
|
||
pod.queue(_locked_telemetry(), TargetGeo(latitude_deg=0.001))
|
||
await b.step(1000) # 遥测新鲜,但此刻若无目标则刹车
|
||
pod.queue(TargetGeo(latitude_deg=0.001))
|
||
out1 = await b.step(1200) # 遥测 200ms 前,仍新鲜 → 速度
|
||
out2 = await b.step(1600) # 遥测 600ms 前,超龄 → 刹车
|
||
return out1, out2
|
||
|
||
out1, out2 = asyncio.run(scenario())
|
||
assert out1.mode.name == "VELOCITY_HEADING"
|
||
assert out2.mode.name == "BRAKE_HOVER"
|
||
|
||
|
||
def test_lock_lost_midflight_falls_back_to_brake():
|
||
"""先锁定引导,随后遥测变为搜索状态 → 立即回落刹车。"""
|
||
px4 = FakePx4(_valid_state(1000))
|
||
pod = FakePod()
|
||
b = _bridge(px4, pod)
|
||
|
||
async def scenario():
|
||
pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry())
|
||
out1 = await b.step(1000)
|
||
pod.queue(Telemetry(status1=0)) # 丢失锁定
|
||
out2 = await b.step(1100)
|
||
return out1, out2
|
||
|
||
out1, out2 = asyncio.run(scenario())
|
||
assert out1.mode.name == "VELOCITY_HEADING"
|
||
assert out2.mode.name == "BRAKE_HOVER"
|
||
|
||
|
||
# --- 激光有效性门控(锁定位置位粘滞时的兜底)-----------------------------------
|
||
def test_laser_invalid_blocks_guidance_when_required():
|
||
"""默认 require_laser_valid=True:锁定位置位但激光无效 → 目标无效 → 刹车。"""
|
||
px4 = FakePx4(_valid_state(1000))
|
||
pod = FakePod()
|
||
pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry(laser=None))
|
||
b = _bridge(px4, pod)
|
||
|
||
out = asyncio.run(b.step(1000))
|
||
assert out.mode.name == "BRAKE_HOVER"
|
||
assert px4.velocity_calls == []
|
||
|
||
|
||
def test_laser_invalid_allowed_when_gate_disabled():
|
||
"""本设备无激光(厂商确认)→ require_laser_valid=False 时照常引导。"""
|
||
px4 = FakePx4(_valid_state(1000))
|
||
pod = FakePod()
|
||
pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry(laser=None))
|
||
cfg = BridgeConfig()
|
||
cfg.link.require_laser_valid = False
|
||
b = _bridge(px4, pod, cfg)
|
||
|
||
out = asyncio.run(b.step(1000))
|
||
assert out.mode.name == "VELOCITY_HEADING"
|
||
|
||
|
||
def test_deployed_config_disables_laser_gate():
|
||
"""部署配置回归:本机无激光,pod_bridge.json 必须关掉激光闸门。"""
|
||
import os
|
||
path = os.path.join(os.path.dirname(os.path.dirname(os.path.dirname(
|
||
os.path.abspath(__file__)))), "config", "pod_bridge.json")
|
||
cfg = load_from_json(path)
|
||
assert cfg.link.require_laser_valid is False
|
||
|
||
|
||
# --- 缺陷③:run() 异常护栏 -----------------------------------------------------
|
||
class FlakyPod(FakePod):
|
||
"""前 fail_times 次 poll 抛串口异常,之后恢复正常。"""
|
||
|
||
def __init__(self, fail_times):
|
||
super().__init__()
|
||
self.fail_times = fail_times
|
||
|
||
def poll(self):
|
||
if self.fail_times > 0:
|
||
self.fail_times -= 1
|
||
raise OSError("serial device disconnected")
|
||
return super().poll()
|
||
|
||
|
||
def _run_bridge(px4, pod, hz=200.0):
|
||
cfg = BridgeConfig()
|
||
cfg.guidance.frequency_hz = hz # 加快节拍,缩短测试时间
|
||
return PodBridge(cfg, px4, pod) # 用真实单调时钟
|
||
|
||
|
||
def test_run_survives_poll_exceptions_and_keeps_heartbeat():
|
||
"""串口持续异常:任务不死、每周期补发刹车保心跳、stop_event 正常退出。"""
|
||
now = int(time.monotonic() * 1000)
|
||
px4 = FakePx4(_valid_state(now))
|
||
pod = FlakyPod(fail_times=10**9) # 永远失败
|
||
b = _run_bridge(px4, pod)
|
||
|
||
async def scenario():
|
||
stop = asyncio.Event()
|
||
task = asyncio.ensure_future(b.run(stop))
|
||
await asyncio.sleep(0.2)
|
||
stop.set()
|
||
await asyncio.wait_for(task, timeout=2.0)
|
||
|
||
asyncio.run(scenario())
|
||
assert len(px4.brake_calls) > 0 # 异常期间持续补发刹车
|
||
assert pod.fail_times > 0 # 循环确实在反复尝试(未提前退出)
|
||
|
||
|
||
def test_run_recovers_after_transient_failure():
|
||
"""串口短暂异常后恢复:异常期刹车,恢复后照常引导。"""
|
||
now = int(time.monotonic() * 1000)
|
||
px4 = FakePx4(_valid_state(now))
|
||
pod = FlakyPod(fail_times=3)
|
||
pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry())
|
||
b = _run_bridge(px4, pod)
|
||
|
||
async def scenario():
|
||
stop = asyncio.Event()
|
||
task = asyncio.ensure_future(b.run(stop))
|
||
await asyncio.sleep(0.3)
|
||
stop.set()
|
||
await asyncio.wait_for(task, timeout=2.0)
|
||
|
||
asyncio.run(scenario())
|
||
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)
|
||
|
||
|
||
# --- 地面坐标注入（A2）-------------------------------------------------------
|
||
def _inject_config(timeout_ms=5000):
|
||
cfg = BridgeConfig()
|
||
cfg.ground_inject.enabled = True
|
||
cfg.ground_inject.timeout_ms = timeout_ms
|
||
return cfg
|
||
|
||
|
||
def test_ground_inject_priority_over_table4():
|
||
"""注入坐标优先于表4：吊舱无表4/无锁定（本设备常态），注入新鲜坐标照常引导。"""
|
||
px4 = FakePx4(_valid_state(1000))
|
||
pod = FakePod()
|
||
injected = GroundTarget(latitude_deg=0.001, longitude_deg=0.0, time_ms=1000)
|
||
b = PodBridge(_inject_config(), px4, pod, now_ms_fn=lambda: 0,
|
||
ground_target_fn=lambda: injected)
|
||
out = asyncio.run(b.step(1000))
|
||
assert out.mode.name == "VELOCITY_HEADING"
|
||
assert len(px4.velocity_calls) == 1
|
||
|
||
|
||
def test_ground_inject_stale_falls_back_to_brake():
|
||
"""注入坐标超龄 → 视为无目标 → 刹车（不得朝过期坐标飞）。"""
|
||
px4 = FakePx4(_valid_state(2000))
|
||
pod = FakePod()
|
||
injected = GroundTarget(latitude_deg=0.001, longitude_deg=0.0,
|
||
time_ms=1000) # 1000ms 前注入
|
||
cfg = _inject_config(timeout_ms=500)
|
||
b = PodBridge(cfg, px4, pod, now_ms_fn=lambda: 0,
|
||
ground_target_fn=lambda: injected)
|
||
out = asyncio.run(b.step(2000)) # 差值 1000ms > 500ms → 超龄
|
||
assert out.mode.name == "BRAKE_HOVER"
|
||
assert px4.velocity_calls == []
|
||
|
||
|
||
def test_ground_inject_disabled_ignores_injection():
|
||
"""未启用地面注入 → 忽略注入坐标，回退表4 原路径（无表4 → 刹车）。"""
|
||
px4 = FakePx4(_valid_state(1000))
|
||
pod = FakePod()
|
||
injected = GroundTarget(latitude_deg=0.001, longitude_deg=0.0, time_ms=1000)
|
||
cfg = BridgeConfig() # ground_inject.enabled=False
|
||
b = PodBridge(cfg, px4, pod, now_ms_fn=lambda: 0,
|
||
ground_target_fn=lambda: injected)
|
||
out = asyncio.run(b.step(1000))
|
||
assert out.mode.name == "BRAKE_HOVER"
|
||
|
||
|
||
# --- 运行时切换控制律（A3：GOTO(L2) ↔ TRACK(L1)）-----------------------------
|
||
def test_set_heading_servo_enabled_runtime_switch():
|
||
"""运行时切换控制律：初始 L1 → 切 L2（刹车）→ 切回 L1（速度）。"""
|
||
px4 = FakePx4(_valid_state(1000))
|
||
pod = FakePod()
|
||
b = _bridge(px4, pod, _l1_config()) # enabled=True → 初始 L1
|
||
|
||
# 初始 L1：锁定 + 脱靶量 → 速度
|
||
pod.queue(_locked_off_target(x=10.0))
|
||
out = asyncio.run(b.step(1000))
|
||
assert out.mode.name == "VELOCITY_HEADING"
|
||
assert out.yaw_rate_dps == pytest.approx(10.0, abs=1e-9)
|
||
|
||
# 切 L2：无表4/无锁定 → 刹车
|
||
b.set_heading_servo_enabled(False)
|
||
out = asyncio.run(b.step(1100))
|
||
assert out.mode.name == "BRAKE_HOVER"
|
||
|
||
# 切回 L1：重新喂锁定脱靶量 → 速度
|
||
b.set_heading_servo_enabled(True)
|
||
pod.queue(_locked_off_target(x=10.0))
|
||
out = asyncio.run(b.step(1200))
|
||
assert out.mode.name == "VELOCITY_HEADING"
|
||
assert out.yaw_rate_dps == pytest.approx(10.0, abs=1e-9)
|
||
|