mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 12:31:13 +00:00
[AZ-416] [AZ-417] [AZ-419] Test batch 72: FT-P-09 AP/iNav + FT-P-11 cold start
- AZ-416 (FT-P-09-AP): fills mavproxy_tlog_reader.iter_messages with pymavlink body (AZ-406 surface kept); adds ap_contract_evaluator covering AC-1 (signing handshake <=5s), AC-2 (GPS_INPUT >=4.5 Hz), AC-3 (EK3_SRC1_POSXY=3), AC-4 (GPS_RAW_INT health >=80%); scenario forces fc_adapter=ardupilot. - AZ-417 (FT-P-09-iNav): msp_frame_observer covering AC-2 (MSP rate) and AC-3 (fix_type/provider/numSat); scenario forces fc_adapter=inav. - AZ-419 (FT-P-11): cold_start_evaluator covering AC-1 (operator manifest origin), AC-2 (FC EKF fallback), AC-3 (no-origin abort), AC-4 (bounded-delta conflict, ADR-010 Principle #11 amended); scenario parametrized on origin_source plus dedicated no-origin abort scenario. - All scenarios skip-gated on upstream frame_source_replay / imu_replay / fdr_reader / sitl_observer extensions. - +67 unit tests; full e2e unit suite: 460 passed. - K=3 cumulative review fired: PASS for batches 70-72. See _docs/03_implementation/batch_72_report.md, _docs/03_implementation/reviews/batch_72_review.md, _docs/03_implementation/cumulative_review_batches_70-72_cycle1_report.md. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -0,0 +1,240 @@
|
||||
"""ArduPilot contract + signing-handshake evaluation for FT-P-09-AP (AZ-416).
|
||||
|
||||
Given the captured ``.tlog`` from ``mavproxy-listener`` plus a single
|
||||
EK3_SRC1_POSXY parameter read, this helper validates:
|
||||
|
||||
* AC-1: signing handshake completes within ≤5 s
|
||||
(``observe_signing_handshake`` — first signed message within the
|
||||
window OR absence of ``BAD_SIGNATURE`` STATUSTEXT during it).
|
||||
* AC-2: GPS_INPUT flow at ≥4.5 Hz over the 60 s replay
|
||||
(``compute_gps_input_rate``).
|
||||
* AC-3: EK3_SRC1_POSXY == 3 (``validate_ek3_src1_posxy`` — pure check
|
||||
on the param value the caller fetched via mavproxy).
|
||||
* AC-4: GPS_RAW_INT health — ``fix_type ≥ 3`` AND ``eph ≤ 200``
|
||||
(HDOP ≤ 2.0) for ≥80 % of the 60 s window
|
||||
(``evaluate_gps_raw_int_health``).
|
||||
|
||||
All inputs are pure ``Iterable[TlogMessage]``; the tlog ingestion is
|
||||
delegated to ``runner.helpers.mavproxy_tlog_reader.iter_messages``.
|
||||
|
||||
Public-boundary discipline: does NOT import any
|
||||
``src/gps_denied_onboard`` symbol.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Iterable, Sequence
|
||||
|
||||
from .mavproxy_tlog_reader import TlogMessage
|
||||
|
||||
HANDSHAKE_BUDGET_S = 5.0
|
||||
GPS_INPUT_TARGET_RATE_HZ = 5.0
|
||||
GPS_INPUT_MIN_RATE_HZ = 4.5
|
||||
GPS_RAW_INT_MIN_FIX_TYPE = 3
|
||||
GPS_RAW_INT_MAX_EPH = 200 # HDOP × 100 ≤ 200 → HDOP ≤ 2.0
|
||||
GPS_RAW_INT_HEALTHY_FRACTION_REQUIRED = 0.80
|
||||
EK3_SRC1_POSXY_REQUIRED = 3 # AP EKF source-set: 3 = GPS
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class HandshakeReport:
|
||||
"""AC-1: signing-handshake completion observation."""
|
||||
|
||||
window_start_us: int
|
||||
window_end_us: int
|
||||
first_signed_us: int | None
|
||||
bad_signature_count: int
|
||||
setup_signing_seen: bool
|
||||
|
||||
@property
|
||||
def lag_s(self) -> float | None:
|
||||
if self.first_signed_us is None:
|
||||
return None
|
||||
return (self.first_signed_us - self.window_start_us) / 1_000_000.0
|
||||
|
||||
@property
|
||||
def passes(self) -> bool:
|
||||
return (
|
||||
self.first_signed_us is not None
|
||||
and self.lag_s is not None
|
||||
and self.lag_s <= HANDSHAKE_BUDGET_S
|
||||
and self.bad_signature_count == 0
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class GpsInputRateReport:
|
||||
"""AC-2: GPS_INPUT rate over the replay window."""
|
||||
|
||||
frame_count: int
|
||||
window_us: int
|
||||
observed_rate_hz: float
|
||||
target_rate_hz: float = GPS_INPUT_TARGET_RATE_HZ
|
||||
min_required_hz: float = GPS_INPUT_MIN_RATE_HZ
|
||||
|
||||
@property
|
||||
def passes(self) -> bool:
|
||||
return (
|
||||
self.window_us > 0
|
||||
and self.observed_rate_hz >= self.min_required_hz
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class GpsRawIntHealthReport:
|
||||
"""AC-4: GPS_RAW_INT fix_type + eph healthy fraction."""
|
||||
|
||||
total_samples: int
|
||||
healthy_samples: int
|
||||
fraction_required: float = GPS_RAW_INT_HEALTHY_FRACTION_REQUIRED
|
||||
|
||||
@property
|
||||
def healthy_fraction(self) -> float:
|
||||
if self.total_samples == 0:
|
||||
return 0.0
|
||||
return self.healthy_samples / self.total_samples
|
||||
|
||||
@property
|
||||
def passes(self) -> bool:
|
||||
return (
|
||||
self.total_samples > 0
|
||||
and self.healthy_fraction >= self.fraction_required
|
||||
)
|
||||
|
||||
|
||||
def observe_signing_handshake(
|
||||
messages: Iterable[TlogMessage],
|
||||
*,
|
||||
handshake_window_us: int = int(HANDSHAKE_BUDGET_S * 1_000_000),
|
||||
) -> HandshakeReport:
|
||||
"""AC-1: first signed message within ``handshake_window_us``.
|
||||
|
||||
The handshake window starts at the FIRST observed message's
|
||||
timestamp (the SUT cannot be heard from before that). The result
|
||||
PASSES if a signed message arrives within the window AND no
|
||||
``STATUSTEXT`` with ``BAD_SIGNATURE`` is observed during it.
|
||||
|
||||
The SETUP_SIGNING handshake exchange itself is unsigned by spec
|
||||
(it's how the key is shared), so its presence is reported but does
|
||||
NOT gate the pass — the gate is the first SIGNED follow-up.
|
||||
"""
|
||||
if handshake_window_us <= 0:
|
||||
raise ValueError(f"handshake_window_us must be > 0, got {handshake_window_us}")
|
||||
window_start: int | None = None
|
||||
window_end: int | None = None
|
||||
first_signed_us: int | None = None
|
||||
bad_sig_count = 0
|
||||
setup_signing_seen = False
|
||||
|
||||
for m in messages:
|
||||
if window_start is None:
|
||||
window_start = m.timestamp_us
|
||||
window_end = window_start + handshake_window_us
|
||||
if window_end is not None and m.timestamp_us > window_end:
|
||||
break
|
||||
if m.msg_type == "SETUP_SIGNING":
|
||||
setup_signing_seen = True
|
||||
if m.signed and first_signed_us is None:
|
||||
first_signed_us = m.timestamp_us
|
||||
if m.msg_type == "STATUSTEXT":
|
||||
text = str(m.fields.get("text", "")).upper()
|
||||
if "BAD_SIGNATURE" in text:
|
||||
bad_sig_count += 1
|
||||
|
||||
return HandshakeReport(
|
||||
window_start_us=window_start or 0,
|
||||
window_end_us=window_end or 0,
|
||||
first_signed_us=first_signed_us,
|
||||
bad_signature_count=bad_sig_count,
|
||||
setup_signing_seen=setup_signing_seen,
|
||||
)
|
||||
|
||||
|
||||
def compute_gps_input_rate(
|
||||
messages: Iterable[TlogMessage],
|
||||
*,
|
||||
target_rate_hz: float = GPS_INPUT_TARGET_RATE_HZ,
|
||||
min_required_hz: float = GPS_INPUT_MIN_RATE_HZ,
|
||||
) -> GpsInputRateReport:
|
||||
"""AC-2: GPS_INPUT cadence over the entire message stream."""
|
||||
if min_required_hz < 0:
|
||||
raise ValueError(f"min_required_hz must be ≥0, got {min_required_hz}")
|
||||
timestamps = [m.timestamp_us for m in messages if m.msg_type == "GPS_INPUT"]
|
||||
if len(timestamps) < 2:
|
||||
return GpsInputRateReport(
|
||||
frame_count=len(timestamps),
|
||||
window_us=0,
|
||||
observed_rate_hz=0.0,
|
||||
target_rate_hz=target_rate_hz,
|
||||
min_required_hz=min_required_hz,
|
||||
)
|
||||
window_us = timestamps[-1] - timestamps[0]
|
||||
if window_us <= 0:
|
||||
return GpsInputRateReport(
|
||||
frame_count=len(timestamps),
|
||||
window_us=window_us,
|
||||
observed_rate_hz=0.0,
|
||||
target_rate_hz=target_rate_hz,
|
||||
min_required_hz=min_required_hz,
|
||||
)
|
||||
observed = (len(timestamps) - 1) / (window_us / 1_000_000.0)
|
||||
return GpsInputRateReport(
|
||||
frame_count=len(timestamps),
|
||||
window_us=window_us,
|
||||
observed_rate_hz=observed,
|
||||
target_rate_hz=target_rate_hz,
|
||||
min_required_hz=min_required_hz,
|
||||
)
|
||||
|
||||
|
||||
def validate_ek3_src1_posxy(value: int) -> bool:
|
||||
"""AC-3: EK3_SRC1_POSXY must equal 3 (GPS source)."""
|
||||
return value == EK3_SRC1_POSXY_REQUIRED
|
||||
|
||||
|
||||
def evaluate_gps_raw_int_health(
|
||||
messages: Iterable[TlogMessage],
|
||||
*,
|
||||
min_fix_type: int = GPS_RAW_INT_MIN_FIX_TYPE,
|
||||
max_eph: int = GPS_RAW_INT_MAX_EPH,
|
||||
fraction_required: float = GPS_RAW_INT_HEALTHY_FRACTION_REQUIRED,
|
||||
) -> GpsRawIntHealthReport:
|
||||
"""AC-4: ≥``fraction_required`` of GPS_RAW_INT samples must be healthy.
|
||||
|
||||
A sample is "healthy" iff ``fix_type ≥ min_fix_type`` AND
|
||||
``eph ≤ max_eph``. Both must hold per the spec text.
|
||||
"""
|
||||
if not 0.0 <= fraction_required <= 1.0:
|
||||
raise ValueError(
|
||||
f"fraction_required must be in [0, 1], got {fraction_required}"
|
||||
)
|
||||
total = 0
|
||||
healthy = 0
|
||||
for m in messages:
|
||||
if m.msg_type != "GPS_RAW_INT":
|
||||
continue
|
||||
total += 1
|
||||
try:
|
||||
fix_type = int(m.fields["fix_type"]) # type: ignore[arg-type]
|
||||
eph = int(m.fields["eph"]) # type: ignore[arg-type]
|
||||
except (KeyError, TypeError, ValueError):
|
||||
continue
|
||||
if fix_type >= min_fix_type and eph <= max_eph:
|
||||
healthy += 1
|
||||
return GpsRawIntHealthReport(
|
||||
total_samples=total,
|
||||
healthy_samples=healthy,
|
||||
fraction_required=fraction_required,
|
||||
)
|
||||
|
||||
|
||||
def collect_messages_to_list(messages: Iterable[TlogMessage]) -> list[TlogMessage]:
|
||||
"""Materialise an iterator into a list — convenience for multi-pass eval.
|
||||
|
||||
The scenario reads the tlog once via ``iter_messages`` and runs
|
||||
multiple analyzers over the result. ``iter_messages`` returns a
|
||||
generator that closes its underlying pymavlink connection on
|
||||
exhaustion, so re-iteration is not safe without materialisation.
|
||||
"""
|
||||
return list(messages)
|
||||
@@ -0,0 +1,309 @@
|
||||
"""Cold-start initialization evaluation for FT-P-11 (AZ-419 / ADR-010 / AC-5.1).
|
||||
|
||||
ADR-010 splits cold-start into two paths:
|
||||
|
||||
* **Primary** (operator manifest, AZ-490): C12 bakes
|
||||
``flight.takeoff_origin`` into the C10 Manifest from the operator-
|
||||
authored mission; airborne C5 consumes it BEFORE any sensor sample
|
||||
via ``set_takeoff_origin``. Used even when the FC EKF has no valid
|
||||
GPS.
|
||||
* **Secondary** (FC EKF, legacy AC-5.1): when the Manifest carries no
|
||||
``takeoff_origin``, the SUT falls back to the FC EKF snapshot.
|
||||
* **Bounded-delta conflict** (Principle #11 amended): both signals
|
||||
present but ``|operator − fc_ekf| > 200 m`` → operator wins; FC GPS
|
||||
is logged as suspect via a ``c5.gps_bounded_delta.reject`` FDR
|
||||
record naming both points.
|
||||
|
||||
This helper owns the pure-logic side:
|
||||
|
||||
* ``write_manifest`` / ``read_manifest`` — manipulate the fixture
|
||||
Manifest the test builder produces.
|
||||
* ``read_cold_boot_fixture`` — parse the AZ-408 cold-boot snapshot
|
||||
JSON into a typed ``ColdBootSnapshot``.
|
||||
* ``evaluate_first_estimate`` — distance vs expected origin + source
|
||||
label rules + FDR record presence checks.
|
||||
|
||||
Public-boundary discipline: does NOT import any
|
||||
``src/gps_denied_onboard`` symbol.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Iterable, Mapping, Sequence
|
||||
|
||||
from .geo import distance_m
|
||||
|
||||
ACCURACY_BUDGET_M = 50.0 # AC-1/AC-2/AC-4: estimate within ±50 m of origin
|
||||
BOUNDED_DELTA_TRIGGER_M = 200.0 # ADR-010 Principle #11 amended
|
||||
FIRST_EMISSION_BUDGET_S = 30.0 # AC-1/AC-NEW-1
|
||||
FORBIDDEN_FIRST_LABEL_BOUNDED_DELTA = "satellite_anchored"
|
||||
FDR_RECORD_ORIGIN_SET = "c5.cold_start_origin.set"
|
||||
FDR_RECORD_ORIGIN_UNAVAILABLE = "c5.cold_start_origin.unavailable"
|
||||
FDR_RECORD_BOUNDED_DELTA_REJECT = "c5.gps_bounded_delta.reject"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class LatLonAlt:
|
||||
"""One geodetic point: WGS84 degrees + altitude in meters."""
|
||||
|
||||
lat_deg: float
|
||||
lon_deg: float
|
||||
alt_m: float
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ManifestOrigin:
|
||||
"""A subset of the C10 Manifest for FT-P-11 — just the takeoff_origin."""
|
||||
|
||||
takeoff_origin: LatLonAlt | None
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ColdBootSnapshot:
|
||||
"""Parsed AZ-408 cold-boot fixture (FC EKF snapshot pose)."""
|
||||
|
||||
lat_deg: float
|
||||
lon_deg: float
|
||||
alt_m: float
|
||||
schema: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class OutboundEstimate:
|
||||
"""First outbound estimate observed by the scenario."""
|
||||
|
||||
monotonic_ms: int
|
||||
lat_deg: float
|
||||
lon_deg: float
|
||||
source_label: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class FdrAuditRecord:
|
||||
"""One FDR record relevant to cold-start auditing."""
|
||||
|
||||
monotonic_ms: int
|
||||
record_type: str
|
||||
payload: Mapping[str, object]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class FirstEstimateReport:
|
||||
"""AC-1 / AC-2 / AC-4: distance + label + FDR record audit."""
|
||||
|
||||
origin_source: str
|
||||
expected_origin: LatLonAlt | None
|
||||
actual_estimate: OutboundEstimate | None
|
||||
distance_m: float | None
|
||||
source_label_ok: bool
|
||||
fdr_origin_set_seen: bool
|
||||
fdr_origin_set_source: str | None
|
||||
fdr_bounded_delta_seen: bool
|
||||
fdr_bounded_delta_a: LatLonAlt | None
|
||||
fdr_bounded_delta_b: LatLonAlt | None
|
||||
|
||||
@property
|
||||
def passes_distance(self) -> bool:
|
||||
return (
|
||||
self.distance_m is not None
|
||||
and self.distance_m <= ACCURACY_BUDGET_M
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class NoOriginReport:
|
||||
"""AC-3: SUT MUST refuse takeoff when no origin is available."""
|
||||
|
||||
estimate_within_budget: bool # True iff an estimate WAS produced — failure mode
|
||||
fdr_origin_unavailable_seen: bool
|
||||
|
||||
@property
|
||||
def passes(self) -> bool:
|
||||
# AC-3 passes when NO estimate was produced AND the FDR records
|
||||
# the takeoff-abort signal.
|
||||
return not self.estimate_within_budget and self.fdr_origin_unavailable_seen
|
||||
|
||||
|
||||
def write_manifest(out_path: Path, takeoff_origin: LatLonAlt | None) -> Path:
|
||||
"""Write a minimal C10-Manifest-shaped JSON for the test fixture builder.
|
||||
|
||||
The schema mirrors the AZ-323 canonical Manifest serialization just
|
||||
closely enough that the SUT's ``set_takeoff_origin`` consumer
|
||||
accepts it. Field shape mirrors `_docs/02_document/contracts/c12_*`.
|
||||
"""
|
||||
out_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
payload: dict[str, object] = {"_schema": "ft-p-11-test-manifest/v1"}
|
||||
if takeoff_origin is not None:
|
||||
payload["flight"] = {
|
||||
"takeoff_origin": {
|
||||
"lat_deg": takeoff_origin.lat_deg,
|
||||
"lon_deg": takeoff_origin.lon_deg,
|
||||
"alt_m": takeoff_origin.alt_m,
|
||||
}
|
||||
}
|
||||
else:
|
||||
payload["flight"] = {}
|
||||
out_path.write_text(json.dumps(payload, indent=2))
|
||||
return out_path
|
||||
|
||||
|
||||
def read_manifest(manifest_path: Path) -> ManifestOrigin:
|
||||
"""Read a Manifest JSON and extract the ``takeoff_origin`` if present."""
|
||||
if not manifest_path.exists():
|
||||
raise FileNotFoundError(f"manifest not found: {manifest_path}")
|
||||
payload = json.loads(manifest_path.read_text())
|
||||
origin_raw = payload.get("flight", {}).get("takeoff_origin")
|
||||
if origin_raw is None:
|
||||
return ManifestOrigin(takeoff_origin=None)
|
||||
return ManifestOrigin(
|
||||
takeoff_origin=LatLonAlt(
|
||||
lat_deg=float(origin_raw["lat_deg"]),
|
||||
lon_deg=float(origin_raw["lon_deg"]),
|
||||
alt_m=float(origin_raw["alt_m"]),
|
||||
)
|
||||
)
|
||||
|
||||
|
||||
def read_cold_boot_fixture(fixture_path: Path) -> ColdBootSnapshot:
|
||||
"""Parse the AZ-408 cold-boot JSON into a typed snapshot.
|
||||
|
||||
Converts the fixture's ``lat_e7 / lon_e7 / alt_mm`` (MAVLink int32
|
||||
units, 1e-7 deg + millimeters) to ``lat_deg / lon_deg / alt_m``.
|
||||
"""
|
||||
if not fixture_path.exists():
|
||||
raise FileNotFoundError(f"cold-boot fixture not found: {fixture_path}")
|
||||
payload = json.loads(fixture_path.read_text())
|
||||
schema = str(payload.get("_schema", ""))
|
||||
pose = payload["global_position_int"]
|
||||
return ColdBootSnapshot(
|
||||
lat_deg=int(pose["lat_e7"]) / 1e7,
|
||||
lon_deg=int(pose["lon_e7"]) / 1e7,
|
||||
alt_m=int(pose["alt_mm"]) / 1000.0,
|
||||
schema=schema,
|
||||
)
|
||||
|
||||
|
||||
def _scan_fdr_for_cold_start(
|
||||
fdr_records: Iterable[FdrAuditRecord],
|
||||
) -> dict[str, object]:
|
||||
"""Single pass collecting all cold-start-relevant FDR signals."""
|
||||
origin_set_source: str | None = None
|
||||
origin_set_seen = False
|
||||
origin_unavailable_seen = False
|
||||
bounded_delta_seen = False
|
||||
bounded_delta_a: LatLonAlt | None = None
|
||||
bounded_delta_b: LatLonAlt | None = None
|
||||
for r in fdr_records:
|
||||
if r.record_type == FDR_RECORD_ORIGIN_SET:
|
||||
origin_set_seen = True
|
||||
src = r.payload.get("source")
|
||||
if src is not None:
|
||||
origin_set_source = str(src)
|
||||
elif r.record_type == FDR_RECORD_ORIGIN_UNAVAILABLE:
|
||||
origin_unavailable_seen = True
|
||||
elif r.record_type == FDR_RECORD_BOUNDED_DELTA_REJECT:
|
||||
bounded_delta_seen = True
|
||||
a = r.payload.get("a")
|
||||
b = r.payload.get("b")
|
||||
if isinstance(a, Mapping):
|
||||
bounded_delta_a = LatLonAlt(
|
||||
lat_deg=float(a["lat_deg"]), # type: ignore[arg-type]
|
||||
lon_deg=float(a["lon_deg"]), # type: ignore[arg-type]
|
||||
alt_m=float(a.get("alt_m", 0.0)), # type: ignore[arg-type]
|
||||
)
|
||||
if isinstance(b, Mapping):
|
||||
bounded_delta_b = LatLonAlt(
|
||||
lat_deg=float(b["lat_deg"]), # type: ignore[arg-type]
|
||||
lon_deg=float(b["lon_deg"]), # type: ignore[arg-type]
|
||||
alt_m=float(b.get("alt_m", 0.0)), # type: ignore[arg-type]
|
||||
)
|
||||
return {
|
||||
"origin_set_seen": origin_set_seen,
|
||||
"origin_set_source": origin_set_source,
|
||||
"origin_unavailable_seen": origin_unavailable_seen,
|
||||
"bounded_delta_seen": bounded_delta_seen,
|
||||
"bounded_delta_a": bounded_delta_a,
|
||||
"bounded_delta_b": bounded_delta_b,
|
||||
}
|
||||
|
||||
|
||||
def evaluate_first_estimate(
|
||||
*,
|
||||
origin_source: str,
|
||||
expected_origin: LatLonAlt | None,
|
||||
first_estimate: OutboundEstimate | None,
|
||||
fdr_records: Sequence[FdrAuditRecord],
|
||||
) -> FirstEstimateReport:
|
||||
"""Evaluate AC-1/AC-2/AC-4 given the first observed outbound estimate.
|
||||
|
||||
``origin_source`` is one of:
|
||||
* ``"operator_manifest"`` — AC-1: distance ≤50 m of A AND FDR has
|
||||
``c5.cold_start_origin.set(source="manifest")``.
|
||||
* ``"fc_ekf"`` — AC-2: distance ≤50 m of FC EKF snapshot AND FDR
|
||||
has ``c5.cold_start_origin.set(source="fc_ekf")``.
|
||||
* ``"bounded_delta_conflict"`` — AC-4: distance ≤50 m of A;
|
||||
source_label != ``satellite_anchored``; FDR has
|
||||
``c5.gps_bounded_delta.reject`` naming both A and B.
|
||||
|
||||
Any other source string raises ``ValueError``.
|
||||
"""
|
||||
if origin_source not in {"operator_manifest", "fc_ekf", "bounded_delta_conflict"}:
|
||||
raise ValueError(
|
||||
f"unknown origin_source {origin_source!r}; expected one of "
|
||||
"{operator_manifest, fc_ekf, bounded_delta_conflict}"
|
||||
)
|
||||
|
||||
distance: float | None = None
|
||||
if first_estimate is not None and expected_origin is not None:
|
||||
distance = distance_m(
|
||||
expected_origin.lat_deg, expected_origin.lon_deg,
|
||||
first_estimate.lat_deg, first_estimate.lon_deg,
|
||||
)
|
||||
|
||||
if origin_source == "bounded_delta_conflict":
|
||||
label_ok = (
|
||||
first_estimate is not None
|
||||
and first_estimate.source_label != FORBIDDEN_FIRST_LABEL_BOUNDED_DELTA
|
||||
)
|
||||
else:
|
||||
label_ok = first_estimate is not None # any label acceptable for AC-1/AC-2
|
||||
|
||||
audit = _scan_fdr_for_cold_start(fdr_records)
|
||||
|
||||
return FirstEstimateReport(
|
||||
origin_source=origin_source,
|
||||
expected_origin=expected_origin,
|
||||
actual_estimate=first_estimate,
|
||||
distance_m=distance,
|
||||
source_label_ok=label_ok,
|
||||
fdr_origin_set_seen=bool(audit["origin_set_seen"]),
|
||||
fdr_origin_set_source=audit["origin_set_source"], # type: ignore[arg-type]
|
||||
fdr_bounded_delta_seen=bool(audit["bounded_delta_seen"]),
|
||||
fdr_bounded_delta_a=audit["bounded_delta_a"], # type: ignore[arg-type]
|
||||
fdr_bounded_delta_b=audit["bounded_delta_b"], # type: ignore[arg-type]
|
||||
)
|
||||
|
||||
|
||||
def evaluate_no_origin_path(
|
||||
*,
|
||||
first_estimate: OutboundEstimate | None,
|
||||
fdr_records: Sequence[FdrAuditRecord],
|
||||
) -> NoOriginReport:
|
||||
"""AC-3: Manifest empty + SITL no GPS → SUT must NOT emit anything.
|
||||
|
||||
Returns ``passes=True`` iff no outbound estimate was produced AND
|
||||
the FDR carries ``c5.cold_start_origin.unavailable``.
|
||||
"""
|
||||
audit = _scan_fdr_for_cold_start(fdr_records)
|
||||
return NoOriginReport(
|
||||
estimate_within_budget=first_estimate is not None,
|
||||
fdr_origin_unavailable_seen=bool(audit["origin_unavailable_seen"]),
|
||||
)
|
||||
|
||||
|
||||
def bounded_delta_distance_m(a: LatLonAlt, b: LatLonAlt) -> float:
|
||||
"""Convenience: AC-4 trigger condition is ``vincenty(A, B) > 200 m``."""
|
||||
return distance_m(a.lat_deg, a.lon_deg, b.lat_deg, b.lon_deg)
|
||||
@@ -10,8 +10,11 @@ This module exposes a small typed wrapper so per-scenario tests can:
|
||||
of signed vs unsigned messages for NFT-SEC-03).
|
||||
3. Attach the source `.tlog` path to the evidence bundler.
|
||||
|
||||
Concrete iteration logic is owned by AZ-416 (FT-P-09-AP); AZ-406 commits
|
||||
to the public surface.
|
||||
AZ-416 (FT-P-09-AP) owns the pymavlink-backed body; AZ-406 committed to
|
||||
the public surface.
|
||||
|
||||
Public-boundary discipline: does NOT import any ``src/gps_denied_onboard``
|
||||
symbol.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
@@ -20,6 +23,8 @@ from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Iterator
|
||||
|
||||
from pymavlink import mavutil
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class TlogMessage:
|
||||
@@ -32,12 +37,53 @@ class TlogMessage:
|
||||
def iter_messages(tlog_path: Path) -> Iterator[TlogMessage]:
|
||||
"""Iterate `.tlog` messages oldest-first.
|
||||
|
||||
AZ-406 raises until AZ-416 fills in the pymavlink-backed iterator.
|
||||
Uses ``pymavlink.mavutil.mavlink_connection`` in tlog-file mode.
|
||||
Each yielded ``TlogMessage`` carries:
|
||||
|
||||
* ``timestamp_us`` — unix microseconds, as recorded by mavproxy
|
||||
(pymavlink exposes this as ``msg._timestamp`` in seconds-float).
|
||||
* ``msg_type`` — message name (e.g. ``"GPS_INPUT"``, ``"GPS_RAW_INT"``).
|
||||
* ``signed`` — True iff the wire frame carried a MAVLink 2.0
|
||||
signature block (`msg.get_signed()` on pymavlink ≥2.4).
|
||||
* ``fields`` — dict of field name → value, via ``msg.to_dict()``
|
||||
minus the ``mavpackettype`` key.
|
||||
|
||||
Bad / unparsable frames are skipped (mavlogfile returns ``None`` or
|
||||
raises internally) but EOF closes the iterator cleanly.
|
||||
"""
|
||||
raise NotImplementedError(
|
||||
"mavproxy_tlog_reader.iter_messages is owned by AZ-416 — "
|
||||
"AZ-406 supplies only the public surface."
|
||||
)
|
||||
if not tlog_path.exists():
|
||||
raise FileNotFoundError(f"tlog not found: {tlog_path}")
|
||||
|
||||
conn = mavutil.mavlink_connection(str(tlog_path))
|
||||
try:
|
||||
while True:
|
||||
msg = conn.recv_match(blocking=False)
|
||||
if msg is None:
|
||||
break
|
||||
msg_type = msg.get_type()
|
||||
if msg_type == "BAD_DATA":
|
||||
continue
|
||||
try:
|
||||
fields = msg.to_dict()
|
||||
except Exception:
|
||||
continue
|
||||
fields.pop("mavpackettype", None)
|
||||
ts_s = getattr(msg, "_timestamp", 0.0) or 0.0
|
||||
try:
|
||||
signed = bool(msg.get_signed())
|
||||
except AttributeError:
|
||||
signed = False
|
||||
yield TlogMessage(
|
||||
timestamp_us=int(ts_s * 1_000_000),
|
||||
msg_type=msg_type,
|
||||
signed=signed,
|
||||
fields=fields,
|
||||
)
|
||||
finally:
|
||||
try:
|
||||
conn.close()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
def count_by_type(tlog_path: Path) -> dict[str, int]:
|
||||
|
||||
@@ -0,0 +1,155 @@
|
||||
"""MSP2 frame observer for FT-P-09-iNav (AZ-417 / AC-4.3).
|
||||
|
||||
iNav consumes MSP2 over a TCP socket on port 5760. The SUT's
|
||||
``c8_fc_adapter`` (iNav-side) emits ``MSP2_SENSOR_GPS`` (function ID
|
||||
0x1F03) frames at a configured cadence (target 5 Hz per AC-2).
|
||||
|
||||
This helper owns the pure-logic side of FT-P-09-iNav:
|
||||
|
||||
* ``compute_rate_hz`` — given a sequence of frame-arrival timestamps,
|
||||
return the observed Hz over a window.
|
||||
* ``count_frames_by_id`` — filter + tally per MSP function ID.
|
||||
* ``evaluate_inav_gps_state`` — given a snapshot of iNav's ``gpsSol``
|
||||
+ ``provider`` after replay, assert AC-3 (fix_type ≥ 3, provider =
|
||||
MSP, numSat matches the emitted value).
|
||||
|
||||
The TCP-probe + actual MSP frame capture path is owned by AZ-407
|
||||
(``runner.helpers.sitl_observer``) and the iNav SITL docker compose
|
||||
service. This module only consumes already-captured data.
|
||||
|
||||
Public-boundary discipline: does NOT import any ``src/gps_denied_onboard``
|
||||
symbol.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Sequence
|
||||
|
||||
MSP2_SENSOR_GPS_FUNCTION_ID = 0x1F03
|
||||
DEFAULT_TARGET_RATE_HZ = 5.0
|
||||
MIN_OBSERVED_RATE_HZ = 4.5 # AC-2: ≥4.5 Hz observed for 5 Hz target
|
||||
MIN_FIX_TYPE = 3 # AC-3: gpsSol.fixType ≥ 3
|
||||
REQUIRED_PROVIDER = "MSP" # AC-3: provider=MSP (no fallback to internal GPS)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MspFrameSample:
|
||||
"""One MSP frame as captured by the SITL-side observer."""
|
||||
|
||||
monotonic_ms: int
|
||||
function_id: int
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class InavGpsSnapshot:
|
||||
"""Snapshot of iNav's ``gpsSol`` + provider state after replay."""
|
||||
|
||||
fix_type: int
|
||||
num_sat: int
|
||||
provider: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RateReport:
|
||||
"""Observed rate over a window with pass/fail vs spec target."""
|
||||
|
||||
frame_count: int
|
||||
window_ms: int
|
||||
observed_rate_hz: float
|
||||
target_rate_hz: float
|
||||
min_required_hz: float
|
||||
|
||||
@property
|
||||
def passes(self) -> bool:
|
||||
return (
|
||||
self.window_ms > 0
|
||||
and self.observed_rate_hz >= self.min_required_hz
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class InavGpsReport:
|
||||
"""Evaluation of iNav GPS state against AC-3."""
|
||||
|
||||
snapshot: InavGpsSnapshot
|
||||
expected_num_sat: int
|
||||
fix_type_ok: bool
|
||||
provider_ok: bool
|
||||
num_sat_ok: bool
|
||||
|
||||
@property
|
||||
def passes(self) -> bool:
|
||||
return self.fix_type_ok and self.provider_ok and self.num_sat_ok
|
||||
|
||||
|
||||
def count_frames_by_id(samples: Sequence[MspFrameSample]) -> dict[int, int]:
|
||||
"""Tally per MSP function ID."""
|
||||
counts: dict[int, int] = {}
|
||||
for s in samples:
|
||||
counts[s.function_id] = counts.get(s.function_id, 0) + 1
|
||||
return counts
|
||||
|
||||
|
||||
def compute_rate_hz(
|
||||
samples: Sequence[MspFrameSample],
|
||||
*,
|
||||
function_id: int = MSP2_SENSOR_GPS_FUNCTION_ID,
|
||||
target_rate_hz: float = DEFAULT_TARGET_RATE_HZ,
|
||||
min_required_hz: float = MIN_OBSERVED_RATE_HZ,
|
||||
) -> RateReport:
|
||||
"""Compute observed Hz for the given function_id over the sample window.
|
||||
|
||||
The window is ``[first_sample.monotonic_ms, last_sample.monotonic_ms]``
|
||||
inclusive. A window of zero ms (≤1 matching sample) is reported but
|
||||
will not pass.
|
||||
"""
|
||||
if min_required_hz < 0:
|
||||
raise ValueError(f"min_required_hz must be ≥0, got {min_required_hz}")
|
||||
filtered = [s for s in samples if s.function_id == function_id]
|
||||
if len(filtered) < 2:
|
||||
return RateReport(
|
||||
frame_count=len(filtered),
|
||||
window_ms=0,
|
||||
observed_rate_hz=0.0,
|
||||
target_rate_hz=target_rate_hz,
|
||||
min_required_hz=min_required_hz,
|
||||
)
|
||||
window_ms = filtered[-1].monotonic_ms - filtered[0].monotonic_ms
|
||||
if window_ms <= 0:
|
||||
return RateReport(
|
||||
frame_count=len(filtered),
|
||||
window_ms=window_ms,
|
||||
observed_rate_hz=0.0,
|
||||
target_rate_hz=target_rate_hz,
|
||||
min_required_hz=min_required_hz,
|
||||
)
|
||||
# Rate = (count - 1) / (window in seconds); the first frame is the
|
||||
# epoch boundary, subsequent frames define the cadence.
|
||||
observed = (len(filtered) - 1) / (window_ms / 1000.0)
|
||||
return RateReport(
|
||||
frame_count=len(filtered),
|
||||
window_ms=window_ms,
|
||||
observed_rate_hz=observed,
|
||||
target_rate_hz=target_rate_hz,
|
||||
min_required_hz=min_required_hz,
|
||||
)
|
||||
|
||||
|
||||
def evaluate_inav_gps_state(
|
||||
snapshot: InavGpsSnapshot,
|
||||
*,
|
||||
expected_num_sat: int,
|
||||
min_fix_type: int = MIN_FIX_TYPE,
|
||||
required_provider: str = REQUIRED_PROVIDER,
|
||||
) -> InavGpsReport:
|
||||
"""Validate AC-3: fix_type ≥3, provider=MSP, numSat matches emitted value."""
|
||||
if expected_num_sat < 0:
|
||||
raise ValueError(f"expected_num_sat must be ≥0, got {expected_num_sat}")
|
||||
return InavGpsReport(
|
||||
snapshot=snapshot,
|
||||
expected_num_sat=expected_num_sat,
|
||||
fix_type_ok=snapshot.fix_type >= min_fix_type,
|
||||
provider_ok=snapshot.provider == required_provider,
|
||||
num_sat_ok=snapshot.num_sat == expected_num_sat,
|
||||
)
|
||||
Reference in New Issue
Block a user