test(mavlink): add GPS_INPUT field encoding unit tests

12 tests verify _eskf_to_gps_input produces MAVLink #232-compliant fields:
- lat/lon: int × 1e7 (degE7)
- ENU→NED velocity conversion
- satellites_visible=10 synthetic (prevents ArduPilot failsafe)
- ConfidenceTier → fix_type mapping (HIGH/MEDIUM=3, LOW/FAILED=0)
- Accuracy from covariance, hdop/vdop floor clamp

Pure unit tests — no SITL/docker dependency.
Ref: docs/superpowers/specs/2026-04-18-oss-stack-tech-audit-design.md §6

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
Yuzviak
2026-04-18 16:24:38 +03:00
parent e4ba7bced3
commit 44f96d6d2d
+154
View File
@@ -0,0 +1,154 @@
"""Unit tests for GPS_INPUT (MAVLink #232) field encoding.
Pure unit tests — no SITL dependency. Verifies that _eskf_to_gps_input
produces field values per MAVLink spec:
- lat/lon: int × 1e7 (degE7)
- alt: float metres MSL
- vn/ve/vd: float m/s (ArduPilot pymavlink encodes to int cm/s on wire)
- satellites_visible: 10 synthetic (prevents ArduPilot satellite-count failsafe)
- fix_type: 0=no fix, 2=2D, 3=3D per ConfidenceTier mapping
Ref: MAVProxy/modules/mavproxy_GPSInput.py, solution.md §GPS_INPUT Population
Ref: docs/superpowers/specs/2026-04-18-oss-stack-tech-audit-design.md §6
"""
import time
import numpy as np
import pytest
from gps_denied.core.mavlink import _confidence_to_fix_type, _eskf_to_gps_input
from gps_denied.schemas import GPSPoint
from gps_denied.schemas.eskf import ConfidenceTier, ESKFState
_ORIGIN = GPSPoint(lat=49.0, lon=32.0)
def _make_state(
pos=(0.0, 0.0, 0.0),
vel=(0.0, 0.0, 0.0),
confidence: ConfidenceTier = ConfidenceTier.HIGH,
cov_scale: float = 1.0,
) -> ESKFState:
return ESKFState(
position=np.array(pos, dtype=float),
velocity=np.array(vel, dtype=float),
quaternion=np.array([1.0, 0.0, 0.0, 0.0]),
accel_bias=np.zeros(3),
gyro_bias=np.zeros(3),
covariance=np.eye(15) * cov_scale,
timestamp=time.time(),
confidence=confidence,
)
def test_gps_input_lat_lon_encoded_as_deg_e7():
"""lat/lon must be int × 1e7 (degE7) per MAVLink #232 spec."""
state = _make_state(pos=(0.0, 0.0, 0.0))
msg = _eskf_to_gps_input(state, _ORIGIN, altitude_m=600.0)
# At origin with zero position, lat/lon should match origin exactly.
assert msg.lat == int(49.0 * 1e7)
assert msg.lon == int(32.0 * 1e7)
assert isinstance(msg.lat, int)
assert isinstance(msg.lon, int)
def test_gps_input_lat_lon_offset_from_enu_position():
"""ENU position is correctly converted to lat/lon offset."""
# 111.319.5 m northward = ~0.001 deg latitude
state = _make_state(pos=(0.0, 111_319.5, 0.0))
msg = _eskf_to_gps_input(state, _ORIGIN, altitude_m=600.0)
# 1 deg north
assert msg.lat == pytest.approx(int((49.0 + 1.0) * 1e7), abs=100)
# lon unchanged
assert msg.lon == int(32.0 * 1e7)
def test_gps_input_alt_in_meters_msl():
"""alt field is float metres MSL (not mm as in some MAVLink variants)."""
state = _make_state()
msg = _eskf_to_gps_input(state, _ORIGIN, altitude_m=600.5)
assert msg.alt == pytest.approx(600.5)
assert isinstance(msg.alt, float)
def test_gps_input_velocity_enu_to_ned_conversion():
"""ENU velocity must be converted to NED before populating vn/ve/vd."""
# ENU velocity: East=+5, North=+10, Up=+2
state = _make_state(vel=(5.0, 10.0, 2.0))
msg = _eskf_to_gps_input(state, _ORIGIN, altitude_m=600.0)
# NED: vn=North=+10, ve=East=+5, vd=-Up=-2
assert msg.vn == pytest.approx(10.0, abs=0.01)
assert msg.ve == pytest.approx(5.0, abs=0.01)
assert msg.vd == pytest.approx(-2.0, abs=0.01)
def test_gps_input_satellites_visible_synthetic_10():
"""satellites_visible=10 prevents ArduPilot satellite-count failsafe."""
state = _make_state()
msg = _eskf_to_gps_input(state, _ORIGIN, altitude_m=600.0)
assert msg.satellites_visible == 10
def test_gps_input_fix_type_high_confidence_is_3d():
"""HIGH confidence (satellite-anchored) → fix_type=3 (3D fix)."""
state = _make_state(confidence=ConfidenceTier.HIGH)
msg = _eskf_to_gps_input(state, _ORIGIN, altitude_m=600.0)
assert msg.fix_type == 3
def test_gps_input_fix_type_medium_confidence_is_3d():
"""MEDIUM confidence (VO tracking valid) → fix_type=3 per solution.md."""
state = _make_state(confidence=ConfidenceTier.MEDIUM)
msg = _eskf_to_gps_input(state, _ORIGIN, altitude_m=600.0)
assert msg.fix_type == 3
def test_gps_input_fix_type_low_confidence_no_fix():
"""LOW confidence (IMU-only) → fix_type=0 (no fix, honest signal)."""
state = _make_state(confidence=ConfidenceTier.LOW)
msg = _eskf_to_gps_input(state, _ORIGIN, altitude_m=600.0)
assert msg.fix_type == 0
def test_gps_input_fix_type_failed_no_fix():
"""FAILED confidence → fix_type=0 (no fix)."""
state = _make_state(confidence=ConfidenceTier.FAILED)
msg = _eskf_to_gps_input(state, _ORIGIN, altitude_m=600.0)
assert msg.fix_type == 0
def test_gps_input_accuracy_from_covariance():
"""horiz_accuracy derived from covariance[0:2, 0:2]; vert from [2,2]."""
state = _make_state(cov_scale=4.0) # σ_h² = 4+4=8, σ_v² = 4
msg = _eskf_to_gps_input(state, _ORIGIN, altitude_m=600.0)
expected_h = (8.0) ** 0.5 # sqrt(var_x + var_y)
expected_v = (4.0) ** 0.5
assert msg.horiz_accuracy == pytest.approx(expected_h, abs=0.01)
assert msg.vert_accuracy == pytest.approx(expected_v, abs=0.01)
def test_gps_input_hdop_vdop_clamped_to_min():
"""hdop/vdop have a floor of 0.1 to prevent ArduPilot divide-by-zero."""
state = _make_state(cov_scale=0.0) # σ = 0 → hdop would be 0 without clamp
msg = _eskf_to_gps_input(state, _ORIGIN, altitude_m=600.0)
assert msg.hdop >= 0.1
assert msg.vdop >= 0.1
# ---------------------------------------------------------------------------
# _confidence_to_fix_type direct mapping
# ---------------------------------------------------------------------------
def test_confidence_tier_mapping_complete():
"""All four ConfidenceTier values map to valid fix_type values."""
assert _confidence_to_fix_type(ConfidenceTier.HIGH) == 3
assert _confidence_to_fix_type(ConfidenceTier.MEDIUM) == 3
assert _confidence_to_fix_type(ConfidenceTier.LOW) == 0
assert _confidence_to_fix_type(ConfidenceTier.FAILED) == 0