"""Direct binary-tlog GPS-truth extractor (AZ-697 / E-DEMO-REPLAY). AUDIT-ONLY (AZ-895): the production replay pipeline now consumes ground truth through :class:`CsvGroundTruth` (AZ-894) driven by the operator's IMU+GPS CSV. This helper is retained for one-off audits and the AZ-699 / AZ-701 validation paths that still operate against legacy ``.tlog`` archives; it is not part of the main replay composition root. Streams ``GLOBAL_POSITION_INT`` (preferred) or ``GPS_RAW_INT`` (fallback) from an ArduPilot binary tlog into a typed :class:`TlogGroundTruth` DTO. Design notes: * Lazy ``pymavlink.mavutil`` import — missing dependency raises :class:`ReplayInputAdapterError` rather than crashing the import. * Optional ``source_factory`` injection point so unit tests can swap in a synthetic source (mirrors the AZ-399 pattern). * Placed under ``replay_input/`` because the GPS extraction is intrinsically tied to the tlog input pipeline; the comparison kernels themselves live in :mod:`helpers.gps_compare`. """ from __future__ import annotations import logging import math from collections.abc import Callable from dataclasses import dataclass from pathlib import Path from typing import Any from gps_denied_onboard.replay_input.errors import ReplayInputAdapterError __all__ = [ "TlogGpsFix", "TlogGroundTruth", "load_tlog_ground_truth", ] _LOGGER = logging.getLogger("gps_denied_onboard.replay_input.tlog_ground_truth") # MAVLink GLOBAL_POSITION_INT / GPS_RAW_INT integer encodings. # lat/lon are deg × 1e7; alt is mm above MSL; vx/vy/vz are cm/s; # hdg/cog are cdeg (0..36000). _LATLON_SCALE: float = 1.0e-7 _MM_PER_M: float = 1000.0 _CM_PER_M_S: float = 100.0 _CDEG_PER_DEG: float = 100.0 # Source-label constants returned in :attr:`TlogGroundTruth.source`. _SOURCE_GLOBAL_POSITION_INT: str = "GLOBAL_POSITION_INT" _SOURCE_GPS_RAW_INT: str = "GPS_RAW_INT" _SOURCE_NONE: str = "" @dataclass(frozen=True, slots=True) class TlogGpsFix: """One time-aligned GPS-truth row extracted from a tlog. Attributes: ts_ns: Absolute timestamp (ns) sourced from pymavlink's ``_timestamp`` field (Unix time × 1e9). Comparable to the airborne runtime clock during replay. lat_deg, lon_deg: Latitude / longitude in degrees (WGS84). alt_m: Altitude above MSL in metres (MAVLink ``alt`` field). hdg_deg: Aircraft heading in degrees [0, 360). When sourced from ``GPS_RAW_INT``, this is course over ground (cog), not the IMU-derived heading. vx_m_s, vy_m_s, vz_m_s: North / east / down velocity in m/s. For ``GPS_RAW_INT``-sourced rows, ``vx`` / ``vy`` are derived from the ground velocity + course over ground; ``vz`` is 0.0 because the message does not expose vertical velocity. """ ts_ns: int lat_deg: float lon_deg: float alt_m: float hdg_deg: float vx_m_s: float vy_m_s: float vz_m_s: float @dataclass(frozen=True, slots=True) class TlogGroundTruth: """Ground-truth GPS series extracted from a tlog. Attributes: records: Time-ordered fixes. Empty when no GPS messages were present in the tlog. source: MAVLink message type the records were sourced from — ``"GLOBAL_POSITION_INT"`` (preferred), ``"GPS_RAW_INT"`` (fallback), or ``""`` (no GPS messages found). """ records: tuple[TlogGpsFix, ...] source: str def load_tlog_ground_truth( tlog_path: Path, *, source_factory: Callable[[str], Any] | None = None, ) -> TlogGroundTruth: """Stream GPS-truth records from a tlog. Args: tlog_path: Path to the binary tlog. Existence is checked at entry. source_factory: Test-only injection — when provided, replaces the pymavlink open call with the factory's return value. The factory must yield an object with ``recv_match`` and ``close`` semantics matching pymavlink's ``mavutil.mavlink_connection``. Returns: A :class:`TlogGroundTruth` whose ``records`` contain ``GLOBAL_POSITION_INT`` rows when any are present; otherwise ``GPS_RAW_INT`` rows; otherwise an empty tuple (with a WARN log). Raises: ReplayInputAdapterError: When the tlog file is missing or pymavlink cannot be imported. """ if not tlog_path.is_file(): raise ReplayInputAdapterError(f"tlog file not found: {tlog_path}") source = _open_tlog(tlog_path, source_factory=source_factory) gpi_records: list[TlogGpsFix] = [] raw_records: list[TlogGpsFix] = [] try: while True: try: msg = source.recv_match( type=[_SOURCE_GLOBAL_POSITION_INT, _SOURCE_GPS_RAW_INT], blocking=False, ) except Exception as exc: # pragma: no cover — defensive. raise ReplayInputAdapterError( f"tlog scan failed on {tlog_path}: {exc!r}" ) from exc if msg is None: break msg_type = _safe_msg_type(msg) if not msg_type: continue ts_ns = _msg_timestamp_ns(msg) if msg_type == _SOURCE_GLOBAL_POSITION_INT: gpi_records.append(_from_global_position_int(msg, ts_ns)) elif msg_type == _SOURCE_GPS_RAW_INT: raw_records.append(_from_gps_raw_int(msg, ts_ns)) finally: if hasattr(source, "close"): try: source.close() except Exception: # pragma: no cover — defensive. pass if gpi_records: return TlogGroundTruth( records=tuple(gpi_records), source=_SOURCE_GLOBAL_POSITION_INT, ) if raw_records: return TlogGroundTruth( records=tuple(raw_records), source=_SOURCE_GPS_RAW_INT, ) _LOGGER.warning( "tlog %s contains no GLOBAL_POSITION_INT or GPS_RAW_INT messages", tlog_path, ) return TlogGroundTruth(records=(), source=_SOURCE_NONE) def _from_global_position_int(msg: Any, ts_ns: int) -> TlogGpsFix: return TlogGpsFix( ts_ns=ts_ns, lat_deg=int(getattr(msg, "lat", 0)) * _LATLON_SCALE, lon_deg=int(getattr(msg, "lon", 0)) * _LATLON_SCALE, alt_m=int(getattr(msg, "alt", 0)) / _MM_PER_M, hdg_deg=int(getattr(msg, "hdg", 0)) / _CDEG_PER_DEG, vx_m_s=int(getattr(msg, "vx", 0)) / _CM_PER_M_S, vy_m_s=int(getattr(msg, "vy", 0)) / _CM_PER_M_S, vz_m_s=int(getattr(msg, "vz", 0)) / _CM_PER_M_S, ) def _from_gps_raw_int(msg: Any, ts_ns: int) -> TlogGpsFix: # GPS_RAW_INT exposes ground velocity + course over ground rather # than NED components. Derive horizontal components; leave vertical # at 0.0 because the message lacks a vz field. Callers that need # vertical velocity from GPS_RAW_INT must source it elsewhere # (e.g., VFR_HUD.climb). vel_cm_s = int(getattr(msg, "vel", 0)) cog_cdeg = int(getattr(msg, "cog", 0)) cog_rad = math.radians(cog_cdeg / _CDEG_PER_DEG) vel_m_s = vel_cm_s / _CM_PER_M_S vx_m_s = vel_m_s * math.cos(cog_rad) vy_m_s = vel_m_s * math.sin(cog_rad) return TlogGpsFix( ts_ns=ts_ns, lat_deg=int(getattr(msg, "lat", 0)) * _LATLON_SCALE, lon_deg=int(getattr(msg, "lon", 0)) * _LATLON_SCALE, alt_m=int(getattr(msg, "alt", 0)) / _MM_PER_M, hdg_deg=cog_cdeg / _CDEG_PER_DEG, vx_m_s=vx_m_s, vy_m_s=vy_m_s, vz_m_s=0.0, ) def _open_tlog( tlog_path: Path, *, source_factory: Callable[[str], Any] | None, ) -> Any: if source_factory is not None: return source_factory(str(tlog_path)) try: from pymavlink import mavutil except ImportError as exc: raise ReplayInputAdapterError( "pymavlink is required for replay tlog ground-truth " "extraction but is not importable in this binary" ) from exc return mavutil.mavlink_connection( str(tlog_path), dialect="ardupilotmega", mavlink_version="2.0", ) def _safe_msg_type(msg: Any) -> str: try: if hasattr(msg, "get_type"): return str(msg.get_type()) except Exception: return "" return type(msg).__name__ def _msg_timestamp_ns(msg: Any) -> int: raw = getattr(msg, "_timestamp", None) if raw is None: raise ReplayInputAdapterError( "tlog message missing _timestamp attribute; pymavlink " "mavlogfile should populate it on every recv_match() return" ) return int(float(raw) * 1_000_000_000)