code-test-drone/utils/tests/pod_bridge/guidance_test.py

"""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  # 复位后首条仍从零渐变