mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-04-22 07:06:38 +00:00
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:
@@ -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
|
||||
Reference in New Issue
Block a user