mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-23 00:51:13 +00:00
[AZ-697] [AZ-702] tlog GPS truth + KHP20S30 factory calibration
Batch 98 (cycle 2) — first two PBIs of epic AZ-696 (real-flight validation harness): AZ-697: direct binary-tlog GPS-truth extractor - New src/gps_denied_onboard/replay_input/tlog_ground_truth.py reads GLOBAL_POSITION_INT (with GPS_RAW_INT fallback) from a binary ArduPilot tlog via pymavlink.mavutil and returns a frozen+slotted TlogGroundTruth DTO with per-record ts_ns / lat_deg / lon_deg / alt_m / hdg_deg / vx_m_s / vy_m_s / vz_m_s. - Promoted l2_horizontal_m + match_percentage + GroundTruthRow from tests/e2e/replay/_helpers.py into the new production module src/gps_denied_onboard/helpers/gps_compare.py. The e2e helper now re-exports the same objects (identity, not copies) so existing test imports continue working untouched. - tests/e2e/replay/conftest.py prefers the real derkachi.tlog when present, falls back to the CSV synth path otherwise. - 22 new unit tests cover AC-1..AC-5 (mypy --strict subprocess test included). All passing. AZ-702: Topotek KHP20S30 factory-sheet camera calibration - New _docs/00_problem/input_data/flight_derkachi/khp20s30_factory.json: fx = fy = 4644.444, cx = 960, cy = 540, HFOV ~ 23.3 deg, VFOV ~ 13.2 deg, computed from the published 8.5 mm focal length + 1/2.8" sensor + 1920x1080 capture at lowest zoom step. Distortion zeroed, body_to_camera_se3 = identity with nadir convention. Acquisition method explicitly recorded as factory_sheet so downstream code can expect higher residual error than a lab calibration. - _docs/00_problem/input_data/flight_derkachi/camera_info.md updated to document the assumptions, expected residual error window, and conftest pick-up rule. - tests/e2e/replay/conftest.py::_calibration_path() prefers khp20s30_factory.json when present, falls back to adti26.json. - 9 new unit tests cover AC-1..AC-4 (schema, intrinsics traceback, doc reference, conftest pick-up). All passing. Test run: 45 new tests, all passing. Full-suite gate deferred to Step 16 (after the last batch in cycle 2 per the implement skill). Adjacent note (not fixed in this batch, recorded in the batch report): auto_sync.py has the same redundant pymavlink type:ignore + a few numpy/cv2 mypy --strict issues. None on this batch's path. Refs: _docs/03_implementation/batch_98_cycle2_report.md Refs: _docs/02_tasks/done/AZ-697_tlog_ground_truth_extractor.md Refs: _docs/02_tasks/done/AZ-702_khp20s30_calibration.md Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -0,0 +1,247 @@
|
||||
"""Direct binary-tlog GPS-truth extractor (AZ-697 / E-DEMO-REPLAY).
|
||||
|
||||
Streams ``GLOBAL_POSITION_INT`` (preferred) or ``GPS_RAW_INT`` (fallback)
|
||||
from an ArduPilot binary tlog into a typed :class:`TlogGroundTruth` DTO,
|
||||
suitable for the AZ-699 (real-flight validation) and AZ-701 (HTTP
|
||||
replay API) comparison paths.
|
||||
|
||||
Design mirrors :mod:`gps_denied_onboard.replay_input.auto_sync`:
|
||||
|
||||
* 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 / AZ-405 pattern).
|
||||
* Production helper only — 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)
|
||||
Reference in New Issue
Block a user