keita
df91b1110e
归档 legacy/：stereo_yolo/stereo_ranger/target_tracker 三包 + 175 条测试 + demo/配置/权重/三份规格；运行链路不再依赖，默认 pytest 446 条，归档测试按需运行。 目标有效性第二判据改为 LOS 运动学一致性（pod_bridge/los_consistency.py），删除 require_vision；pod_locked() 叠加该判据，状态机随之后退重搜。 自定义返航（task_runner/return_home.py）：保持转场高度回起飞点 → 恢复初始航向 → 垂直降落，不再爬 RTL_RETURN_ALT；未起飞/无起飞点/超时/异常仍退回 PX4 原生 RTL。 任务链安全兜底：主循环异常→紧急返航、stop 幂等、landing_stop 保留遥测、in_air 流复用+超时、解锁/起飞与吊舱识别仅执行一次、rtl_timeout 上限。 入口共享件拆到 utils/sitl_support.py；pod_sitl_test.py→l1_sitl_test.py；删除旧架构 test.py；相机脚本移入 tools/。 新增 .gitignore 并剔除历史误跟踪的 59 个 .pyc；requirements 按实际依赖重写；文档全面对齐，新增 legacy/README.md 与 tools/README.md。
768 lines
27 KiB
Python
768 lines
27 KiB
Python
"""bridge_test.py —— PodBridge 闭环主循环单元测试（FakePx4 + FakePod，无硬件）。
|
||
|
||
覆盖：单周期 step（有/无目标、位置无效、位置时间戳超龄）、姿态下发节流与帧内容、
|
||
吊舱帧入站缓存、锁定门控（表2-2 bit2 + 遥测超龄）、触发式控制指令编码、
|
||
start/stop 生命周期与停机刹车、run() 主循环异常护栏与故障恢复。
|
||
"""
|
||
|
||
import asyncio
|
||
import math
|
||
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 = []
|
||
self.yaw_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
|
||
|
||
async def send_yaw_rate(self, yaw_rate_dps):
|
||
self.yaw_calls.append(yaw_rate_dps)
|
||
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_control_law_throttled_to_own_frequency():
|
||
"""生效律按自身频率推进：L2 配置 10Hz，外部 20Hz 调用时控制律只跑 10 次/秒。
|
||
|
||
链路维持（表3 姿态下发）不受此节流影响——否则吊舱解算会断流。
|
||
"""
|
||
px4 = FakePx4(_valid_state(1000))
|
||
pod = FakePod()
|
||
cfg = BridgeConfig()
|
||
cfg.heading_servo.enabled = False # L2 地理引导（10Hz）
|
||
cfg.guidance.frequency_hz = 10.0
|
||
cfg.link.attitude_downlink_hz = 20.0
|
||
cfg.ground_inject.enabled = True
|
||
ground = GroundTarget(latitude_deg=0.01, longitude_deg=0.0, time_ms=1000)
|
||
b = PodBridge(cfg, px4, pod, now_ms_fn=lambda: 1000,
|
||
ground_target_fn=lambda: ground)
|
||
|
||
# 统计控制律实际推进次数（包装 update，不改变行为）
|
||
law_updates = {"n": 0}
|
||
real_update = b._guidance.update
|
||
|
||
def counting_update(inp, now):
|
||
law_updates["n"] += 1
|
||
return real_update(inp, now)
|
||
|
||
b._guidance.update = counting_update
|
||
|
||
ticks = 10 # 20Hz 调用 10 拍 = 500ms
|
||
|
||
async def scenario():
|
||
for i in range(ticks):
|
||
await b.step(1000 + i * 50)
|
||
asyncio.run(scenario())
|
||
|
||
# 10Hz 下 500ms 只应推进 5 拍控制律；其余 5 拍重发上一拍输出保心跳
|
||
assert law_updates["n"] == 5
|
||
assert len(px4.velocity_calls) == ticks # offboard 设定点每拍都在下发
|
||
assert len(pod.sent) == ticks # 表3 姿态下发不受控制律节流影响
|
||
|
||
|
||
def test_control_law_throttle_resets_on_law_switch():
|
||
"""切律后立即出第一拍（不被上一拍的节流压住）。"""
|
||
px4 = FakePx4(_valid_state(1000))
|
||
pod = FakePod()
|
||
cfg = BridgeConfig()
|
||
cfg.guidance.frequency_hz = 10.0
|
||
b = _bridge(px4, pod, cfg)
|
||
|
||
async def scenario():
|
||
await b.step(1000)
|
||
b.set_heading_servo_enabled(True)
|
||
await b.step(1010) # 切律后 10ms 即应推进
|
||
asyncio.run(scenario())
|
||
assert b._last_law_ms == 1010
|
||
|
||
|
||
def test_stop_is_idempotent():
|
||
"""重复 stop() 只真正停机一次（真机在环实测：RTL 与收尾各调一次）。"""
|
||
px4 = FakePx4(_valid_state(1000))
|
||
pod = FakePod()
|
||
b = _bridge(px4, pod)
|
||
|
||
async def scenario():
|
||
await b.start()
|
||
await b.stop()
|
||
await b.stop() # 第二次应为 no-op
|
||
asyncio.run(scenario())
|
||
|
||
assert pod.closed is True
|
||
assert px4.offboard_stopped is True
|
||
assert len(px4.brake_calls) == 1 # 刹车只发一次
|
||
|
||
|
||
def test_start_after_stop_reenables_stop():
|
||
"""停机后重新 start() 可再次生效（标志位复位）。"""
|
||
px4 = FakePx4(_valid_state(1000))
|
||
pod = FakePod()
|
||
b = _bridge(px4, pod)
|
||
|
||
async def scenario():
|
||
await b.start()
|
||
await b.stop()
|
||
assert b._stopped is True
|
||
await b.start()
|
||
assert b._stopped is False
|
||
await b.stop()
|
||
asyncio.run(scenario())
|
||
assert len(px4.brake_calls) == 2 # 两次停机各发一次刹车
|
||
|
||
|
||
def test_landing_stop_keeps_telemetry_and_pod_open():
|
||
"""降落前只退 offboard：遥测与串口保留，供落地判定与高度曲线。"""
|
||
px4 = FakePx4(_valid_state(1000))
|
||
pod = FakePod()
|
||
b = _bridge(px4, pod)
|
||
|
||
async def scenario():
|
||
await b.start()
|
||
await b.landing_stop()
|
||
assert b._landing_stopped is True
|
||
assert b._stopped is False # 未全停
|
||
assert px4.offboard_stopped is True
|
||
assert px4.telemetry_stopped is False # 遥测仍在
|
||
assert pod.closed is False # 串口仍在
|
||
await b.stop() # 收尾才全停
|
||
asyncio.run(scenario())
|
||
assert px4.telemetry_stopped is True
|
||
assert pod.closed is True
|
||
assert len(px4.brake_calls) == 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 _set_pose(px4, now_ms: int, yaw_deg: float):
|
||
"""更新假飞控姿态与时间戳（保证控制律的姿态超龄判定通过）。"""
|
||
px4._state = replace(px4._state, yaw_rad=math.radians(yaw_deg),
|
||
time_ms=now_ms, position_time_ms=now_ms)
|
||
|
||
|
||
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_los_consistent_keeps_tracking():
|
||
"""机头右转多少、脱靶量就左移多少（目标在世界系静止）→ 判一致，正常跟踪。"""
|
||
clock = {"ms": 1_000_000}
|
||
px4 = FakePx4(_valid_state(clock["ms"]))
|
||
pod = FakePod()
|
||
b = PodBridge(_l1_config(), px4, pod, now_ms_fn=lambda: clock["ms"])
|
||
|
||
async def scenario():
|
||
for i in range(80): # 4s，20°/s 右转、脱靶量 −20°/s
|
||
clock["ms"] += 50
|
||
yaw = 20.0 * (i + 1) * 0.05
|
||
beta = 10.0 - 20.0 * (i + 1) * 0.05
|
||
_set_pose(px4, clock["ms"], yaw)
|
||
pod.queue(_locked_off_target(x=beta))
|
||
out = await b.step(clock["ms"])
|
||
return out
|
||
|
||
out = asyncio.run(scenario())
|
||
assert b._los_consistent is True
|
||
assert out.mode.name == "VELOCITY_HEADING"
|
||
assert b.pod_locked(clock["ms"]) is True
|
||
|
||
|
||
def test_l1_los_frozen_angle_blocks_tracking():
|
||
"""机头转了 60° 而脱靶量几乎不变 → 角度冻结/幽灵目标 → 判丢（刹车）。"""
|
||
clock = {"ms": 1_000_000}
|
||
px4 = FakePx4(_valid_state(clock["ms"]))
|
||
pod = FakePod()
|
||
b = PodBridge(_l1_config(), px4, pod, now_ms_fn=lambda: clock["ms"])
|
||
|
||
async def scenario():
|
||
out = None
|
||
for i in range(80): # 4s，右转 20°/s，脱靶量恒定 10°
|
||
clock["ms"] += 50
|
||
yaw = 20.0 * (i + 1) * 0.05
|
||
_set_pose(px4, clock["ms"], yaw)
|
||
pod.queue(_locked_off_target(x=10.0))
|
||
out = await b.step(clock["ms"])
|
||
return out
|
||
|
||
out = asyncio.run(scenario())
|
||
assert b._los_consistent is False
|
||
assert out.mode.name == "BRAKE_HOVER" # 控制律被喂 tracked=False
|
||
assert b.pod_locked(clock["ms"]) is False # 状态机侧也判为丢目标
|
||
|
||
|
||
def test_l1_los_disabled_falls_back_to_bit2():
|
||
"""关掉一致性判据 → 退回"只信 bit2"（角度冻结也不再判丢）。"""
|
||
clock = {"ms": 1_000_000}
|
||
px4 = FakePx4(_valid_state(clock["ms"]))
|
||
pod = FakePod()
|
||
b = PodBridge(_l1_config(los_consistency_enabled=False), px4, pod,
|
||
now_ms_fn=lambda: clock["ms"])
|
||
|
||
async def scenario():
|
||
out = None
|
||
for i in range(80):
|
||
clock["ms"] += 50
|
||
yaw = 20.0 * (i + 1) * 0.05
|
||
_set_pose(px4, clock["ms"], yaw)
|
||
pod.queue(_locked_off_target(x=10.0))
|
||
out = await b.step(clock["ms"])
|
||
return out
|
||
|
||
out = asyncio.run(scenario())
|
||
assert b._los_consistent is True
|
||
assert out.mode.name == "VELOCITY_HEADING"
|
||
|
||
|
||
# --- 地面坐标注入（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)
|
||
|