新增pod_bridge吊舱桥接中间件(树莓派4B部署)

- pod_bridge: 协议G版编解码/地理位置引导律/PX4网关/串口链路/闭环主循环 - 含三项安全修复: position_time_ms拆分、表2-2锁定门控、run()异常护栏 - 测试92条全绿; 附树莓派4B(Ubuntu 24.04)部署注意事项与最小依赖清单
2026-09-23 14:38:23 +08:00
parent 3a8e2c7f4b
commit 477ad5f5de
17 changed files with 2758 additions and 0 deletions
@@ -0,0 +1,152 @@
+"""guidance_test.py —— 地理位置引导律单元测试。
+
+覆盖：大地测量（方位/距离）、对准/未对准/背向目标的速率式输出、到达悬停、
+目标/位置超龄与不可用刹车、速度上限与加速度限幅渐变。
+"""
+
+import math
+
+import pytest
+
+from pod_bridge.guidance import (GuidanceConfig, GuidanceInput, GuidanceLaw,
+                                 haversine_distance_m, initial_bearing_deg)
+from visual_track_control.types import ControlMode
+
+_DEG_LAT_M = 111194.93  # 1° 纬度 ≈ 此米数（R=6371008.8）
+
+
+def _inp(own_lat=0.0, own_lon=0.0, own_yaw=0.0, tgt_lat=0.0, tgt_lon=0.0,
+         pos_valid=True, tgt_valid=True, pos_ms=1000, tgt_ms=1000):
+    return GuidanceInput(
+        own_lat_deg=own_lat, own_lon_deg=own_lon, own_yaw_deg=own_yaw,
+        own_position_valid=pos_valid, own_position_time_ms=pos_ms,
+        target_lat_deg=tgt_lat, target_lon_deg=tgt_lon,
+        target_valid=tgt_valid, target_time_ms=tgt_ms,
+    )
+
+
+# --- 大地测量 ---------------------------------------------------------------
+@pytest.mark.parametrize("tgt,expected", [
+    ((0.001, 0.0), 0.0),      # 正北
+    ((0.0, 0.001), 90.0),     # 正东（赤道）
+    ((-0.001, 0.0), 180.0),   # 正南
+    ((0.0, -0.001), 270.0),   # 正西
+])
+def test_initial_bearing_cardinals(tgt, expected):
+    b = initial_bearing_deg(0.0, 0.0, tgt[0], tgt[1])
+    # 0 与 360 等价，用回绕差判定
+    diff = abs((b - expected + 180.0) % 360.0 - 180.0)
+    assert diff < 0.5
+
+
+def test_haversine_distance_one_millidegree():
+    d = haversine_distance_m(0.0, 0.0, 0.001, 0.0)
+    assert d == pytest.approx(_DEG_LAT_M / 1000.0, abs=1.0)
+
+
+def test_haversine_zero_distance():
+    assert haversine_distance_m(31.2, 121.4, 31.2, 121.4) == pytest.approx(0.0, abs=1e-6)
+
+
+# --- 对准目标：前进、不偏航 -------------------------------------------------
+def test_aligned_target_moves_forward_no_yaw():
+    law = GuidanceLaw(GuidanceConfig())
+    out = law.update(_inp(tgt_lat=0.001, own_yaw=0.0), now_ms=1000)  # 正北 111m，机头朝北
+    assert out.mode is ControlMode.VELOCITY_HEADING
+    assert out.valid is True
+    assert out.vx_body_mps > 0.0
+    assert out.yaw_rate_dps == pytest.approx(0.0, abs=1e-6)
+    assert out.vz_mps == 0.0
+    assert law.last_bearing_deg == pytest.approx(0.0, abs=0.5)
+
+
+def test_target_to_east_yaws_right_no_forward():
+    law = GuidanceLaw(GuidanceConfig())
+    # 目标正东（方位 90），机头朝北（0）→ 误差 +90：右转为正、未对准时前向门控为 0
+    out = law.update(_inp(tgt_lon=0.001, own_yaw=0.0), now_ms=1000)
+    assert out.yaw_rate_dps > 0.0
+    assert out.vx_body_mps == pytest.approx(0.0, abs=1e-6)
+
+
+def test_facing_target_moves_forward():
+    law = GuidanceLaw(GuidanceConfig())
+    # 目标正东、机头也朝东（yaw=90）→ 已对准，前进不偏航
+    out = law.update(_inp(tgt_lon=0.001, own_yaw=90.0), now_ms=1000)
+    assert out.vx_body_mps > 0.0
+    assert out.yaw_rate_dps == pytest.approx(0.0, abs=1e-6)
+
+
+def test_target_behind_no_forward_and_turns():
+    law = GuidanceLaw(GuidanceConfig())
+    out = law.update(_inp(tgt_lat=-0.001, own_yaw=0.0), now_ms=1000)  # 正南，机头朝北
+    assert out.vx_body_mps == pytest.approx(0.0, abs=1e-6)  # cos(180°)<0 门控为 0
+    assert abs(out.yaw_rate_dps) > 0.0
+
+
+# --- 到达 / 不可用 / 超龄 → 刹车 -------------------------------------------
+def test_arrived_within_stop_radius_brakes():
+    law = GuidanceLaw(GuidanceConfig(stop_radius_m=5.0))
+    near = 2.0 / _DEG_LAT_M  # ≈2m 纬度偏移
+    out = law.update(_inp(tgt_lat=near), now_ms=1000)
+    assert out.mode is ControlMode.BRAKE_HOVER
+    assert out.vx_body_mps == 0.0
+
+
+def test_stale_target_brakes():
+    law = GuidanceLaw(GuidanceConfig(target_stale_ms=1000))
+    out = law.update(_inp(tgt_lat=0.001, tgt_ms=1000), now_ms=5000)
+    assert out.mode is ControlMode.BRAKE_HOVER
+
+
+def test_invalid_position_brakes():
+    law = GuidanceLaw(GuidanceConfig())
+    out = law.update(_inp(tgt_lat=0.001, pos_valid=False), now_ms=1000)
+    assert out.mode is ControlMode.BRAKE_HOVER
+
+
+def test_invalid_target_brakes():
+    law = GuidanceLaw(GuidanceConfig())
+    out = law.update(_inp(tgt_lat=0.001, tgt_valid=False), now_ms=1000)
+    assert out.mode is ControlMode.BRAKE_HOVER
+
+
+# --- 速度上限与加速度限幅渐变 ----------------------------------------------
+def test_speed_ramps_up_and_clamps_to_max():
+    cfg = GuidanceConfig(frequency_hz=10.0, max_speed_mps=5.0,
+                         speed_slew_mps2=2.0, stop_radius_m=5.0)
+    law = GuidanceLaw(cfg)
+    forward = 0.0
+    now = 1000
+    for _ in range(60):  # 6s，足够加速度限幅爬到上限
+        out = law.update(_inp(tgt_lat=0.05, own_yaw=0.0, pos_ms=now, tgt_ms=now),
+                         now_ms=now)
+        forward = out.vx_body_mps
+        assert forward <= cfg.max_speed_mps + 1e-9
+        now += 100
+    assert forward == pytest.approx(cfg.max_speed_mps, abs=0.1)
+
+
+def test_first_command_is_slew_limited_not_full_speed():
+    law = GuidanceLaw(GuidanceConfig(frequency_hz=10.0, max_speed_mps=5.0,
+                                     speed_slew_mps2=2.0))
+    out = law.update(_inp(tgt_lat=0.05, own_yaw=0.0), now_ms=1000)
+    # 首拍从零渐变：dt=0.1s，加速度上限 2 → 前向 ≤ 0.2，远小于 5
+    assert 0.0 < out.vx_body_mps <= 0.2 + 1e-9
+
+
+def test_brake_then_recover_ramps_from_zero():
+    law = GuidanceLaw(GuidanceConfig(frequency_hz=10.0, speed_slew_mps2=2.0))
+    law.update(_inp(tgt_lat=0.05, own_yaw=0.0), now_ms=1000)   # 前进
+    law.update(_inp(tgt_lat=0.05, tgt_valid=False), now_ms=1100)  # 刹车，清状态
+    out = law.update(_inp(tgt_lat=0.05, own_yaw=0.0), now_ms=1200)  # 恢复
+    assert out.mode is ControlMode.VELOCITY_HEADING
+    assert out.vx_body_mps <= 0.2 + 1e-9  # 从零重新渐变
+
+
+def test_reset_clears_state():
+    law = GuidanceLaw(GuidanceConfig())
+    law.update(_inp(tgt_lat=0.05, own_yaw=0.0), now_ms=1000)
+    law.reset()
+    out = law.update(_inp(tgt_lat=0.05, own_yaw=0.0, pos_ms=2000, tgt_ms=2000),
+                     now_ms=2000)
+    assert out.vx_body_mps <= 0.2 + 1e-9  # 复位后首条仍从零渐变