mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 23:41:13 +00:00
[AZ-420] Batch 81: FT-P-12 + FT-P-13 GCS scenarios
FT-P-12: parse mavproxy-listener tlog over a 60 s Derkachi replay and assert SUT->GCS GLOBAL_POSITION_INT cadence lands in [1, 2] Hz (AC-6.1). FT-P-13: inject `RELOC:<lat>,<lon>,<radius_m>` STATUSTEXT while the SUT is in dead_reckoned; verify FDR `c8.gcs.operator_command` ack <=2s, `anchor_search_region` centre shifts toward the hint, and no BAD_SIGNATURE / UNAUTHORIZED / REJECTED STATUSTEXT lands in the post-inject window (AC-6.2). Adds runner.helpers.gcs_telemetry_evaluator (rate, hint-ack correlation, haversine search-region shift, rejection scan) and sitl_observer.capture_gcs_tlog (parity surface to capture_ap_tlog). Pure-logic coverage: 39 new unit tests; full e2e/_unit_tests/ suite 746 passing (was 700). Scenarios skip locally on missing SITL replay fixture; production hooks (inbound STATUSTEXT parser, anchor_search_region FDR emitter) tracked outside this task. See _docs/03_implementation/batch_81_report.md + reviews/batch_81_review.md. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -0,0 +1,429 @@
|
||||
"""GCS telemetry evaluation for FT-P-12 + FT-P-13 (AZ-420 / AC-6.1, AC-6.2).
|
||||
|
||||
Two evaluators sourced from the GCS-side ``.tlog`` captured by
|
||||
``mavproxy-listener`` plus the FDR archive:
|
||||
|
||||
* **FT-P-12 / AC-6.1**: SUT→GCS summary cadence must land in [1, 2] Hz
|
||||
over the 60 s replay window. The SUT's C8 ``QgcTelemetryAdapter`` pairs
|
||||
``GLOBAL_POSITION_INT`` + ``NAMED_VALUE_FLOAT`` at the configured
|
||||
``summary_rate_hz``; we count ``GLOBAL_POSITION_INT`` bursts since the
|
||||
``NAMED_VALUE_FLOAT`` companion is decorative.
|
||||
* **FT-P-13 / AC-6.2**: GCS-originated ``STATUSTEXT`` carrying an operator
|
||||
re-loc hint:
|
||||
* acknowledgement latency from inject → FDR ``c8.gcs.operator_command``
|
||||
record must be ≤ 2 s (AC-2);
|
||||
* the next per-frame ``anchor_search_region`` FDR record's centre must
|
||||
move closer to the hinted location than the last pre-hint region
|
||||
(AC-3);
|
||||
* no ``BAD_SIGNATURE`` / ``UNAUTHORIZED`` STATUSTEXT may appear in the
|
||||
rejection window after the hint (AC-4).
|
||||
|
||||
All inputs are pure iterables / sequences. The tlog ingestion is
|
||||
delegated to ``runner.helpers.mavproxy_tlog_reader.iter_messages`` and
|
||||
the FDR ingestion to ``runner.helpers.fdr_reader.iter_records``.
|
||||
|
||||
Public-boundary discipline: this module does NOT import any
|
||||
``src/gps_denied_onboard`` symbol.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from typing import Iterable, Sequence
|
||||
|
||||
from .mavproxy_tlog_reader import TlogMessage
|
||||
|
||||
GCS_SUMMARY_RATE_MIN_HZ = 1.0
|
||||
GCS_SUMMARY_RATE_MAX_HZ = 2.0
|
||||
GCS_SUMMARY_POSITION_MSG_TYPE = "GLOBAL_POSITION_INT"
|
||||
GCS_SUMMARY_COMPANION_MSG_TYPE = "NAMED_VALUE_FLOAT"
|
||||
|
||||
HINT_ACK_MAX_LATENCY_MS = 2000.0
|
||||
HINT_FDR_KIND = "c8.gcs.operator_command"
|
||||
HINT_REJECTION_STATUSTEXT_TOKENS = ("BAD_SIGNATURE", "UNAUTHORIZED", "REJECTED")
|
||||
|
||||
ANCHOR_SEARCH_REGION_FDR_KIND = "anchor_search_region"
|
||||
|
||||
_EARTH_RADIUS_M = 6_371_008.8
|
||||
|
||||
|
||||
# ─────────────────────── FT-P-12 / AC-6.1 ───────────────────────
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class GcsSummaryRateReport:
|
||||
"""AC-6.1: SUT→GCS summary cadence over the replay window."""
|
||||
|
||||
total_summary_messages: int
|
||||
window_us: int
|
||||
observed_rate_hz: float
|
||||
min_required_hz: float = GCS_SUMMARY_RATE_MIN_HZ
|
||||
max_required_hz: float = GCS_SUMMARY_RATE_MAX_HZ
|
||||
|
||||
@property
|
||||
def passes(self) -> bool:
|
||||
if self.window_us <= 0:
|
||||
return False
|
||||
return self.min_required_hz <= self.observed_rate_hz <= self.max_required_hz
|
||||
|
||||
|
||||
def compute_gcs_summary_rate(
|
||||
messages: Iterable[TlogMessage],
|
||||
*,
|
||||
position_msg_type: str = GCS_SUMMARY_POSITION_MSG_TYPE,
|
||||
min_required_hz: float = GCS_SUMMARY_RATE_MIN_HZ,
|
||||
max_required_hz: float = GCS_SUMMARY_RATE_MAX_HZ,
|
||||
) -> GcsSummaryRateReport:
|
||||
"""AC-6.1: rate of ``GLOBAL_POSITION_INT`` messages emitted to the GCS.
|
||||
|
||||
Each SUT→GCS summary "burst" is one ``GLOBAL_POSITION_INT`` paired
|
||||
with one ``NAMED_VALUE_FLOAT(horiz_m)`` per the C8 ``QgcTelemetryAdapter``
|
||||
implementation; only the position message is counted to avoid
|
||||
double-counting the decorative companion.
|
||||
|
||||
Rate is computed over the (first, last) timestamp span — i.e.,
|
||||
``(N-1) / window_seconds`` — to match ``compute_gps_input_rate`` in
|
||||
``ap_contract_evaluator``.
|
||||
"""
|
||||
if min_required_hz < 0:
|
||||
raise ValueError(f"min_required_hz must be ≥0, got {min_required_hz}")
|
||||
if max_required_hz < min_required_hz:
|
||||
raise ValueError(
|
||||
f"max_required_hz ({max_required_hz}) must be ≥ "
|
||||
f"min_required_hz ({min_required_hz})"
|
||||
)
|
||||
|
||||
timestamps = [m.timestamp_us for m in messages if m.msg_type == position_msg_type]
|
||||
if len(timestamps) < 2:
|
||||
return GcsSummaryRateReport(
|
||||
total_summary_messages=len(timestamps),
|
||||
window_us=0,
|
||||
observed_rate_hz=0.0,
|
||||
min_required_hz=min_required_hz,
|
||||
max_required_hz=max_required_hz,
|
||||
)
|
||||
window_us = timestamps[-1] - timestamps[0]
|
||||
if window_us <= 0:
|
||||
return GcsSummaryRateReport(
|
||||
total_summary_messages=len(timestamps),
|
||||
window_us=window_us,
|
||||
observed_rate_hz=0.0,
|
||||
min_required_hz=min_required_hz,
|
||||
max_required_hz=max_required_hz,
|
||||
)
|
||||
observed_hz = (len(timestamps) - 1) / (window_us / 1_000_000.0)
|
||||
return GcsSummaryRateReport(
|
||||
total_summary_messages=len(timestamps),
|
||||
window_us=window_us,
|
||||
observed_rate_hz=observed_hz,
|
||||
min_required_hz=min_required_hz,
|
||||
max_required_hz=max_required_hz,
|
||||
)
|
||||
|
||||
|
||||
# ─────────────────────── FT-P-13 / AC-6.2 ───────────────────────
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class InboundHint:
|
||||
"""A GCS-originated re-loc hint observed inbound on the SUT side.
|
||||
|
||||
Sourced from a ``STATUSTEXT`` MAVLink message captured in the GCS
|
||||
tlog. ``hint_text`` is the raw payload (the operator's hint string).
|
||||
"""
|
||||
|
||||
inject_timestamp_us: int
|
||||
hint_text: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class FdrCommandAck:
|
||||
"""An FDR record acknowledging the inbound operator command.
|
||||
|
||||
Sourced from ``kind='log'`` records whose payload ``kv.kind`` equals
|
||||
``c8.gcs.operator_command`` (the kind the QGC adapter emits when it
|
||||
translates an inbound command into an ``OperatorCommand`` DTO).
|
||||
"""
|
||||
|
||||
ack_timestamp_us: int
|
||||
payload_kv: dict
|
||||
|
||||
|
||||
def correlate_hint_acks(
|
||||
hints: Sequence[InboundHint],
|
||||
acks: Sequence[FdrCommandAck],
|
||||
) -> "HintAckReport":
|
||||
"""AC-6.2 / AC-2: pair each hint with its earliest succeeding ack.
|
||||
|
||||
Pairing is greedy in injection order. A given FDR ack can match at
|
||||
most one hint; an ack whose timestamp precedes every hint is
|
||||
ignored (it cannot be an ack for those hints).
|
||||
"""
|
||||
sorted_acks = sorted(acks, key=lambda a: a.ack_timestamp_us)
|
||||
cursor = 0
|
||||
pairs: list[tuple[InboundHint, FdrCommandAck | None]] = []
|
||||
for hint in hints:
|
||||
match: FdrCommandAck | None = None
|
||||
while cursor < len(sorted_acks):
|
||||
ack = sorted_acks[cursor]
|
||||
if ack.ack_timestamp_us < hint.inject_timestamp_us:
|
||||
cursor += 1
|
||||
continue
|
||||
match = ack
|
||||
cursor += 1
|
||||
break
|
||||
pairs.append((hint, match))
|
||||
latencies: list[float | None] = []
|
||||
for hint, ack in pairs:
|
||||
if ack is None:
|
||||
latencies.append(None)
|
||||
else:
|
||||
latencies.append((ack.ack_timestamp_us - hint.inject_timestamp_us) / 1000.0)
|
||||
return HintAckReport(
|
||||
hints=tuple(hints),
|
||||
acks=tuple(sorted_acks),
|
||||
latencies_ms=tuple(latencies),
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class HintAckReport:
|
||||
"""AC-2 of FT-P-13: per-hint inject→ack latency."""
|
||||
|
||||
hints: tuple[InboundHint, ...]
|
||||
acks: tuple[FdrCommandAck, ...]
|
||||
latencies_ms: tuple[float | None, ...]
|
||||
max_required_ms: float = HINT_ACK_MAX_LATENCY_MS
|
||||
|
||||
@property
|
||||
def acked_count(self) -> int:
|
||||
return sum(1 for latency in self.latencies_ms if latency is not None)
|
||||
|
||||
@property
|
||||
def passes(self) -> bool:
|
||||
if not self.hints:
|
||||
return False
|
||||
return all(
|
||||
latency is not None and latency <= self.max_required_ms
|
||||
for latency in self.latencies_ms
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SearchRegionRecord:
|
||||
"""One ``anchor_search_region`` FDR record.
|
||||
|
||||
Schema (AC-NEW-3 family): per-frame record of the satellite-anchor
|
||||
search region the C2 backbone is currently scanning. Centre is in
|
||||
WGS84 degrees; radius is in metres.
|
||||
"""
|
||||
|
||||
monotonic_us: int
|
||||
centre_lat_deg: float
|
||||
centre_lon_deg: float
|
||||
radius_m: float
|
||||
|
||||
|
||||
def haversine_distance_m(
|
||||
lat_a_deg: float, lon_a_deg: float, lat_b_deg: float, lon_b_deg: float
|
||||
) -> float:
|
||||
"""Great-circle distance between two WGS84 points in metres.
|
||||
|
||||
Uses the spherical haversine formula with the mean Earth radius.
|
||||
Accurate to ≪1 m for the sub-100 km separations FT-P-13 cares about.
|
||||
"""
|
||||
phi_a = math.radians(lat_a_deg)
|
||||
phi_b = math.radians(lat_b_deg)
|
||||
dphi = math.radians(lat_b_deg - lat_a_deg)
|
||||
dlam = math.radians(lon_b_deg - lon_a_deg)
|
||||
a = math.sin(dphi / 2) ** 2 + math.cos(phi_a) * math.cos(phi_b) * math.sin(dlam / 2) ** 2
|
||||
c = 2 * math.asin(min(1.0, math.sqrt(a)))
|
||||
return _EARTH_RADIUS_M * c
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SearchRegionShiftReport:
|
||||
"""AC-3 of FT-P-13: did the search region shift toward the hint?"""
|
||||
|
||||
hint_lat_deg: float
|
||||
hint_lon_deg: float
|
||||
region_before: SearchRegionRecord | None
|
||||
region_after: SearchRegionRecord | None
|
||||
distance_before_m: float | None
|
||||
distance_after_m: float | None
|
||||
|
||||
@property
|
||||
def passes(self) -> bool:
|
||||
if self.region_after is None or self.distance_after_m is None:
|
||||
return False
|
||||
if self.region_before is None or self.distance_before_m is None:
|
||||
return True
|
||||
return self.distance_after_m < self.distance_before_m
|
||||
|
||||
|
||||
def evaluate_search_region_shift(
|
||||
regions: Sequence[SearchRegionRecord],
|
||||
hint_inject_timestamp_us: int,
|
||||
hint_lat_deg: float,
|
||||
hint_lon_deg: float,
|
||||
) -> SearchRegionShiftReport:
|
||||
"""AC-3: compare the last pre-hint region to the first post-hint region.
|
||||
|
||||
The "shift toward the hint" signal is positive iff the first
|
||||
region observed AFTER ``hint_inject_timestamp_us`` is closer to
|
||||
``(hint_lat_deg, hint_lon_deg)`` than the last region observed
|
||||
BEFORE the inject. If no pre-hint region exists, any post-hint
|
||||
region counts as a pass (the bias was set before the C2 backbone
|
||||
had a chance to publish anything).
|
||||
"""
|
||||
region_before: SearchRegionRecord | None = None
|
||||
region_after: SearchRegionRecord | None = None
|
||||
for region in regions:
|
||||
if region.monotonic_us < hint_inject_timestamp_us:
|
||||
region_before = region # keep moving forward to find the last pre-hint
|
||||
elif region_after is None:
|
||||
region_after = region
|
||||
distance_before = (
|
||||
haversine_distance_m(
|
||||
region_before.centre_lat_deg,
|
||||
region_before.centre_lon_deg,
|
||||
hint_lat_deg,
|
||||
hint_lon_deg,
|
||||
)
|
||||
if region_before is not None
|
||||
else None
|
||||
)
|
||||
distance_after = (
|
||||
haversine_distance_m(
|
||||
region_after.centre_lat_deg,
|
||||
region_after.centre_lon_deg,
|
||||
hint_lat_deg,
|
||||
hint_lon_deg,
|
||||
)
|
||||
if region_after is not None
|
||||
else None
|
||||
)
|
||||
return SearchRegionShiftReport(
|
||||
hint_lat_deg=hint_lat_deg,
|
||||
hint_lon_deg=hint_lon_deg,
|
||||
region_before=region_before,
|
||||
region_after=region_after,
|
||||
distance_before_m=distance_before,
|
||||
distance_after_m=distance_after,
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class HintRejectionReport:
|
||||
"""AC-4 of FT-P-13: no security/auth rejection of the well-formed hint."""
|
||||
|
||||
inject_timestamp_us: int
|
||||
window_us: int
|
||||
rejection_count: int
|
||||
rejection_texts: tuple[str, ...]
|
||||
|
||||
@property
|
||||
def passes(self) -> bool:
|
||||
return self.rejection_count == 0
|
||||
|
||||
|
||||
def detect_hint_rejection(
|
||||
messages: Iterable[TlogMessage],
|
||||
inject_timestamp_us: int,
|
||||
*,
|
||||
window_us: int = int(HINT_ACK_MAX_LATENCY_MS * 1000.0),
|
||||
rejection_tokens: Sequence[str] = HINT_REJECTION_STATUSTEXT_TOKENS,
|
||||
) -> HintRejectionReport:
|
||||
"""AC-4: scan ``STATUSTEXT`` in the post-inject window for rejection markers.
|
||||
|
||||
A rejection is any ``STATUSTEXT`` whose payload ``text`` field (case
|
||||
insensitive) contains any of ``rejection_tokens``. The window opens
|
||||
at the inject timestamp and closes ``window_us`` later — beyond that
|
||||
a rejection cannot be causally tied to this hint.
|
||||
"""
|
||||
if window_us <= 0:
|
||||
raise ValueError(f"window_us must be > 0, got {window_us}")
|
||||
window_end = inject_timestamp_us + window_us
|
||||
tokens_upper = tuple(token.upper() for token in rejection_tokens)
|
||||
rejection_texts: list[str] = []
|
||||
for msg in messages:
|
||||
if msg.msg_type != "STATUSTEXT":
|
||||
continue
|
||||
if not (inject_timestamp_us <= msg.timestamp_us <= window_end):
|
||||
continue
|
||||
text = str(msg.fields.get("text", "")).upper()
|
||||
if any(token in text for token in tokens_upper):
|
||||
rejection_texts.append(str(msg.fields.get("text", "")))
|
||||
return HintRejectionReport(
|
||||
inject_timestamp_us=inject_timestamp_us,
|
||||
window_us=window_us,
|
||||
rejection_count=len(rejection_texts),
|
||||
rejection_texts=tuple(rejection_texts),
|
||||
)
|
||||
|
||||
|
||||
# ─────────────────────── tlog→hint adapter ───────────────────────
|
||||
|
||||
|
||||
def extract_inbound_hints(
|
||||
messages: Iterable[TlogMessage],
|
||||
*,
|
||||
hint_prefix: str = "RELOC:",
|
||||
) -> list[InboundHint]:
|
||||
"""Extract operator-injected reloc-hint STATUSTEXTs from the tlog.
|
||||
|
||||
The test fixture builder injects ``STATUSTEXT`` messages whose
|
||||
payload ``text`` begins with ``hint_prefix`` (default ``"RELOC:"``)
|
||||
followed by a comma-separated payload (e.g. ``"RELOC:50.0,36.0,200"``
|
||||
encoding lat,lon,radius_m). The exact payload shape is not
|
||||
interpreted here — that belongs to the scenario test. We only
|
||||
identify which STATUSTEXTs are hints so the FDR correlator knows
|
||||
when the operator pressed "send".
|
||||
"""
|
||||
out: list[InboundHint] = []
|
||||
for msg in messages:
|
||||
if msg.msg_type != "STATUSTEXT":
|
||||
continue
|
||||
text = str(msg.fields.get("text", ""))
|
||||
if not text.startswith(hint_prefix):
|
||||
continue
|
||||
out.append(InboundHint(inject_timestamp_us=msg.timestamp_us, hint_text=text))
|
||||
return out
|
||||
|
||||
|
||||
def parse_reloc_payload(hint_text: str, *, hint_prefix: str = "RELOC:") -> tuple[float, float, float]:
|
||||
"""Parse ``RELOC:<lat>,<lon>,<radius_m>`` into ``(lat, lon, radius)``.
|
||||
|
||||
Raises ``ValueError`` on malformed payload — scenarios should let
|
||||
that surface so the run fails loudly rather than silently scoring
|
||||
AC-3 against garbage coordinates.
|
||||
"""
|
||||
if not hint_text.startswith(hint_prefix):
|
||||
raise ValueError(
|
||||
f"hint text does not start with {hint_prefix!r}: {hint_text!r}"
|
||||
)
|
||||
body = hint_text[len(hint_prefix):]
|
||||
parts = body.split(",")
|
||||
if len(parts) != 3:
|
||||
raise ValueError(
|
||||
f"hint payload must have 3 comma-separated fields "
|
||||
f"(lat,lon,radius_m); got {len(parts)}: {body!r}"
|
||||
)
|
||||
try:
|
||||
lat = float(parts[0])
|
||||
lon = float(parts[1])
|
||||
radius_m = float(parts[2])
|
||||
except ValueError as exc:
|
||||
raise ValueError(f"hint payload fields must be floats: {body!r}") from exc
|
||||
return (lat, lon, radius_m)
|
||||
|
||||
|
||||
def collect_messages_to_list(messages: Iterable[TlogMessage]) -> list[TlogMessage]:
|
||||
"""Materialise an iterator into a list — convenience for multi-pass eval.
|
||||
|
||||
Mirrors ``ap_contract_evaluator.collect_messages_to_list``: scenarios
|
||||
parse the tlog once via ``iter_messages`` and run multiple analyzers
|
||||
over the result.
|
||||
"""
|
||||
return list(messages)
|
||||
@@ -29,6 +29,8 @@ Fixture file naming (under `${E2E_SITL_REPLAY_DIR}/`):
|
||||
{messages: [{image_id?, lat_deg, lon_deg} | null, ...]}
|
||||
* `ap_parameters_<host>.json` — {<param_name>: <value>, ...}
|
||||
* `ap_tlog_<host>.tlog` — raw mavproxy tlog (any binary content)
|
||||
* `gcs_tlog_<host>.tlog` — raw mavproxy-listener tlog from the GCS link
|
||||
(SUT→GCS summary stream + GCS→SUT operator commands; FT-P-12, FT-P-13)
|
||||
* `inav_handshake_<host>.json` — {established_within_s: float | None}
|
||||
* `inav_msp_frames_<host>.json` — {frames: [...], expected_num_sat: int}
|
||||
* `inav_gps_state_<host>.json` — {fix_type, num_sat, provider}
|
||||
@@ -418,6 +420,35 @@ def capture_ap_tlog(host: str, duration_s: float) -> Path:
|
||||
return path
|
||||
|
||||
|
||||
def capture_gcs_tlog(host: str, duration_s: float) -> Path:
|
||||
"""Return the path to the GCS-side mavproxy-listener tlog for ``host``.
|
||||
|
||||
Fixture: ``${E2E_SITL_REPLAY_DIR}/gcs_tlog_<host>.tlog``. The tlog
|
||||
captures both directions over the QGC GCS link — SUT→GCS summary
|
||||
bursts (``GLOBAL_POSITION_INT`` + ``NAMED_VALUE_FLOAT``) and
|
||||
GCS→SUT operator commands (``STATUSTEXT`` reloc-hints,
|
||||
``COMMAND_LONG`` parameter reads, etc.).
|
||||
|
||||
``duration_s`` is recorded for future live-mode use but ignored here
|
||||
— under FDR-replay the fixture file IS the captured stream.
|
||||
|
||||
Raises ``RuntimeError`` if env var unset or fixture missing.
|
||||
"""
|
||||
if duration_s <= 0:
|
||||
raise RuntimeError(f"capture_gcs_tlog: duration_s must be positive; got {duration_s}")
|
||||
root = replay_dir()
|
||||
if root is None:
|
||||
raise RuntimeError(
|
||||
f"capture_gcs_tlog: {_ENV_VAR} env var not set"
|
||||
)
|
||||
path = root / f"gcs_tlog_{host}.tlog"
|
||||
if not path.exists():
|
||||
raise RuntimeError(
|
||||
f"capture_gcs_tlog: fixture not found at {path}"
|
||||
)
|
||||
return path
|
||||
|
||||
|
||||
# read_ap_parameter — reads from param-dump JSON
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user