From 44f96d6d2df5a9b71fea8d61c71a270d6001dbb1 Mon Sep 17 00:00:00 2001 From: Yuzviak Date: Sat, 18 Apr 2026 16:24:38 +0300 Subject: [PATCH] test(mavlink): add GPS_INPUT field encoding unit tests MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- tests/test_gps_input_encoding.py | 154 +++++++++++++++++++++++++++++++ 1 file changed, 154 insertions(+) create mode 100644 tests/test_gps_input_encoding.py diff --git a/tests/test_gps_input_encoding.py b/tests/test_gps_input_encoding.py new file mode 100644 index 0000000..4f12dc3 --- /dev/null +++ b/tests/test_gps_input_encoding.py @@ -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