mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-21 08:41:12 +00:00
[AZ-385] C5 SourceLabelStateMachine + spoof-promotion gate
Implements Invariants 5 + 8 + AC-NEW-2 / AC-NEW-8: the EstimatorOutput.source_label now reflects a real state machine (DEAD_RECKONED → SATELLITE_ANCHORED ↔ VISUAL_PROPAGATED) governed by a spoof-promotion gate that latches closed on FC SPOOFED GPS health and re-opens only when BOTH conditions hold — ≥10 s STABLE_NON_SPOOFED AND next anchor within spoof_promotion_visual_consistency_tol_m. Every reject emits a c5.state.spoof_rejected FDR record plus a subscriber-fan-out STATUSTEXT (severity WARNING, 50-char cap per MAVLink). FDR and subscriber paths bypass the standard logger so silencing logs cannot suppress the spoof trail (R07 / AC-6). GtsamIsam2StateEstimator now eagerly builds the SM from C5StateConfig in __init__; new public methods notify_gps_health() (delegates to SM, called by composition root from C8 inbound) and subscribe_spoof_rejection() (composition root attaches C8's QgcTelemetryAdapter here). health_snapshot.spoof_promotion_blocked + current_estimate.source_label now flow from the live SM. 25 new unit tests across all 12 ACs plus cancellation, subscriber exception isolation, and estimator wire-up integration cases. One AZ-384 test renamed + updated to expect DEAD_RECKONED before any anchor (was VISUAL_PROPAGATED placeholder pre-AZ-385). Full suite: 632 passed, 2 skipped. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -0,0 +1,92 @@
|
||||
# Batch 17 — Cycle 1 Implementation Report
|
||||
|
||||
**Batch**: 17 of N
|
||||
**Tasks landed**: AZ-385 (`SourceLabelStateMachine` + AC-NEW-2 / AC-NEW-8 spoof-promotion gate)
|
||||
**Cycle**: 1
|
||||
**Date**: 2026-05-11
|
||||
|
||||
## Scope
|
||||
|
||||
| Task | Component | Purpose |
|
||||
|------|-----------|---------|
|
||||
| AZ-385 | C5 state estimator | Implements Invariant 5 (`source_label` reflects gate state) + Invariant 8 (spoof-rejection always lands in FDR + GCS STATUSTEXT, unsilenceable) + AC-NEW-2 / AC-NEW-8 (gate opens only when BOTH ≥10 s `STABLE_NON_SPOOFED` AND next anchor within `tol_m`). Adds a new `SourceLabelStateMachine` helper module owned by C5; the estimator constructs one eagerly per instance and exposes two new public surfaces — `notify_gps_health(GpsHealth, now_ns=None)` (forwarded from C8 inbound; transitions the gate latch + the stable-dwell timer) and `subscribe_spoof_rejection(cb) -> RejectionSubscription` (composition root wires C8's `QgcTelemetryAdapter.send_statustext` here so STATUSTEXT mirrors fire on the C8 outbound thread). `health_snapshot.spoof_promotion_blocked` is now backed by the live state machine; `current_estimate.source_label` likewise reflects the live label rather than the AZ-384-era hard-coded default. Every reject emits a `c5.state.spoof_rejected` FDR record AND a subscriber callback in the same call — both paths bypass the standard logger so silencing logs cannot suppress the spoof trail (R07 / AC-6). |
|
||||
|
||||
## Files added / modified
|
||||
|
||||
### Added (prod)
|
||||
|
||||
- `src/gps_denied_onboard/components/c5_state/_source_label_sm.py` — new `SourceLabelStateMachine` class. Public read API: `current_label()`, `is_spoof_promotion_blocked()`. Public write API: `notify_gps_health(gps_health, now_ns=None)` (transitions `_promotion_blocked` on first `SPOOFED`, starts the dwell timer on transition to `STABLE_NON_SPOOFED`, clears it on any other status), `notify_satellite_anchor(now_ns, gps_consistency_delta_m)` (updates `_last_anchored_frame_ns`; while blocked, tries to lift the gate using the BOTH-conditions test, otherwise emits a `c5.state.spoof_rejected` FDR record + subscriber fan-out). Subscription API: `subscribe_rejection(cb) -> RejectionSubscription` with cancellable handle. Internal label rules: `DEAD_RECKONED` until first anchor → `SATELLITE_ANCHORED` if gate open AND anchor < 1 s old → `VISUAL_PROPAGATED` otherwise. Threading: explicit `threading.Lock` around every state mutation; subscriber callbacks dispatched OUTSIDE the lock so a slow subscriber doesn't block writers.
|
||||
|
||||
### Modified (prod)
|
||||
|
||||
- `src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py` — three changes: (1) the constructor now eagerly builds a `SourceLabelStateMachine` from `C5StateConfig.spoof_promotion_min_stable_s` and `spoof_promotion_visual_consistency_tol_m` and the injected `fdr_client`; (2) `add_pose_anchor` on the MARGINALS path now calls a new internal helper `_notify_source_label_anchor(pose)` that forwards the anchor to the SM with `gps_consistency_delta_m=None` (the visual-vs-GPS delta will be supplied by AZ-389 orthorectifier or the composition root once that wiring lands); (3) new public methods `notify_gps_health(gps_health, now_ns=None)` (delegates to the SM) and `subscribe_spoof_rejection(callback) -> RejectionSubscription` (delegates; raises `StateEstimatorConfigError` if `attach_source_label_state_machine` was used with a stub that lacks the subscription surface). The pre-existing `attach_source_label_state_machine` injection point is preserved and re-documented as a test-only override (production code uses the eagerly-built instance).
|
||||
|
||||
### Added (tests)
|
||||
|
||||
- `tests/unit/c5_state/test_az385_source_label_spoof_gate.py` — 25 tests across all 12 ACs plus subscription cancellation, subscriber-exception isolation, and three estimator-wire-up integration tests. Uses a deterministic synthetic `_Clock` (no real wall-clock dependence). Mocks the SM's logger via `mock.patch.object` to prove AC-6 — FDR and subscriber paths fire even when every `logger.*` call is a no-op. The estimator integration tests use the same `_seed_prior` helper pattern from AZ-388 to seed a minimal iSAM2 prior so `current_estimate` returns without raising.
|
||||
|
||||
### Modified (tests)
|
||||
|
||||
- `tests/unit/c5_state/test_az384_marginals_outputs.py` — `test_ac9_default_source_label_is_visual_propagated` renamed to `test_ac9_default_source_label_is_dead_reckoned_before_any_anchor` and updated to expect `DEAD_RECKONED`. The previous behaviour (no SM attached → `VISUAL_PROPAGATED`) was an AZ-384-era placeholder; AZ-385 supersedes it by attaching the real SM eagerly. Inline comment cites AC-1 of AZ-385 + Invariant 5 for traceability.
|
||||
|
||||
## Architectural notes
|
||||
|
||||
- **Eager construction over lazy injection** — AZ-384 left `_source_label_machine` as `Any | None` with `attach_source_label_state_machine` as the wiring seam, on the theory that AZ-385 would own the actual machine instance. With the SM now landed, eagerly constructing it in `__init__` removes the "is it attached yet?" branch from every read path AND lets `health_snapshot.spoof_promotion_blocked` return a meaningful answer from the very first call. `attach_source_label_state_machine` stays available as a test-only override (one test exercises it; see `test_estimator_subscribe_after_attach_with_stub_raises`).
|
||||
- **Subscriber pattern over direct GCS dependency** — same shape as AZ-388's `FallbackWatcher.subscribe_engaged`. The C5 estimator does NOT take a `GcsAdapter` in its constructor; instead C8's `QgcTelemetryAdapter` registers itself via `subscribe_spoof_rejection` at composition time. This keeps the C5 module's import graph free of C8 dependencies and matches the contract's "C8 owns the STATUSTEXT broadcast wire" phrasing.
|
||||
- **Unsilenceable logging path (R07 / AC-6)** — both the FDR record and the subscriber fan-out go through dedicated methods (`FdrClient.enqueue`, direct callback invocation) that do NOT touch the `logging` module's filter chain. The informational `c5.state.spoof_rejected` WARN log is emitted AFTER both side-effects, so even if every `logger.*` call is monkey-patched to a no-op, the FDR + STATUSTEXT trail still lands. Verified by `test_ac6_silenced_logger_does_not_suppress_fdr_or_subscriber`.
|
||||
- **BOTH-conditions promotion (AC-NEW-2 / AC-NEW-8)** — the gate is lifted ONLY when `(now - _gps_health_stable_since_ns) >= min_stable_ns` AND `gps_consistency_delta_m is not None AND <= consistency_tol_m`. Each condition is tested in isolation (`test_ac5_only_stable_dwell_does_not_lift_block`, `test_ac5_only_consistency_does_not_lift_block`) — the AND semantics are not paraphrased into OR by accident.
|
||||
- **Default `gps_consistency_delta_m=None` from the estimator** — the production wire-up cannot compute the visual-vs-GPS delta yet (AZ-389 orthorectifier supplies the rectified ENU position for cross-check; the composition root will own the comparison once that task lands). Until then, ALL in-block anchor attempts emit a reject for `no_gps_observation` — which is the conservative + correct behaviour (you can't unblock until you have evidence). When AZ-389 lands, the composition root will replace the `gps_consistency_delta_m=None` call with the real delta and the dwell+consistency AND-test will be exercised end-to-end.
|
||||
- **Anchor-staleness threshold = 1 s** — encoded as a module-level constant `_ANCHOR_STALENESS_THRESHOLD_MS = 1000` rather than a new config field. The C5 contract's "Config schema additions" section already enumerates the fields the operator may tune; adding a sixth field for an internal SM-only decision would be over-configuration. Documented in the module docstring.
|
||||
- **STATUSTEXT cap at 50 chars (AC-12)** — the formatter pre-caps the message to 50 chars in `_format_statustext`; reason tokens (`gps_spoofed`, `dwell_short`, `consistency`, `no_gps_obs`) are intentionally short so the full `"GPS spoof rejected: <reason>"` fits without truncation in the common case.
|
||||
- **Reject-reason classification ladder** — the four reasons cover the four possible failure modes of the AND-test: GPS still SPOOFED (highest-priority — never check anything else), no GPS observation at all (`gps_consistency_delta_m is None`), dwell insufficient, consistency violation. The ladder is deterministic; tests assert on the reason string per case.
|
||||
|
||||
## Test counts
|
||||
|
||||
| Suite | Before (B16) | After (B17) | Delta |
|
||||
|-------|--------------|-------------|-------|
|
||||
| Total passing | 607 | 632 | +25 |
|
||||
| Skipped | 2 | 2 | 0 |
|
||||
| AZ-385 (new) | 0 | 25 | +25 |
|
||||
| AZ-384 (preserved) | 27 | 27 | 0 (test renamed + flipped expectation; count unchanged) |
|
||||
|
||||
Run command: `PYTHONPATH=src pytest tests/ -q` → `632 passed, 2 skipped in ~32s`.
|
||||
|
||||
## Lint / type
|
||||
|
||||
- `ruff check src/gps_denied_onboard/components/c5_state/ tests/unit/c5_state/` — clean after one auto-fix (import order in `gtsam_isam2_estimator.py`).
|
||||
- `ruff format` — 1 file reformatted, 3 already formatted; second pass clean.
|
||||
- `ReadLints` on touched files — 0 errors.
|
||||
|
||||
## Acceptance evidence
|
||||
|
||||
| AC | Test(s) | Status |
|
||||
|----|---------|--------|
|
||||
| AC-1 Initial label DEAD_RECKONED | `test_ac1_initial_label_is_dead_reckoned`, `test_ac1_initial_label_remains_dead_reckoned_after_gps_only` | PASS |
|
||||
| AC-2 First anchor → SATELLITE_ANCHORED | `test_ac2_first_anchor_promotes_to_satellite_anchored`, `test_ac2_first_anchor_when_blocked_emits_reject` | PASS |
|
||||
| AC-3 Stale anchor → VISUAL_PROPAGATED | `test_ac3_stale_anchor_falls_back_to_visual_propagated` | PASS |
|
||||
| AC-4 Spoof detection latches + emits reject | `test_ac4_spoofed_status_latches_gate_closed`, `test_ac4_reject_fires_subscriber_callback`, `test_ac4_reject_fires_fdr_record` | PASS |
|
||||
| AC-5 BOTH conditions required | `test_ac5_only_stable_dwell_does_not_lift_block`, `test_ac5_only_consistency_does_not_lift_block`, `test_ac5_both_conditions_lift_block` | PASS |
|
||||
| AC-6 Logging cannot be silenced | `test_ac6_silenced_logger_does_not_suppress_fdr_or_subscriber` | PASS |
|
||||
| AC-7 `is_spoof_promotion_blocked()` | `test_ac7_block_query_reflects_latch` | PASS |
|
||||
| AC-8 `health_snapshot.spoof_promotion_blocked` wired | `test_ac8_health_snapshot_reflects_spoof_block` | PASS |
|
||||
| AC-9 Configurable thresholds | `test_ac9_min_stable_30s_shifts_dwell_window`, `test_ac9_consistency_tol_5m_rejects_15m_delta` | PASS |
|
||||
| AC-10 Label-change INFO log | `test_ac10_label_change_emits_info_log` | PASS |
|
||||
| AC-11 Reject FDR record shape | `test_ac11_reject_fdr_record_shape` | PASS |
|
||||
| AC-12 STATUSTEXT severity + 50-char cap | `test_ac12_statustext_severity_is_warning`, `test_ac12_statustext_max_50_chars` | PASS |
|
||||
| Subscription cancellation | `test_subscription_cancel_stops_callbacks` | PASS |
|
||||
| Subscriber exception isolation | `test_subscriber_exception_does_not_break_state_machine` | PASS |
|
||||
| Estimator wire-up — label flows | `test_estimator_current_estimate_label_reflects_sm` | PASS |
|
||||
| Estimator wire-up — subscription delegates | `test_estimator_subscribe_spoof_rejection_returns_handle` | PASS |
|
||||
| Stub injection raises on subscribe | `test_estimator_subscribe_after_attach_with_stub_raises` | PASS |
|
||||
|
||||
## Known gaps / followups
|
||||
|
||||
- **Visual-vs-GPS consistency delta** — currently the estimator passes `None` on every anchor notification. AZ-389 (orthorectifier → C6) will supply the rectified ENU position; the composition root will then compute the delta against the FC GPS WGS84 → ENU position and pass it in via a new method (likely `notify_visual_consistency` on the SM). Until that lands the BOTH-conditions test always falls into the `no_gps_observation` reject reason, which is the conservative + safe default.
|
||||
- **ESKF wire-up** — `EskfStateEstimator` (AZ-386) needs the same SM wire-up (one constructor line + the `add_pose_anchor` hook). Per the AZ-385 contract "both estimators participate"; the AZ-386 wire-up cost will be a single line each.
|
||||
- **C5-IT-06 / C5-IT-07 / C5-ST-01** — scoped out per AZ-385 § Excluded; live in E-BBT.
|
||||
- **C8 outbound STATUSTEXT subscription** — AZ-261 will register the C8 `QgcTelemetryAdapter` via `subscribe_spoof_rejection` at composition time. AZ-385 only exposes the subscription point.
|
||||
|
||||
## Risks accepted
|
||||
|
||||
- **SM lock contention under high anchor rate** — anchor cadence is bounded by D-C5-3 (~3 Hz worst case); lock-acquire is microseconds. Verified informally; if profiling reveals contention later we can swap to a CAS pattern or split into a state-machine-per-input.
|
||||
- **`gps_consistency_delta_m=None` always rejecting under spoof** — by design until AZ-389 lands. The conservative behaviour is "stay blocked until evidence proves otherwise", which is exactly what R07 wants.
|
||||
@@ -8,7 +8,7 @@ status: in_progress
|
||||
sub_step:
|
||||
phase: 6
|
||||
name: implement-tasks
|
||||
detail: "batch 16 of N committed (AZ-388 c5 ac-5.2 fallback: FallbackWatcher + threshold/rate-limit + FDR engagement/recovery + GCS STATUSTEXT severities + watchdog API + subscriber pattern for C8)"
|
||||
detail: "batch 17 of N committed (AZ-385 c5 source-label state machine + spoof-promotion gate: dwell + visual-consistency conditions + unsilenceable FDR/STATUSTEXT reject path + estimator wire-up via notify_gps_health/subscribe_spoof_rejection)"
|
||||
retry_count: 0
|
||||
cycle: 1
|
||||
tracker: jira
|
||||
|
||||
@@ -0,0 +1,443 @@
|
||||
"""AZ-385 ``SourceLabelStateMachine`` — source-label + spoof-promotion gate.
|
||||
|
||||
Per the C5 contract (Invariants 5 + 8 + AC-NEW-2 / AC-NEW-8) the
|
||||
``EstimatorOutput.source_label`` MUST reflect three real states:
|
||||
|
||||
* ``SATELLITE_ANCHORED`` — the spoof-promotion gate is open AND a
|
||||
recent satellite anchor exists.
|
||||
* ``VISUAL_PROPAGATED`` — anchor is stale OR the spoof-promotion gate
|
||||
is closed.
|
||||
* ``DEAD_RECKONED`` — no satellite anchor has ever been observed.
|
||||
|
||||
The gate latches CLOSED on the first FC-reported ``SPOOFED`` GPS
|
||||
health; it re-opens only when BOTH of these are true:
|
||||
|
||||
1. FC GPS health has been ``STABLE_NON_SPOOFED`` for at least
|
||||
``spoof_promotion_min_stable_s`` (default 10 s).
|
||||
2. The next satellite anchor agrees with the FC GPS within
|
||||
``spoof_promotion_visual_consistency_tol_m`` metres (default 30 m).
|
||||
|
||||
Every attempted promotion that fails either condition emits ONE
|
||||
``c5.state.spoof_rejected`` FDR record + fans out a STATUSTEXT
|
||||
callback to subscribers (composition root wires the C8 GCS adapter
|
||||
here). R07 / AC-6 — the FDR and subscriber paths bypass the standard
|
||||
logger, so silencing logs cannot suppress the reject trail.
|
||||
|
||||
Threading: the state machine takes an explicit lock around every
|
||||
state mutation so the composition root's eventual C8-inbound-thread
|
||||
producer (``notify_gps_health``) does not race the C5 ingest-thread
|
||||
producer (``notify_satellite_anchor`` + reads from
|
||||
``current_estimate``). Single-writer thread per Invariant 1 is the
|
||||
deployed contract, but the lock is cheap and makes the unit-test
|
||||
matrix simpler.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import threading
|
||||
import time
|
||||
from collections.abc import Callable
|
||||
from datetime import datetime, timezone
|
||||
from typing import TYPE_CHECKING, Final, Protocol, runtime_checkable
|
||||
|
||||
from gps_denied_onboard._types.fc import GpsStatus, Severity
|
||||
from gps_denied_onboard._types.state import PoseSourceLabel
|
||||
from gps_denied_onboard.fdr_client.records import FdrRecord
|
||||
from gps_denied_onboard.logging import get_logger
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from gps_denied_onboard._types.fc import GpsHealth
|
||||
from gps_denied_onboard.fdr_client.client import FdrClient
|
||||
|
||||
__all__ = [
|
||||
"RejectionCallback",
|
||||
"RejectionSubscription",
|
||||
"SourceLabelStateMachine",
|
||||
]
|
||||
|
||||
|
||||
# Subscriber signature — composition root receives
|
||||
# (reason, severity, statustext) on every reject. ``severity`` is
|
||||
# always ``WARNING`` per AC-12. ``statustext`` is pre-formatted and
|
||||
# capped at 50 chars (MAVLink ``STATUSTEXT.text`` max).
|
||||
RejectionCallback = Callable[[str, Severity, str], None]
|
||||
|
||||
|
||||
# AC-12 MAVLink STATUSTEXT max payload length (50 chars; longer texts
|
||||
# are silently truncated on the wire).
|
||||
_STATUSTEXT_MAX_LEN: Final[int] = 50
|
||||
|
||||
# AC-12 STATUSTEXT severity — WARNING for spoof rejections.
|
||||
_REJECT_SEVERITY: Final[Severity] = Severity.WARNING
|
||||
|
||||
# Anchor-staleness threshold for AC-3 ``VISUAL_PROPAGATED`` (ms).
|
||||
# Anchors older than this fall back to the visual-propagated label
|
||||
# even when the gate is open. The contract spec lists the threshold
|
||||
# as configurable in principle, but the only consumer (this state
|
||||
# machine) treats it as an implementation constant tied to the
|
||||
# D-C5-3 keyframe cadence (~3 Hz → 1 s between anchors at worst).
|
||||
_ANCHOR_STALENESS_THRESHOLD_MS: Final[int] = 1000
|
||||
|
||||
# Reject reason codes — short tokens that fit into the 50-char
|
||||
# STATUSTEXT budget AFTER the ``"GPS spoof rejected: "`` prefix.
|
||||
_REASON_GPS_STILL_SPOOFED: Final[str] = "gps_spoofed"
|
||||
_REASON_DWELL_INSUFFICIENT: Final[str] = "dwell_short"
|
||||
_REASON_CONSISTENCY_VIOLATION: Final[str] = "consistency"
|
||||
_REASON_NO_GPS_OBSERVATION: Final[str] = "no_gps_obs"
|
||||
|
||||
|
||||
@runtime_checkable
|
||||
class RejectionSubscription(Protocol):
|
||||
"""Handle returned by :meth:`SourceLabelStateMachine.subscribe_rejection`.
|
||||
|
||||
Calling :meth:`cancel` removes the callback from the next
|
||||
rejection dispatch. Subsequent cancels are no-ops.
|
||||
"""
|
||||
|
||||
def cancel(self) -> None: ...
|
||||
|
||||
|
||||
class _Subscription:
|
||||
def __init__(
|
||||
self,
|
||||
registry: dict[int, RejectionCallback],
|
||||
sub_id: int,
|
||||
lock: threading.Lock,
|
||||
) -> None:
|
||||
self._registry = registry
|
||||
self._sub_id = sub_id
|
||||
self._lock = lock
|
||||
|
||||
def cancel(self) -> None:
|
||||
with self._lock:
|
||||
self._registry.pop(self._sub_id, None)
|
||||
|
||||
|
||||
class SourceLabelStateMachine:
|
||||
"""Source-label + spoof-promotion state machine. One per estimator.
|
||||
|
||||
Public API:
|
||||
|
||||
* :meth:`notify_gps_health` — feed FC-reported GPS health; updates
|
||||
the spoof-block latch + the stable-dwell timer.
|
||||
* :meth:`notify_satellite_anchor` — feed a new satellite anchor;
|
||||
may lift the spoof block (when BOTH AC-NEW-2 conditions hold)
|
||||
or emit a reject record (otherwise, while blocked).
|
||||
* :meth:`current_label` — pure read; returns the current label.
|
||||
* :meth:`is_spoof_promotion_blocked` — pure read; returns the
|
||||
gate state.
|
||||
* :meth:`subscribe_rejection` — register a callback for the
|
||||
STATUSTEXT mirror; returns a cancel handle.
|
||||
|
||||
The label is recomputed lazily on every read (``current_label``),
|
||||
using the cached anchor timestamp + block state + clock. The
|
||||
machine never mutates state from a read; only the two
|
||||
``notify_*`` methods change anything observable.
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
spoof_promotion_min_stable_s: float,
|
||||
spoof_promotion_visual_consistency_tol_m: float,
|
||||
fdr_client: FdrClient | None,
|
||||
producer_id: str = "c5_state",
|
||||
clock_ns: Callable[[], int] = time.monotonic_ns,
|
||||
) -> None:
|
||||
if spoof_promotion_min_stable_s <= 0.0:
|
||||
raise ValueError(
|
||||
"SourceLabelStateMachine.spoof_promotion_min_stable_s must be > 0; "
|
||||
f"got {spoof_promotion_min_stable_s}"
|
||||
)
|
||||
if spoof_promotion_visual_consistency_tol_m <= 0.0:
|
||||
raise ValueError(
|
||||
"SourceLabelStateMachine.spoof_promotion_visual_consistency_tol_m "
|
||||
f"must be > 0; got {spoof_promotion_visual_consistency_tol_m}"
|
||||
)
|
||||
self._min_stable_ns: int = int(spoof_promotion_min_stable_s * 1_000_000_000)
|
||||
self._consistency_tol_m: float = spoof_promotion_visual_consistency_tol_m
|
||||
self._fdr_client: FdrClient | None = fdr_client
|
||||
self._producer_id: str = producer_id
|
||||
self._clock_ns: Callable[[], int] = clock_ns
|
||||
self._log = get_logger("c5_state.source_label_sm")
|
||||
|
||||
self._lock = threading.Lock()
|
||||
# Cached state — all writes go through the two ``notify_*``
|
||||
# methods. Reads from ``current_label`` / ``is_spoof_promotion_blocked``
|
||||
# snapshot under the same lock for consistency.
|
||||
self._last_anchored_frame_ns: int | None = None
|
||||
self._gps_status: GpsStatus | None = None
|
||||
# Set when the FC reports STABLE_NON_SPOOFED; cleared on any
|
||||
# other status. The dwell test is ``now - stable_since_ns >=
|
||||
# min_stable_ns``.
|
||||
self._gps_health_stable_since_ns: int | None = None
|
||||
self._promotion_blocked: bool = False
|
||||
self._last_label: PoseSourceLabel = PoseSourceLabel.DEAD_RECKONED
|
||||
|
||||
# Subscriber registry — composition root attaches C8's
|
||||
# ``QgcTelemetryAdapter.send_statustext`` here so STATUSTEXT
|
||||
# mirrors fire on the C8 outbound thread.
|
||||
self._rejection_subs: dict[int, RejectionCallback] = {}
|
||||
self._next_sub_id: int = 1
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Read API — pure; never mutates.
|
||||
|
||||
def current_label(self) -> PoseSourceLabel:
|
||||
"""Recompute + return the current source label.
|
||||
|
||||
Recomputation rules (per AC-1/2/3 + Invariant 5):
|
||||
|
||||
* No anchor ever observed → ``DEAD_RECKONED``.
|
||||
* Spoof gate latched closed → ``VISUAL_PROPAGATED``.
|
||||
* Last anchor older than ``_ANCHOR_STALENESS_THRESHOLD_MS``
|
||||
→ ``VISUAL_PROPAGATED``.
|
||||
* Else → ``SATELLITE_ANCHORED``.
|
||||
"""
|
||||
with self._lock:
|
||||
label = self._recompute_label_locked()
|
||||
self._last_label = label
|
||||
return label
|
||||
|
||||
def is_spoof_promotion_blocked(self) -> bool:
|
||||
"""Return whether the spoof-promotion gate is currently closed."""
|
||||
with self._lock:
|
||||
return self._promotion_blocked
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Write API — both methods may transition state.
|
||||
|
||||
def notify_gps_health(self, gps_health: GpsHealth, now_ns: int | None = None) -> None:
|
||||
"""Feed a new ``GpsHealth`` observation.
|
||||
|
||||
On a SPOOFED → True transition the gate latches closed and one
|
||||
``c5.state.spoof_gate_engaged`` INFO log fires. On a transition
|
||||
to ``STABLE_NON_SPOOFED`` the dwell timer starts; on any other
|
||||
status the dwell timer clears.
|
||||
"""
|
||||
ts = now_ns if now_ns is not None else self._clock_ns()
|
||||
transition_info: tuple[bool, str, str] | None = None # (block_engaged, from, to)
|
||||
with self._lock:
|
||||
prev_status = self._gps_status
|
||||
self._gps_status = gps_health.status
|
||||
if gps_health.status == GpsStatus.STABLE_NON_SPOOFED:
|
||||
if prev_status != GpsStatus.STABLE_NON_SPOOFED:
|
||||
self._gps_health_stable_since_ns = ts
|
||||
else:
|
||||
self._gps_health_stable_since_ns = None
|
||||
if gps_health.status == GpsStatus.SPOOFED and not self._promotion_blocked:
|
||||
self._promotion_blocked = True
|
||||
transition_info = (
|
||||
True,
|
||||
(prev_status.value if prev_status is not None else "init"),
|
||||
gps_health.status.value,
|
||||
)
|
||||
|
||||
if transition_info is not None:
|
||||
_engaged, prev, curr = transition_info
|
||||
self._log.warning(
|
||||
"c5.state.spoof_gate_engaged",
|
||||
extra={
|
||||
"kind": "c5.state.spoof_gate_engaged",
|
||||
"kv": {"from": prev, "to": curr},
|
||||
},
|
||||
)
|
||||
|
||||
def notify_satellite_anchor(
|
||||
self,
|
||||
now_ns: int,
|
||||
gps_consistency_delta_m: float | None,
|
||||
) -> None:
|
||||
"""Feed a new satellite anchor.
|
||||
|
||||
While the gate is open this is a pure bookkeeping call —
|
||||
updates ``_last_anchored_frame_ns`` + may transition the
|
||||
label from ``DEAD_RECKONED`` → ``SATELLITE_ANCHORED``.
|
||||
|
||||
While the gate is closed this is also a *promotion attempt*:
|
||||
if both AC-NEW-2 conditions hold (≥10 s STABLE_NON_SPOOFED
|
||||
AND consistency within tol_m), the gate re-opens and one
|
||||
``c5.state.spoof_gate_lifted`` INFO log fires; otherwise one
|
||||
``c5.state.spoof_rejected`` FDR record + STATUSTEXT mirror
|
||||
fires (AC-6 — unsilenceable; bypasses the standard logger).
|
||||
"""
|
||||
emit_reject: tuple[str, str, float, float | None] | None = None
|
||||
emit_gate_lift: bool = False
|
||||
emit_label_change: tuple[PoseSourceLabel, PoseSourceLabel] | None = None
|
||||
with self._lock:
|
||||
prev_label = self._last_label
|
||||
self._last_anchored_frame_ns = now_ns
|
||||
if self._promotion_blocked:
|
||||
stable_ok = self._stable_long_enough_locked(now_ns)
|
||||
consistency_ok = (
|
||||
gps_consistency_delta_m is not None
|
||||
and gps_consistency_delta_m <= self._consistency_tol_m
|
||||
)
|
||||
if stable_ok and consistency_ok:
|
||||
self._promotion_blocked = False
|
||||
self._gps_health_stable_since_ns = None
|
||||
emit_gate_lift = True
|
||||
else:
|
||||
reason = self._classify_reject_reason_locked(
|
||||
stable_ok=stable_ok,
|
||||
consistency_ok=consistency_ok,
|
||||
gps_consistency_delta_m=gps_consistency_delta_m,
|
||||
)
|
||||
time_since_stable_s = self._time_since_stable_s_locked(now_ns)
|
||||
gps_status_str = (
|
||||
self._gps_status.value if self._gps_status is not None else "unknown"
|
||||
)
|
||||
emit_reject = (
|
||||
reason,
|
||||
gps_status_str,
|
||||
time_since_stable_s,
|
||||
gps_consistency_delta_m,
|
||||
)
|
||||
new_label = self._recompute_label_locked()
|
||||
if new_label != prev_label:
|
||||
self._last_label = new_label
|
||||
emit_label_change = (prev_label, new_label)
|
||||
|
||||
if emit_reject is not None:
|
||||
self._emit_reject(*emit_reject)
|
||||
if emit_gate_lift:
|
||||
self._log.info(
|
||||
"c5.state.spoof_gate_lifted",
|
||||
extra={
|
||||
"kind": "c5.state.spoof_gate_lifted",
|
||||
"kv": {"consistency_tol_m": self._consistency_tol_m},
|
||||
},
|
||||
)
|
||||
if emit_label_change is not None:
|
||||
prev, curr = emit_label_change
|
||||
self._log.info(
|
||||
"c5.state.source_label_changed",
|
||||
extra={
|
||||
"kind": "c5.state.source_label_changed",
|
||||
"kv": {
|
||||
"from": prev.value,
|
||||
"to": curr.value,
|
||||
"reason": "anchor_event",
|
||||
},
|
||||
},
|
||||
)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Subscription API.
|
||||
|
||||
def subscribe_rejection(self, callback: RejectionCallback) -> RejectionSubscription:
|
||||
"""Register a callback invoked once per spoof-rejection event."""
|
||||
with self._lock:
|
||||
sub_id = self._next_sub_id
|
||||
self._next_sub_id += 1
|
||||
self._rejection_subs[sub_id] = callback
|
||||
return _Subscription(self._rejection_subs, sub_id, self._lock)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Internal helpers — all run under the lock unless suffixed
|
||||
# ``_unlocked``.
|
||||
|
||||
def _recompute_label_locked(self) -> PoseSourceLabel:
|
||||
if self._last_anchored_frame_ns is None:
|
||||
return PoseSourceLabel.DEAD_RECKONED
|
||||
if self._promotion_blocked:
|
||||
return PoseSourceLabel.VISUAL_PROPAGATED
|
||||
now_ns = self._clock_ns()
|
||||
age_ms = (now_ns - self._last_anchored_frame_ns) / 1_000_000
|
||||
if age_ms > _ANCHOR_STALENESS_THRESHOLD_MS:
|
||||
return PoseSourceLabel.VISUAL_PROPAGATED
|
||||
return PoseSourceLabel.SATELLITE_ANCHORED
|
||||
|
||||
def _stable_long_enough_locked(self, now_ns: int) -> bool:
|
||||
if self._gps_health_stable_since_ns is None:
|
||||
return False
|
||||
return (now_ns - self._gps_health_stable_since_ns) >= self._min_stable_ns
|
||||
|
||||
def _time_since_stable_s_locked(self, now_ns: int) -> float:
|
||||
if self._gps_health_stable_since_ns is None:
|
||||
return 0.0
|
||||
return (now_ns - self._gps_health_stable_since_ns) / 1_000_000_000
|
||||
|
||||
def _classify_reject_reason_locked(
|
||||
self,
|
||||
*,
|
||||
stable_ok: bool,
|
||||
consistency_ok: bool,
|
||||
gps_consistency_delta_m: float | None,
|
||||
) -> str:
|
||||
if self._gps_status == GpsStatus.SPOOFED:
|
||||
return _REASON_GPS_STILL_SPOOFED
|
||||
if gps_consistency_delta_m is None:
|
||||
return _REASON_NO_GPS_OBSERVATION
|
||||
if not stable_ok:
|
||||
return _REASON_DWELL_INSUFFICIENT
|
||||
if not consistency_ok:
|
||||
return _REASON_CONSISTENCY_VIOLATION
|
||||
return _REASON_DWELL_INSUFFICIENT
|
||||
|
||||
def _emit_reject(
|
||||
self,
|
||||
reason: str,
|
||||
gps_status: str,
|
||||
time_since_stable_s: float,
|
||||
gps_consistency_delta_m: float | None,
|
||||
) -> None:
|
||||
# AC-6: FDR + subscriber paths bypass the standard logger so
|
||||
# silencing logs cannot suppress the spoof-rejection trail.
|
||||
record = FdrRecord(
|
||||
schema_version=1,
|
||||
ts=datetime.now(tz=timezone.utc).isoformat(),
|
||||
producer_id=self._producer_id,
|
||||
kind="c5.state.spoof_rejected",
|
||||
payload={
|
||||
"reason": reason,
|
||||
"gps_health": gps_status,
|
||||
"time_since_stable_s": time_since_stable_s,
|
||||
"visual_consistency_delta_m": gps_consistency_delta_m,
|
||||
},
|
||||
)
|
||||
if self._fdr_client is not None:
|
||||
try:
|
||||
self._fdr_client.enqueue(record)
|
||||
except Exception as exc:
|
||||
self._log.debug(
|
||||
"c5.state.spoof_rejected_fdr_enqueue_failed",
|
||||
extra={
|
||||
"kind": "c5.state.spoof_rejected_fdr_enqueue_failed",
|
||||
"kv": {"error": repr(exc)},
|
||||
},
|
||||
)
|
||||
text = _format_statustext(reason)
|
||||
with self._lock:
|
||||
subs = list(self._rejection_subs.values())
|
||||
for cb in subs:
|
||||
try:
|
||||
cb(reason, _REJECT_SEVERITY, text)
|
||||
except Exception as exc:
|
||||
self._log.debug(
|
||||
"c5.state.spoof_rejected_callback_failed",
|
||||
extra={
|
||||
"kind": "c5.state.spoof_rejected_callback_failed",
|
||||
"kv": {"error": repr(exc)},
|
||||
},
|
||||
)
|
||||
self._log.warning(
|
||||
"c5.state.spoof_rejected",
|
||||
extra={
|
||||
"kind": "c5.state.spoof_rejected",
|
||||
"kv": {
|
||||
"reason": reason,
|
||||
"gps_health": gps_status,
|
||||
"time_since_stable_s": time_since_stable_s,
|
||||
"visual_consistency_delta_m": gps_consistency_delta_m,
|
||||
},
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
def _format_statustext(reason: str) -> str:
|
||||
"""Build the 50-char MAVLink STATUSTEXT payload for a reject reason."""
|
||||
text = f"GPS spoof rejected: {reason}"
|
||||
if len(text) > _STATUSTEXT_MAX_LEN:
|
||||
text = text[:_STATUSTEXT_MAX_LEN]
|
||||
return text
|
||||
@@ -58,6 +58,11 @@ from gps_denied_onboard.components.c5_state._isam2_handle import (
|
||||
ISam2GraphHandle,
|
||||
ISam2GraphHandleImpl,
|
||||
)
|
||||
from gps_denied_onboard.components.c5_state._source_label_sm import (
|
||||
RejectionCallback,
|
||||
RejectionSubscription,
|
||||
SourceLabelStateMachine,
|
||||
)
|
||||
from gps_denied_onboard.components.c5_state.config import C5StateConfig
|
||||
from gps_denied_onboard.components.c5_state.errors import (
|
||||
EstimatorDegradedError,
|
||||
@@ -69,6 +74,7 @@ from gps_denied_onboard.helpers.wgs_converter import WgsConverter
|
||||
from gps_denied_onboard.logging import get_logger
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from gps_denied_onboard._types.fc import GpsHealth
|
||||
from gps_denied_onboard._types.nav import ImuWindow
|
||||
from gps_denied_onboard._types.pose import PoseEstimate
|
||||
from gps_denied_onboard._types.vio import VioOutput
|
||||
@@ -194,11 +200,20 @@ class GtsamIsam2StateEstimator(StateEstimator):
|
||||
# origin via :meth:`set_enu_origin` from the first satellite
|
||||
# anchor.
|
||||
self._enu_origin: LatLonAlt | None = None
|
||||
# Source-label state machine (AZ-385). When None,
|
||||
# ``current_estimate`` emits ``VISUAL_PROPAGATED`` per the
|
||||
# contract default and ``health_snapshot`` reports
|
||||
# ``spoof_promotion_blocked=False``.
|
||||
self._source_label_machine: Any | None = None
|
||||
# Source-label state machine (AZ-385). Constructed eagerly
|
||||
# so ``current_estimate`` + ``health_snapshot`` always have a
|
||||
# real machine to query. The composition root feeds GPS
|
||||
# health updates via :meth:`notify_gps_health` (sourced from
|
||||
# C8 inbound, AZ-391) and subscribes the C8 GCS adapter to
|
||||
# rejection events via :meth:`subscribe_spoof_rejection`.
|
||||
# :meth:`attach_source_label_state_machine` remains available
|
||||
# as an override for tests that need to inject a stub.
|
||||
self._source_label_machine: SourceLabelStateMachine = SourceLabelStateMachine(
|
||||
spoof_promotion_min_stable_s=block.spoof_promotion_min_stable_s,
|
||||
spoof_promotion_visual_consistency_tol_m=block.spoof_promotion_visual_consistency_tol_m,
|
||||
fdr_client=fdr_client,
|
||||
producer_id="c5_state",
|
||||
)
|
||||
# AC-NEW-8 rolling window of ``(ts_monotonic_ns, cov_norm)``
|
||||
# tuples for ``cov_norm_growing_for_s`` accounting.
|
||||
self._cov_norm_window: deque[tuple[int, float]] = deque()
|
||||
@@ -270,14 +285,50 @@ class GtsamIsam2StateEstimator(StateEstimator):
|
||||
self._enu_origin = origin
|
||||
|
||||
def attach_source_label_state_machine(self, machine: Any) -> None:
|
||||
"""Wire the AZ-385 source-label / spoof-promotion state machine.
|
||||
"""Override the AZ-385 source-label / spoof-promotion state machine.
|
||||
|
||||
Reserved for tests that need to inject a stub or alternative
|
||||
implementation. In production the estimator constructs a
|
||||
:class:`SourceLabelStateMachine` in ``__init__`` already; this
|
||||
override REPLACES that instance. The composition root should
|
||||
NOT call this in steady-state — it should drive the
|
||||
pre-built machine via :meth:`notify_gps_health` and
|
||||
:meth:`subscribe_spoof_rejection`.
|
||||
|
||||
The injected object MUST expose ``current_label() -> PoseSourceLabel``
|
||||
and ``is_spoof_promotion_blocked() -> bool``. AZ-384 only holds
|
||||
the reference; AZ-385 owns the actual transition logic.
|
||||
and ``is_spoof_promotion_blocked() -> bool``.
|
||||
"""
|
||||
self._source_label_machine = machine
|
||||
|
||||
def notify_gps_health(self, gps_health: GpsHealth, now_ns: int | None = None) -> None:
|
||||
"""Forward an FC ``GpsHealth`` observation to the AZ-385 state machine.
|
||||
|
||||
Composition root wires C8 inbound's GPS-health subscription to
|
||||
this method. The machine may transition the spoof-promotion
|
||||
gate as a side-effect.
|
||||
"""
|
||||
machine = self._source_label_machine
|
||||
if isinstance(machine, SourceLabelStateMachine):
|
||||
machine.notify_gps_health(gps_health, now_ns=now_ns)
|
||||
|
||||
def subscribe_spoof_rejection(self, callback: RejectionCallback) -> RejectionSubscription:
|
||||
"""Subscribe to AZ-385 spoof-rejection events.
|
||||
|
||||
Composition root attaches C8's ``QgcTelemetryAdapter`` here so
|
||||
every reject mirrors as a MAVLink STATUSTEXT (severity
|
||||
WARNING, capped at 50 chars per AC-12). See
|
||||
:class:`SourceLabelStateMachine` for the exact callback
|
||||
signature.
|
||||
"""
|
||||
machine = self._source_label_machine
|
||||
if not isinstance(machine, SourceLabelStateMachine):
|
||||
raise StateEstimatorConfigError(
|
||||
"subscribe_spoof_rejection requires the built-in "
|
||||
"SourceLabelStateMachine; replace via attach_source_label_state_machine "
|
||||
"with an instance that supports subscribe_rejection if needed."
|
||||
)
|
||||
return machine.subscribe_rejection(callback)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# AZ-388: AC-5.2 fallback public API.
|
||||
|
||||
@@ -447,6 +498,7 @@ class GtsamIsam2StateEstimator(StateEstimator):
|
||||
raise EstimatorDegradedError(f"add_pose_anchor failed: {exc}") from exc
|
||||
self._reset_staging()
|
||||
self._record_committed_pose_key(pose_key)
|
||||
self._notify_source_label_anchor(pose)
|
||||
self._log.debug(
|
||||
"c5.state.add_pose_anchor_ok",
|
||||
extra={
|
||||
@@ -890,6 +942,33 @@ class GtsamIsam2StateEstimator(StateEstimator):
|
||||
except Exception:
|
||||
return (0.0, 0.0, 0.0)
|
||||
|
||||
def _notify_source_label_anchor(self, pose: PoseEstimate) -> None:
|
||||
"""Forward a successful satellite-anchor add to the AZ-385 SM.
|
||||
|
||||
The estimator does not currently compute the visual-vs-GPS
|
||||
consistency delta itself (AZ-389 orthorectifier or the
|
||||
composition root will supply it externally once available).
|
||||
Until then we pass ``None``, which causes the state machine
|
||||
to classify any in-block promotion attempt as
|
||||
``no_gps_observation`` — the conservative reject path.
|
||||
"""
|
||||
machine = self._source_label_machine
|
||||
if not isinstance(machine, SourceLabelStateMachine):
|
||||
return
|
||||
try:
|
||||
machine.notify_satellite_anchor(
|
||||
now_ns=time.monotonic_ns(),
|
||||
gps_consistency_delta_m=None,
|
||||
)
|
||||
except Exception as exc:
|
||||
self._log.error(
|
||||
"c5.state.source_label_anchor_notify_failed",
|
||||
extra={
|
||||
"kind": "c5.state.source_label_anchor_notify_failed",
|
||||
"kv": {"frame_id": str(pose.frame_id), "error": str(exc)},
|
||||
},
|
||||
)
|
||||
|
||||
def _derive_source_label(self) -> PoseSourceLabel:
|
||||
"""Read the source label from the AZ-385 state machine if wired.
|
||||
|
||||
|
||||
@@ -412,13 +412,16 @@ def test_ac9_state_machine_drives_source_label() -> None:
|
||||
assert out.source_label == PoseSourceLabel.SATELLITE_ANCHORED
|
||||
|
||||
|
||||
def test_ac9_default_source_label_is_visual_propagated() -> None:
|
||||
def test_ac9_default_source_label_is_dead_reckoned_before_any_anchor() -> None:
|
||||
# AZ-385 superseded the AZ-384 default: the auto-constructed
|
||||
# SourceLabelStateMachine returns DEAD_RECKONED until the first
|
||||
# satellite anchor is observed (AC-1 of AZ-385 + Invariant 5).
|
||||
estimator = _build_estimator()
|
||||
_seed_prior(estimator)
|
||||
|
||||
out = estimator.current_estimate()
|
||||
|
||||
assert out.source_label == PoseSourceLabel.VISUAL_PROPAGATED
|
||||
assert out.source_label == PoseSourceLabel.DEAD_RECKONED
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
|
||||
@@ -0,0 +1,520 @@
|
||||
"""AZ-385 — SourceLabelStateMachine + spoof-promotion gate.
|
||||
|
||||
Twelve ACs from ``_docs/02_tasks/todo/AZ-385_c5_source_label_spoof_gate.md``:
|
||||
|
||||
- AC-1 Initial label is ``DEAD_RECKONED``.
|
||||
- AC-2 First successful anchor → ``SATELLITE_ANCHORED``.
|
||||
- AC-3 Stale anchor → ``VISUAL_PROPAGATED`` (anchor age > 1 s).
|
||||
- AC-4 Spoof detection latches the gate + every reject emits FDR +
|
||||
STATUSTEXT subscriber callback.
|
||||
- AC-5 Recovery requires BOTH ≥10 s ``STABLE_NON_SPOOFED`` AND a
|
||||
next anchor within ``tol_m``.
|
||||
- AC-6 Logging cannot be silenced — mocking the logger to drop
|
||||
everything still fires FDR + subscriber paths.
|
||||
- AC-7 ``is_spoof_promotion_blocked()`` mirrors the gate latch.
|
||||
- AC-8 ``health_snapshot.spoof_promotion_blocked`` wired through.
|
||||
- AC-9 Configurable thresholds (``min_stable_s = 30`` shifts the
|
||||
dwell test).
|
||||
- AC-10 Every label change emits one ``c5.state.source_label_changed``
|
||||
INFO log.
|
||||
- AC-11 Reject FDR record has the documented payload shape.
|
||||
- AC-12 STATUSTEXT severity is ``WARNING`` + message fits 50 chars.
|
||||
|
||||
The 13th block covers the wire-up into ``GtsamIsam2StateEstimator``:
|
||||
the estimator constructs the SM eagerly, ``notify_gps_health`` is
|
||||
forwarded, ``subscribe_spoof_rejection`` returns a cancellable
|
||||
handle, and ``health_snapshot.spoof_promotion_blocked`` reflects the
|
||||
gate state.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from datetime import datetime, timezone
|
||||
from unittest import mock
|
||||
|
||||
import gtsam
|
||||
import pytest
|
||||
|
||||
from gps_denied_onboard._types.fc import GpsHealth, GpsStatus, Severity
|
||||
from gps_denied_onboard._types.state import PoseSourceLabel
|
||||
from gps_denied_onboard.components.c5_state._source_label_sm import (
|
||||
SourceLabelStateMachine,
|
||||
)
|
||||
from gps_denied_onboard.components.c5_state.config import C5StateConfig
|
||||
from gps_denied_onboard.components.c5_state.errors import StateEstimatorConfigError
|
||||
from gps_denied_onboard.components.c5_state.gtsam_isam2_estimator import (
|
||||
GtsamIsam2StateEstimator,
|
||||
create,
|
||||
)
|
||||
from gps_denied_onboard.runtime_root.state_factory import clear_state_registry
|
||||
|
||||
|
||||
@pytest.fixture(autouse=True)
|
||||
def _registry_isolation():
|
||||
# Arrange
|
||||
clear_state_registry()
|
||||
yield
|
||||
clear_state_registry()
|
||||
|
||||
|
||||
class _Clock:
|
||||
"""Synthetic ``monotonic_ns()`` source for deterministic timelines."""
|
||||
|
||||
def __init__(self, t_ns: int = 0) -> None:
|
||||
self.t_ns = t_ns
|
||||
|
||||
def __call__(self) -> int:
|
||||
return self.t_ns
|
||||
|
||||
|
||||
def _make_sm(
|
||||
*,
|
||||
min_stable_s: float = 10.0,
|
||||
tol_m: float = 30.0,
|
||||
clock: _Clock | None = None,
|
||||
fdr_client: mock.MagicMock | None = None,
|
||||
) -> tuple[SourceLabelStateMachine, _Clock, mock.MagicMock]:
|
||||
if clock is None:
|
||||
clock = _Clock(0)
|
||||
fdr = fdr_client if fdr_client is not None else mock.MagicMock()
|
||||
sm = SourceLabelStateMachine(
|
||||
spoof_promotion_min_stable_s=min_stable_s,
|
||||
spoof_promotion_visual_consistency_tol_m=tol_m,
|
||||
fdr_client=fdr,
|
||||
producer_id="c5_state",
|
||||
clock_ns=clock,
|
||||
)
|
||||
return sm, clock, fdr
|
||||
|
||||
|
||||
def _gps(
|
||||
status: GpsStatus,
|
||||
fix_age_ms: int = 100,
|
||||
captured_at: int = 0,
|
||||
) -> GpsHealth:
|
||||
return GpsHealth(status=status, fix_age_ms=fix_age_ms, captured_at=captured_at)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-1: initial label is DEAD_RECKONED
|
||||
|
||||
|
||||
def test_ac1_initial_label_is_dead_reckoned() -> None:
|
||||
sm, _clock, _fdr = _make_sm()
|
||||
|
||||
assert sm.current_label() == PoseSourceLabel.DEAD_RECKONED
|
||||
|
||||
|
||||
def test_ac1_initial_label_remains_dead_reckoned_after_gps_only() -> None:
|
||||
sm, clock, _fdr = _make_sm()
|
||||
clock.t_ns = int(1e9)
|
||||
sm.notify_gps_health(_gps(GpsStatus.STABLE_NON_SPOOFED))
|
||||
|
||||
assert sm.current_label() == PoseSourceLabel.DEAD_RECKONED
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-2: first successful anchor → SATELLITE_ANCHORED
|
||||
|
||||
|
||||
def test_ac2_first_anchor_promotes_to_satellite_anchored() -> None:
|
||||
sm, clock, _fdr = _make_sm()
|
||||
clock.t_ns = int(1e9)
|
||||
sm.notify_satellite_anchor(now_ns=clock.t_ns, gps_consistency_delta_m=None)
|
||||
|
||||
assert sm.current_label() == PoseSourceLabel.SATELLITE_ANCHORED
|
||||
|
||||
|
||||
def test_ac2_first_anchor_when_blocked_emits_reject() -> None:
|
||||
sm, clock, fdr = _make_sm()
|
||||
clock.t_ns = int(0.5 * 1e9)
|
||||
sm.notify_gps_health(_gps(GpsStatus.SPOOFED))
|
||||
clock.t_ns = int(1e9)
|
||||
sm.notify_satellite_anchor(now_ns=clock.t_ns, gps_consistency_delta_m=None)
|
||||
|
||||
# The block latched on the SPOOFED notify → the anchor that
|
||||
# comes in next is a promotion attempt against a closed gate →
|
||||
# one reject + label stays VISUAL_PROPAGATED.
|
||||
assert sm.current_label() == PoseSourceLabel.VISUAL_PROPAGATED
|
||||
fdr.enqueue.assert_called_once()
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-3: stale anchor → VISUAL_PROPAGATED
|
||||
|
||||
|
||||
def test_ac3_stale_anchor_falls_back_to_visual_propagated() -> None:
|
||||
# Default anchor staleness threshold is 1 s; here we anchor
|
||||
# at t=1s and check at t=2.5s — the anchor is 1.5s old → VP.
|
||||
sm, clock, _fdr = _make_sm()
|
||||
clock.t_ns = int(1e9)
|
||||
sm.notify_satellite_anchor(now_ns=clock.t_ns, gps_consistency_delta_m=None)
|
||||
assert sm.current_label() == PoseSourceLabel.SATELLITE_ANCHORED
|
||||
|
||||
clock.t_ns = int(2.5 * 1e9)
|
||||
assert sm.current_label() == PoseSourceLabel.VISUAL_PROPAGATED
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-4: spoof detection → block promotion + reject emits FDR + STATUSTEXT
|
||||
|
||||
|
||||
def test_ac4_spoofed_status_latches_gate_closed() -> None:
|
||||
sm, clock, _fdr = _make_sm()
|
||||
clock.t_ns = int(1e9)
|
||||
sm.notify_gps_health(_gps(GpsStatus.SPOOFED))
|
||||
|
||||
assert sm.is_spoof_promotion_blocked() is True
|
||||
|
||||
|
||||
def test_ac4_reject_fires_subscriber_callback() -> None:
|
||||
sm, clock, _fdr = _make_sm()
|
||||
seen: list[tuple[str, Severity, str]] = []
|
||||
sm.subscribe_rejection(lambda reason, sev, text: seen.append((reason, sev, text)))
|
||||
|
||||
clock.t_ns = int(0.5 * 1e9)
|
||||
sm.notify_gps_health(_gps(GpsStatus.SPOOFED))
|
||||
clock.t_ns = int(1e9)
|
||||
sm.notify_satellite_anchor(now_ns=clock.t_ns, gps_consistency_delta_m=None)
|
||||
|
||||
assert len(seen) == 1
|
||||
reason, sev, text = seen[0]
|
||||
assert reason == "gps_spoofed"
|
||||
assert sev == Severity.WARNING
|
||||
assert text.startswith("GPS spoof rejected: ")
|
||||
|
||||
|
||||
def test_ac4_reject_fires_fdr_record() -> None:
|
||||
sm, clock, fdr = _make_sm()
|
||||
clock.t_ns = int(0.5 * 1e9)
|
||||
sm.notify_gps_health(_gps(GpsStatus.SPOOFED))
|
||||
clock.t_ns = int(1e9)
|
||||
sm.notify_satellite_anchor(now_ns=clock.t_ns, gps_consistency_delta_m=None)
|
||||
|
||||
fdr.enqueue.assert_called_once()
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-5: recovery requires BOTH ≥10s stable + visual consistency
|
||||
|
||||
|
||||
def test_ac5_only_stable_dwell_does_not_lift_block() -> None:
|
||||
sm, clock, _fdr = _make_sm(min_stable_s=10.0, tol_m=30.0)
|
||||
clock.t_ns = int(0.5 * 1e9)
|
||||
sm.notify_gps_health(_gps(GpsStatus.SPOOFED))
|
||||
|
||||
clock.t_ns = int(1.0 * 1e9)
|
||||
sm.notify_gps_health(_gps(GpsStatus.STABLE_NON_SPOOFED))
|
||||
# 11 s after stable started, but consistency_delta = None.
|
||||
clock.t_ns = int(12.0 * 1e9)
|
||||
sm.notify_satellite_anchor(now_ns=clock.t_ns, gps_consistency_delta_m=None)
|
||||
|
||||
assert sm.is_spoof_promotion_blocked() is True
|
||||
|
||||
|
||||
def test_ac5_only_consistency_does_not_lift_block() -> None:
|
||||
sm, clock, _fdr = _make_sm(min_stable_s=10.0, tol_m=30.0)
|
||||
clock.t_ns = int(0.5 * 1e9)
|
||||
sm.notify_gps_health(_gps(GpsStatus.SPOOFED))
|
||||
|
||||
clock.t_ns = int(1.0 * 1e9)
|
||||
sm.notify_gps_health(_gps(GpsStatus.STABLE_NON_SPOOFED))
|
||||
# Only 3 s of stable — dwell insufficient — even though
|
||||
# consistency_delta = 5 m (well within tol).
|
||||
clock.t_ns = int(4.0 * 1e9)
|
||||
sm.notify_satellite_anchor(now_ns=clock.t_ns, gps_consistency_delta_m=5.0)
|
||||
|
||||
assert sm.is_spoof_promotion_blocked() is True
|
||||
|
||||
|
||||
def test_ac5_both_conditions_lift_block() -> None:
|
||||
sm, clock, _fdr = _make_sm(min_stable_s=10.0, tol_m=30.0)
|
||||
clock.t_ns = int(0.5 * 1e9)
|
||||
sm.notify_gps_health(_gps(GpsStatus.SPOOFED))
|
||||
|
||||
clock.t_ns = int(1.0 * 1e9)
|
||||
sm.notify_gps_health(_gps(GpsStatus.STABLE_NON_SPOOFED))
|
||||
clock.t_ns = int(12.0 * 1e9)
|
||||
sm.notify_satellite_anchor(now_ns=clock.t_ns, gps_consistency_delta_m=5.0)
|
||||
|
||||
assert sm.is_spoof_promotion_blocked() is False
|
||||
assert sm.current_label() == PoseSourceLabel.SATELLITE_ANCHORED
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-6: logging cannot be silenced
|
||||
|
||||
|
||||
def test_ac6_silenced_logger_does_not_suppress_fdr_or_subscriber() -> None:
|
||||
sm, clock, fdr = _make_sm()
|
||||
seen: list[str] = []
|
||||
sm.subscribe_rejection(lambda reason, _sev, _text: seen.append(reason))
|
||||
|
||||
# Mock the SM's logger so any call to it is a no-op — does NOT
|
||||
# affect the FDR enqueue or the subscriber dispatch path.
|
||||
with mock.patch.object(sm, "_log", mock.MagicMock()):
|
||||
clock.t_ns = int(0.5 * 1e9)
|
||||
sm.notify_gps_health(_gps(GpsStatus.SPOOFED))
|
||||
clock.t_ns = int(1e9)
|
||||
sm.notify_satellite_anchor(now_ns=clock.t_ns, gps_consistency_delta_m=None)
|
||||
|
||||
fdr.enqueue.assert_called_once()
|
||||
assert seen == ["gps_spoofed"]
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-7: is_spoof_promotion_blocked()
|
||||
|
||||
|
||||
def test_ac7_block_query_reflects_latch() -> None:
|
||||
sm, clock, _fdr = _make_sm()
|
||||
assert sm.is_spoof_promotion_blocked() is False
|
||||
|
||||
clock.t_ns = int(1e9)
|
||||
sm.notify_gps_health(_gps(GpsStatus.SPOOFED))
|
||||
assert sm.is_spoof_promotion_blocked() is True
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-8: health_snapshot.spoof_promotion_blocked wired through
|
||||
|
||||
|
||||
def _build_estimator() -> GtsamIsam2StateEstimator:
|
||||
block = C5StateConfig(strategy="gtsam_isam2", keyframe_window_size=15)
|
||||
cfg = mock.MagicMock()
|
||||
cfg.components = {"c5_state": block}
|
||||
estimator, _ = create(
|
||||
config=cfg,
|
||||
imu_preintegrator=mock.MagicMock(),
|
||||
se3_utils=mock.MagicMock(),
|
||||
wgs_converter=mock.MagicMock(),
|
||||
fdr_client=mock.MagicMock(),
|
||||
)
|
||||
return estimator
|
||||
|
||||
|
||||
def test_ac8_health_snapshot_reflects_spoof_block() -> None:
|
||||
estimator = _build_estimator()
|
||||
health_before = estimator.health_snapshot()
|
||||
assert health_before.spoof_promotion_blocked is False
|
||||
|
||||
estimator.notify_gps_health(_gps(GpsStatus.SPOOFED))
|
||||
health_after = estimator.health_snapshot()
|
||||
assert health_after.spoof_promotion_blocked is True
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-9: configurable thresholds
|
||||
|
||||
|
||||
def test_ac9_min_stable_30s_shifts_dwell_window() -> None:
|
||||
sm, clock, _fdr = _make_sm(min_stable_s=30.0, tol_m=30.0)
|
||||
clock.t_ns = int(0.5 * 1e9)
|
||||
sm.notify_gps_health(_gps(GpsStatus.SPOOFED))
|
||||
clock.t_ns = int(1.0 * 1e9)
|
||||
sm.notify_gps_health(_gps(GpsStatus.STABLE_NON_SPOOFED))
|
||||
|
||||
# 15 s — would unblock at default 10 s, but min_stable_s = 30.
|
||||
clock.t_ns = int(16.0 * 1e9)
|
||||
sm.notify_satellite_anchor(now_ns=clock.t_ns, gps_consistency_delta_m=5.0)
|
||||
assert sm.is_spoof_promotion_blocked() is True
|
||||
|
||||
# 32 s — now dwell satisfied.
|
||||
clock.t_ns = int(33.0 * 1e9)
|
||||
sm.notify_satellite_anchor(now_ns=clock.t_ns, gps_consistency_delta_m=5.0)
|
||||
assert sm.is_spoof_promotion_blocked() is False
|
||||
|
||||
|
||||
def test_ac9_consistency_tol_5m_rejects_15m_delta() -> None:
|
||||
sm, clock, _fdr = _make_sm(min_stable_s=10.0, tol_m=5.0)
|
||||
clock.t_ns = int(0.5 * 1e9)
|
||||
sm.notify_gps_health(_gps(GpsStatus.SPOOFED))
|
||||
clock.t_ns = int(1.0 * 1e9)
|
||||
sm.notify_gps_health(_gps(GpsStatus.STABLE_NON_SPOOFED))
|
||||
|
||||
# 12 s stable + 15 m delta — dwell OK, consistency NOT OK.
|
||||
clock.t_ns = int(13.0 * 1e9)
|
||||
sm.notify_satellite_anchor(now_ns=clock.t_ns, gps_consistency_delta_m=15.0)
|
||||
assert sm.is_spoof_promotion_blocked() is True
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-10: every label change emits ONE state-transition INFO log
|
||||
|
||||
|
||||
def test_ac10_label_change_emits_info_log() -> None:
|
||||
sm, clock, _fdr = _make_sm()
|
||||
with mock.patch.object(sm, "_log") as log:
|
||||
clock.t_ns = int(1e9)
|
||||
sm.notify_satellite_anchor(now_ns=clock.t_ns, gps_consistency_delta_m=None)
|
||||
|
||||
log.info.assert_called()
|
||||
kinds = {call.args[0] for call in log.info.call_args_list}
|
||||
assert "c5.state.source_label_changed" in kinds
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-11: reject FDR record shape
|
||||
|
||||
|
||||
def test_ac11_reject_fdr_record_shape() -> None:
|
||||
sm, clock, fdr = _make_sm()
|
||||
clock.t_ns = int(0.5 * 1e9)
|
||||
sm.notify_gps_health(_gps(GpsStatus.SPOOFED))
|
||||
clock.t_ns = int(1e9)
|
||||
sm.notify_satellite_anchor(now_ns=clock.t_ns, gps_consistency_delta_m=None)
|
||||
|
||||
fdr.enqueue.assert_called_once()
|
||||
record = fdr.enqueue.call_args.args[0]
|
||||
assert record.kind == "c5.state.spoof_rejected"
|
||||
assert record.producer_id == "c5_state"
|
||||
assert set(record.payload.keys()) == {
|
||||
"reason",
|
||||
"gps_health",
|
||||
"time_since_stable_s",
|
||||
"visual_consistency_delta_m",
|
||||
}
|
||||
assert record.payload["gps_health"] == "spoofed"
|
||||
assert record.payload["time_since_stable_s"] == pytest.approx(0.0)
|
||||
# The ts field is a non-empty ISO-8601 string at the decode boundary.
|
||||
datetime.fromisoformat(record.ts.replace("Z", "+00:00")) if record.ts.endswith(
|
||||
"Z"
|
||||
) else datetime.fromisoformat(record.ts).astimezone(timezone.utc)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-12: STATUSTEXT severity + 50-char cap
|
||||
|
||||
|
||||
def test_ac12_statustext_severity_is_warning() -> None:
|
||||
sm, clock, _fdr = _make_sm()
|
||||
seen: list[Severity] = []
|
||||
sm.subscribe_rejection(lambda _reason, sev, _text: seen.append(sev))
|
||||
|
||||
clock.t_ns = int(0.5 * 1e9)
|
||||
sm.notify_gps_health(_gps(GpsStatus.SPOOFED))
|
||||
clock.t_ns = int(1e9)
|
||||
sm.notify_satellite_anchor(now_ns=clock.t_ns, gps_consistency_delta_m=None)
|
||||
|
||||
assert seen == [Severity.WARNING]
|
||||
|
||||
|
||||
def test_ac12_statustext_max_50_chars() -> None:
|
||||
sm, clock, _fdr = _make_sm()
|
||||
seen: list[str] = []
|
||||
sm.subscribe_rejection(lambda _reason, _sev, text: seen.append(text))
|
||||
|
||||
clock.t_ns = int(0.5 * 1e9)
|
||||
sm.notify_gps_health(_gps(GpsStatus.SPOOFED))
|
||||
clock.t_ns = int(1e9)
|
||||
sm.notify_satellite_anchor(now_ns=clock.t_ns, gps_consistency_delta_m=None)
|
||||
|
||||
assert len(seen) == 1
|
||||
assert len(seen[0]) <= 50
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# Subscription cancellation
|
||||
|
||||
|
||||
def test_subscription_cancel_stops_callbacks() -> None:
|
||||
sm, clock, _fdr = _make_sm()
|
||||
seen: list[str] = []
|
||||
sub = sm.subscribe_rejection(lambda reason, _sev, _text: seen.append(reason))
|
||||
|
||||
clock.t_ns = int(0.5 * 1e9)
|
||||
sm.notify_gps_health(_gps(GpsStatus.SPOOFED))
|
||||
clock.t_ns = int(1e9)
|
||||
sm.notify_satellite_anchor(now_ns=clock.t_ns, gps_consistency_delta_m=None)
|
||||
assert seen == ["gps_spoofed"]
|
||||
|
||||
sub.cancel()
|
||||
clock.t_ns = int(2e9)
|
||||
sm.notify_satellite_anchor(now_ns=clock.t_ns, gps_consistency_delta_m=None)
|
||||
assert seen == ["gps_spoofed"] # unchanged
|
||||
|
||||
|
||||
def test_subscriber_exception_does_not_break_state_machine() -> None:
|
||||
sm, clock, _fdr = _make_sm()
|
||||
|
||||
def _broken(_reason: str, _sev: Severity, _text: str) -> None:
|
||||
raise RuntimeError("subscriber crash")
|
||||
|
||||
sm.subscribe_rejection(_broken)
|
||||
clock.t_ns = int(0.5 * 1e9)
|
||||
sm.notify_gps_health(_gps(GpsStatus.SPOOFED))
|
||||
clock.t_ns = int(1e9)
|
||||
sm.notify_satellite_anchor(now_ns=clock.t_ns, gps_consistency_delta_m=None)
|
||||
|
||||
# A subsequent attempt — should still emit (state machine intact).
|
||||
seen: list[str] = []
|
||||
sm.subscribe_rejection(lambda reason, _sev, _text: seen.append(reason))
|
||||
clock.t_ns = int(2e9)
|
||||
sm.notify_satellite_anchor(now_ns=clock.t_ns, gps_consistency_delta_m=None)
|
||||
assert seen == ["gps_spoofed"]
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# Estimator wire-up — current_estimate carries the SM's label
|
||||
|
||||
|
||||
def _seed_prior(estimator: GtsamIsam2StateEstimator) -> int:
|
||||
import gtsam_unstable
|
||||
|
||||
pose = gtsam.Pose3()
|
||||
key = gtsam.symbol("x", estimator._next_key_counter)
|
||||
estimator._next_key_counter += 1
|
||||
noise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
graph.add(gtsam.PriorFactorPose3(key, pose, noise))
|
||||
values = gtsam.Values()
|
||||
values.insert(key, pose)
|
||||
ts_map = gtsam_unstable.FixedLagSmootherKeyTimestampMap()
|
||||
ts_map.insert((key, 0.0))
|
||||
estimator._isam2_handle.update(graph, values, timestamps=ts_map)
|
||||
estimator._record_committed_pose_key(key)
|
||||
return key
|
||||
|
||||
|
||||
def test_estimator_current_estimate_label_reflects_sm() -> None:
|
||||
estimator = _build_estimator()
|
||||
_seed_prior(estimator)
|
||||
|
||||
out_before = estimator.current_estimate()
|
||||
assert out_before.source_label == PoseSourceLabel.DEAD_RECKONED
|
||||
|
||||
# Drive a satellite anchor — the SM should transition.
|
||||
estimator._source_label_machine.notify_satellite_anchor(
|
||||
now_ns=estimator._source_label_machine._clock_ns(),
|
||||
gps_consistency_delta_m=None,
|
||||
)
|
||||
|
||||
out_after = estimator.current_estimate()
|
||||
assert out_after.source_label == PoseSourceLabel.SATELLITE_ANCHORED
|
||||
|
||||
|
||||
def test_estimator_subscribe_spoof_rejection_returns_handle() -> None:
|
||||
estimator = _build_estimator()
|
||||
seen: list[str] = []
|
||||
sub = estimator.subscribe_spoof_rejection(lambda reason, _sev, _text: seen.append(reason))
|
||||
|
||||
estimator.notify_gps_health(_gps(GpsStatus.SPOOFED))
|
||||
estimator._source_label_machine.notify_satellite_anchor(
|
||||
now_ns=estimator._source_label_machine._clock_ns(),
|
||||
gps_consistency_delta_m=None,
|
||||
)
|
||||
|
||||
assert seen == ["gps_spoofed"]
|
||||
sub.cancel()
|
||||
|
||||
|
||||
def test_estimator_subscribe_after_attach_with_stub_raises() -> None:
|
||||
estimator = _build_estimator()
|
||||
stub = mock.MagicMock()
|
||||
stub.current_label = mock.MagicMock(return_value=PoseSourceLabel.SATELLITE_ANCHORED)
|
||||
stub.is_spoof_promotion_blocked = mock.MagicMock(return_value=False)
|
||||
|
||||
estimator.attach_source_label_state_machine(stub)
|
||||
|
||||
with pytest.raises(StateEstimatorConfigError):
|
||||
estimator.subscribe_spoof_rejection(lambda *_args: None)
|
||||
Reference in New Issue
Block a user