精简仓库结构并增强任务链路：归档双目可选后端、新增LOS判据与自定义返航

归档 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。
2026-10-21 14:48:41 +08:00
parent 53fdd3b13f
commit df91b1110e
159 changed files with 6121 additions and 1617 deletions
@@ -0,0 +1,141 @@
+"""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