azaion/gps-denied-onboard
1
0
Fork 0
mirror of https://github.com/azaion/gps-denied-onboard.git synced 2026-06-22 21:51:12 +00:00
Code Issues Packages Projects Releases Wiki Activity

[AZ-697] [AZ-702] tlog GPS truth + KHP20S30 factory calibration

Browse Source 
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
2026-05-20 16:09:03 +03:00
parent a12638dd92
commit 64d961f60c
16 changed files with 1503 additions and 134 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tests/e2e/replay/_helpers.py
+15 -105
View File
@@ -1,18 +1,22 @@
"""Helpers shared by the AZ-404 E2E replay tests.
The numerical kernels (``l2_horizontal_m``, ``match_percentage``,
``GroundTruthRow``) moved into production code at
:mod:`gps_denied_onboard.helpers.gps_compare` in AZ-697; they're
re-exported here so existing import sites stay stable.
* :func:`parse_jsonl` — read the ``JsonlReplaySink`` output into a list
of dicts with one entry per emit.
* :func:`l2_horizontal_m` — WGS84-aware L2 horizontal distance between
two ``(lat, lon)`` pairs in metres.
* :func:`match_percentage` — share of estimator emissions whose
L2 distance to the closest ground-truth row is within a threshold.
* :class:`CapturingMavlinkTransport` — test-only ``MavlinkTransport``
impl that records every ``write`` so AC-4b can compare the byte
streams produced by ``compose_root(config_live)`` vs.
``compose_root(config_replay)``.
* :func:`load_ground_truth_csv` — the IMU CSV's ``GLOBAL_POSITION_INT``
columns ARE the AC-3 reference (the original tlog's GPS rows
exported to CSV); this helper materialises them.
exported to CSV); this helper materialises them. Retained for the
CSV-only fallback path; the real-tlog branch uses
:func:`gps_denied_onboard.replay_input.load_tlog_ground_truth`
instead.
All functions are pure / deterministic and stay safely importable on
dev macOS without ``RUN_REPLAY_E2E``; the regular regression suite
@@ -24,11 +28,15 @@ from __future__ import annotations
import csv
import json
import math
from dataclasses import dataclass
from pathlib import Path
from typing import Any
from gps_denied_onboard.helpers.gps_compare import (
GroundTruthRow,
l2_horizontal_m,
match_percentage,
)
__all__ = [
"CapturingMavlinkTransport",
"GroundTruthRow",
@@ -39,22 +47,6 @@ __all__ = [
]
# WGS84 mean Earth radius. Matches the value used by
# `helpers/wgs_converter.py` (AZ-279) so the e2e check is consistent
# with the production converter.
_EARTH_RADIUS_M: float = 6_371_008.8
@dataclass(frozen=True)
class GroundTruthRow:
"""One row from the Derkachi data_imu.csv ground-truth slice."""
t_s: float
lat_deg: float
lon_deg: float
alt_m: float
def parse_jsonl(path: Path) -> list[dict[str, Any]]:
"""Return one dict per line of a JsonlReplaySink output file.
@@ -77,29 +69,6 @@ def parse_jsonl(path: Path) -> list[dict[str, Any]]:
return records
def l2_horizontal_m(
lat1_deg: float, lon1_deg: float, lat2_deg: float, lon2_deg: float
) -> float:
"""WGS84-spherical great-circle distance in metres.
Uses the haversine formula with the C5/AZ-279 mean Earth radius.
Sufficient for the AC-3 ≤ 100 m threshold (sub-metre accuracy at
the Derkachi latitude band; the spherical approximation diverges
from the WGS84 ellipsoid by < 0.5 % at these latitudes — well
within the AC-3 budget).
"""
phi1 = math.radians(lat1_deg)
phi2 = math.radians(lat2_deg)
dphi = phi2 - phi1
dlam = math.radians(lon2_deg - lon1_deg)
a = (
math.sin(dphi / 2.0) ** 2
+ math.cos(phi1) * math.cos(phi2) * math.sin(dlam / 2.0) ** 2
)
c = 2.0 * math.asin(min(1.0, math.sqrt(a)))
return _EARTH_RADIUS_M * c
def load_ground_truth_csv(csv_path: Path) -> list[GroundTruthRow]:
"""Load the Derkachi IMU CSV's GPS rows as ground truth.
@@ -123,65 +92,6 @@ def load_ground_truth_csv(csv_path: Path) -> list[GroundTruthRow]:
return rows
def match_percentage(
emissions: list[dict[str, Any]],
ground_truth: list[GroundTruthRow],
*,
threshold_m: float,
) -> float:
"""Share of emissions within ``threshold_m`` of the closest GT row.
For each emitted ``EstimatorOutput`` JSONL record, find the
nearest-in-time ground-truth row, compute the horizontal L2
distance, and count it as a hit when ≤ ``threshold_m``. Returns
the hit ratio in [0.0, 1.0].
Nearest-in-time is sufficient because the IMU CSV's 10 Hz cadence
(matching the C5 emit rate) means the candidate row is typically
< 50 ms off the emit timestamp — well below the AC-3 100 m budget.
"""
if not emissions:
return 0.0
if not ground_truth:
raise AssertionError("ground_truth must be non-empty")
gt_sorted = sorted(ground_truth, key=lambda r: r.t_s)
gt_times = [r.t_s for r in gt_sorted]
hits = 0
for emit in emissions:
emit_ts_ns = int(emit["emitted_at"])
emit_t_s = emit_ts_ns / 1e9
idx = _bisect_left(gt_times, emit_t_s)
candidates = []
if idx > 0:
candidates.append(gt_sorted[idx - 1])
if idx < len(gt_sorted):
candidates.append(gt_sorted[idx])
# Nearest-in-time row.
nearest = min(candidates, key=lambda r: abs(r.t_s - emit_t_s))
emit_pos = emit["position_wgs84"]
d = l2_horizontal_m(
emit_pos["lat_deg"],
emit_pos["lon_deg"],
nearest.lat_deg,
nearest.lon_deg,
)
if d <= threshold_m:
hits += 1
return hits / len(emissions)
def _bisect_left(seq: list[float], target: float) -> int:
"""Stdlib bisect_left, inlined to keep import surface narrow."""
lo, hi = 0, len(seq)
while lo < hi:
mid = (lo + hi) // 2
if seq[mid] < target:
lo = mid + 1
else:
hi = mid
return lo
class CapturingMavlinkTransport:
"""Test-only :class:`MavlinkTransport` that records every write.
tests/e2e/replay/conftest.py
+56 -22
View File
@@ -21,18 +21,21 @@ from typing import Any
import pytest
from gps_denied_onboard.replay_input import load_tlog_ground_truth
from tests.e2e.replay._helpers import GroundTruthRow, load_ground_truth_csv
from tests.e2e.replay._tlog_synth import synthesize_tlog
# Derkachi clip range — anchored at the start of the data_imu.csv
# (Time=0.0). The fixture clip is deliberately the first 60 s rather
# than a mid-flight slice: the take-off region exercises the AZ-405
# IMU-take-off auto-sync detector, and the steady cruise that follows
# stresses the satellite-anchor + VIO drift-correction path. The
# trim is documented in `tests/e2e/replay/README.md`.
_CLIP_START_S: float = 0.0
_CLIP_END_S: float = 60.0
# Derkachi clip range — 60 s starting at the start of the GT series.
# For the CSV-synth fallback, the series begins at Time=0.0; for the
# real-tlog branch, the series begins at the wall-clock timestamp of
# the first GPS message (and the clip becomes [t0, t0 + 60]). The
# fixture clip is deliberately the first 60 s rather than a mid-flight
# slice: the take-off region exercises the AZ-405 IMU-take-off
# auto-sync detector, and the steady cruise that follows stresses the
# satellite-anchor + VIO drift-correction path. The trim is documented
# in `tests/e2e/replay/README.md`.
_CLIP_DURATION_S: float = 60.0
# ----------------------------------------------------------------------
@@ -48,11 +51,15 @@ def _derkachi_dir() -> Path:
def _calibration_path() -> Path:
# Placeholder calibration: the real Topotek KHP20S30 intrinsics
# are unknown per `_docs/00_problem/input_data/flight_derkachi/
# camera_info.md`. AC-3 is `xfail`ed until a real calibration
# ships; AC-1 / AC-2 / AC-5 / AC-6 do not depend on intrinsics
# accuracy.
# AZ-702 ships a factory-sheet approximation for the Topotek
# KHP20S30 nadir camera at
# `_docs/00_problem/input_data/flight_derkachi/khp20s30_factory.json`.
# When present we use it; otherwise we fall back to the
# `adti26.json` placeholder so the AC-1/2/5/6 path stays
# exercisable on dev macOS without the AZ-702 deliverable.
factory_path = _derkachi_dir() / "khp20s30_factory.json"
if factory_path.is_file():
return factory_path
return _repo_root() / "tests" / "fixtures" / "calibration" / "adti26.json"
@@ -87,17 +94,45 @@ def derkachi_replay_inputs(tmp_path_factory: pytest.TempPathFactory) -> Derkachi
derkachi = _derkachi_dir()
csv_path = derkachi / "data_imu.csv"
video_path = derkachi / "flight_derkachi.mp4"
if not csv_path.is_file():
pytest.fail(
f"Derkachi fixture missing: {csv_path} — see "
"_docs/00_problem/input_data/flight_derkachi/README.md"
)
real_tlog_path = derkachi / "derkachi.tlog"
if not video_path.is_file():
pytest.fail(f"Derkachi fixture missing: {video_path}")
work_dir = tmp_path_factory.mktemp("derkachi")
tlog_path = work_dir / "synth.tlog"
synthesize_tlog(csv_path, tlog_path)
# AZ-697: prefer the real binary tlog when present; fall back to
# synthesizing one from the CSV so dev environments without the
# 5.8 MB binary blob still exercise the e2e path.
if real_tlog_path.is_file():
tlog_path = real_tlog_path
gt_series = load_tlog_ground_truth(real_tlog_path).records
if gt_series:
t0_s = gt_series[0].ts_ns / 1e9
ground_truth_full = [
GroundTruthRow(
t_s=fix.ts_ns / 1e9,
lat_deg=fix.lat_deg,
lon_deg=fix.lon_deg,
alt_m=fix.alt_m,
)
for fix in gt_series
]
clip_start_s = t0_s
clip_end_s = t0_s + _CLIP_DURATION_S
else:
ground_truth_full = []
clip_start_s = 0.0
clip_end_s = _CLIP_DURATION_S
else:
if not csv_path.is_file():
pytest.fail(
f"Derkachi fixture missing: {csv_path} — see "
"_docs/00_problem/input_data/flight_derkachi/README.md"
)
tlog_path = work_dir / "synth.tlog"
synthesize_tlog(csv_path, tlog_path)
ground_truth_full = load_ground_truth_csv(csv_path)
clip_start_s = 0.0
clip_end_s = _CLIP_DURATION_S
# Empty signing key — the airborne replay path runs the signing
# handshake against `NoopMavlinkTransport`, so the key contents do
@@ -118,9 +153,8 @@ def derkachi_replay_inputs(tmp_path_factory: pytest.TempPathFactory) -> Derkachi
output_path = work_dir / "estimator_output.jsonl"
ground_truth_full = load_ground_truth_csv(csv_path)
ground_truth = [
r for r in ground_truth_full if _CLIP_START_S <= r.t_s <= _CLIP_END_S
r for r in ground_truth_full if clip_start_s <= r.t_s <= clip_end_s
]
return DerkachiReplayInputs(