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。
142 lines
5.4 KiB
Python
142 lines
5.4 KiB
Python
"""los_consistency_test.py —— 吊舱角度运动学一致性判据的纯逻辑测试。
|
||
|
||
背景：本吊舱无电机、相机刚性固连机体，机头转 Δψ 时真实目标脱靶量必变 ≈−Δψ。
|
||
判据 `residual = Δβ + Δψ`：超限且激励足够 → 判不一致（角度冻结/幽灵目标）。
|
||
"""
|
||
|
||
import pytest
|
||
|
||
from pod_bridge.los_consistency import (LosConsistencyConfig,
|
||
LosConsistencyMonitor, wrap_deg_180)
|
||
|
||
|
||
def _mon(cfg=None):
|
||
return LosConsistencyMonitor(cfg or LosConsistencyConfig())
|
||
|
||
|
||
def _drive(mon, n, dt_ms, yaw0=0.0, beta0=0.0, yaw_rate=0.0, beta_rate=0.0,
|
||
t0=0):
|
||
"""按固定角速度喂 n 拍，返回最后一次状态。"""
|
||
status = None
|
||
for i in range(n):
|
||
t = t0 + (i + 1) * dt_ms
|
||
yaw = yaw0 + yaw_rate * (i + 1) * dt_ms / 1000.0
|
||
beta = beta0 + beta_rate * (i + 1) * dt_ms / 1000.0
|
||
status = mon.update(t, yaw, beta)
|
||
return status
|
||
|
||
|
||
# --- 条件开关 ---------------------------------------------------------------
|
||
def test_disabled_always_consistent():
|
||
mon = _mon(LosConsistencyConfig(enabled=False))
|
||
status = _drive(mon, 100, 50, yaw_rate=30.0, beta_rate=0.0)
|
||
assert status.consistent is True
|
||
assert status.excited is False
|
||
|
||
|
||
def test_no_history_is_not_excited():
|
||
mon = _mon()
|
||
status = mon.update(1000, 0.0, 10.0)
|
||
assert status.consistent is True
|
||
assert status.excited is False
|
||
|
||
|
||
def test_insufficient_span_is_not_excited():
|
||
"""窗口未积累到 3/4 之前不做判断（避免一开始就误判丢目标）。"""
|
||
mon = _mon(LosConsistencyConfig(window_ms=3000))
|
||
status = _drive(mon, 20, 50, yaw_rate=40.0, beta_rate=0.0) # 仅 1s
|
||
assert status.excited is False
|
||
assert status.consistent is True
|
||
|
||
|
||
# --- 一致：机头转、脱靶量反向跟随 -------------------------------------------
|
||
def test_consistent_rotation_is_ok():
|
||
"""右转 20°/s 且 β 以 −20°/s 变化 → 残差≈0。"""
|
||
mon = _mon()
|
||
status = _drive(mon, 80, 50, yaw_rate=20.0, beta_rate=-20.0) # 4s
|
||
assert status.excited is True
|
||
assert status.consistent is True
|
||
assert status.residual_deg == pytest.approx(0.0, abs=0.5)
|
||
|
||
|
||
def test_consistent_left_rotation_is_ok():
|
||
"""左转（负角速度）且 β 正向跟随 → 同样一致（符号对称）。"""
|
||
mon = _mon()
|
||
status = _drive(mon, 80, 50, yaw_rate=-20.0, beta_rate=20.0)
|
||
assert status.excited is True
|
||
assert status.consistent is True
|
||
|
||
|
||
def test_target_motion_within_residual_tolerance_is_ok():
|
||
"""目标自身在动（β 变化与 −Δψ 不完全相等）但残差在容差内 → 仍判一致。"""
|
||
mon = _mon(LosConsistencyConfig(max_residual_deg=8.0))
|
||
# 4s 内 Δψ=80°，β 只反向变 76° → 残差 4°（< 8°）
|
||
status = _drive(mon, 80, 50, yaw_rate=20.0, beta_rate=-19.0)
|
||
assert status.excited is True
|
||
assert status.consistent is True
|
||
|
||
|
||
# --- 不一致：角度冻结/幽灵目标 ---------------------------------------------
|
||
def test_frozen_beta_while_yawing_is_inconsistent():
|
||
"""机头转 80° 而脱靶量纹丝不动 → 角度冻结 → 判不一致。
|
||
|
||
比较窗口为 3s（默认），故 4s 数据里只比较最后 3s：Δψ = 20°/s × 3s = 60°。
|
||
"""
|
||
mon = _mon()
|
||
status = _drive(mon, 80, 50, yaw_rate=20.0, beta_rate=0.0)
|
||
assert status.excited is True
|
||
assert status.consistent is False
|
||
assert status.frozen is True
|
||
assert status.residual_deg == pytest.approx(60.0, abs=2.0) # 窗口内 Δψ
|
||
|
||
|
||
def test_small_yaw_change_does_not_judge_even_if_residual_big():
|
||
"""转得不够多（< 激励门槛）时不判不一致 —— 避免误杀。"""
|
||
mon = _mon(LosConsistencyConfig(min_yaw_change_deg=20.0))
|
||
# 窗口内只转 8°，虽然 β 没动（残差 8°）但未达激励门槛
|
||
status = _drive(mon, 80, 50, yaw_rate=2.0, beta_rate=0.0)
|
||
assert status.excited is False
|
||
assert status.consistent is True
|
||
|
||
|
||
# --- 历史管理 ---------------------------------------------------------------
|
||
def test_reset_clears_history():
|
||
mon = _mon()
|
||
_drive(mon, 80, 50, yaw_rate=20.0, beta_rate=0.0)
|
||
assert mon._samples # 有历史
|
||
mon.reset()
|
||
assert not mon._samples
|
||
status = mon.update(9999, 0.0, 0.0)
|
||
assert status.excited is False
|
||
|
||
|
||
def test_old_samples_dropped():
|
||
"""超出历史保留期的样本被丢弃，不会永久污染判定。"""
|
||
mon = _mon(LosConsistencyConfig(window_ms=1000, history_factor=2.0))
|
||
_drive(mon, 40, 50, yaw_rate=50.0, beta_rate=0.0) # 2s
|
||
# 再喂一拍"很久之后"，旧样本应被清掉
|
||
status = mon.update(60_000, 0.0, 0.0)
|
||
assert status.excited is False
|
||
|
||
|
||
# --- 角度回绕 ---------------------------------------------------------------
|
||
@pytest.mark.parametrize("deg,expect", [
|
||
(0.0, 0.0), (179.0, 179.0), (181.0, -179.0),
|
||
(-181.0, 179.0), (359.0, -1.0),
|
||
])
|
||
def test_wrap_deg_180(deg, expect):
|
||
assert wrap_deg_180(deg) == pytest.approx(expect)
|
||
|
||
|
||
def test_wrap_around_north_is_consistent():
|
||
"""航向跨过 ±180 与 β 同步变化时，不应因回绕误判。"""
|
||
mon = _mon()
|
||
# 从 yaw=170 转到 190（即 -170），β 从 10 到 -10
|
||
for i in range(80):
|
||
t = (i + 1) * 50
|
||
yaw = 170.0 + 20.0 * (i + 1) * 0.05
|
||
beta = 10.0 - 20.0 * (i + 1) * 0.05
|
||
status = mon.update(t, wrap_deg_180(yaw), beta)
|
||
assert status.excited is True
|
||
assert status.consistent is True
|