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

[AZ-381] C5 StateEstimator protocol + factory + C8 DTO reshape

Browse Source 
- Add StateEstimator Protocol (6 methods, @runtime_checkable) + DTOs
  (EstimatorOutput, EstimatorHealth, IsamState, PoseSourceLabel, Quat)
  in _types/state.py per state_estimator_protocol.md v1.0.0.
- Add C5 error hierarchy (StateEstimatorError + 3 subclasses) and
  C5StateConfig (strategy, keyframe_window, spoof gates,
  no_estimate_fallback_s) with __post_init__ validation.
- Add ISam2GraphHandle Protocol + ISam2GraphHandleImpl skeleton (all
  4 methods raise NotImplementedError naming AZ-382 as owner).
- Add build_state_estimator factory + bind_state_ingest_thread for
  single-writer enforcement; ADR-002 build-flag gating
  (BUILD_STATE_<variant>); INFO log on success.
- Strict reshape of legacy EstimatorOutput / EstimatorHealth across
  all 6 C8 production files (_outbound_provenance,
  _covariance_projector, pymavlink_ardupilot_adapter,
  msp2_inav_adapter, mavlink_gcs_adapter, interface) + 6 C8 test
  files (UUID frame_id, LatLonAlt position_wgs84, Quat orientation,
  PoseSourceLabel enum source_label). Remove ad-hoc DTOs from
  _types/pose.py and from C4's public __init__ (EstimatorOutput is a
  C5 concept, not a C4 one).
- 20 AZ-381 AC tests (10 ACs + 4 config range + NFR + conformance).
- Full suite: 521 passed, 2 skipped (+20 vs Batch 11).
- Contracts: state_estimator_protocol.md v1.0.0 -> active;
  composition_root_protocol.md v1.2.0 -> v1.3.0 (additive state
  block + factory + ingest-thread binding).
- Impl report: _docs/03_implementation/batch_12_cycle1_report.md.

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-11 05:35:20 +03:00
parent 8a9cf88a46
commit beed43724f
32 changed files with 1394 additions and 157 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/gps_denied_onboard/_types/emitted.py
+5 -2
View File
@@ -15,6 +15,7 @@ from __future__ import annotations
from dataclasses import dataclass
from gps_denied_onboard._types.fc import FcKind
from gps_denied_onboard._types.state import PoseSourceLabel
__all__ = ["EmittedExternalPosition"]
@@ -25,11 +26,13 @@ class EmittedExternalPosition:
Constructed by the AP / iNav outbound bodies (AZ-393 / AZ-394)
immediately after the wire write succeeds; consumed by the
runtime root for FDR logging.
runtime root for FDR logging. ``source_label`` is a
:class:`PoseSourceLabel` enum (AZ-381 reshape); serialise via
``.value`` on the FDR-write side.
"""
fc_kind: FcKind
horiz_accuracy_m: float
source_label: str
source_label: PoseSourceLabel
emitted_at: int
sequence_number: int
src/gps_denied_onboard/_types/pose.py
+11 -33
View File
@@ -1,8 +1,17 @@
"""C4 PoseEstimator + C5 StateEstimator output DTOs."""
"""C4 PoseEstimator output DTOs.
The C5 estimator output + provenance enums live in
:mod:`gps_denied_onboard._types.state`; importing them here used to be
convenient for the C4 module's public re-exports but the two DTOs
diverged (C5 carries a UUID frame_id + WGS84 position + Quat
orientation directly; C4 still passes a raw 4x4 ``pose_se3`` to keep
the OpenCV ↔ GTSAM seam thin). Components that need the C5 surface
import from ``_types.state`` directly.
"""
from __future__ import annotations
from dataclasses import dataclass, field
from dataclasses import dataclass
from datetime import datetime
from typing import Any
@@ -17,34 +26,3 @@ class PoseEstimate:
covariance_6x6: Any | None = None
covariance_mode: str = "marginals"
mre_px: float | None = None
@dataclass(frozen=True)
class EstimatorOutput:
"""C5 state-estimator output (smoothed pose + uncertainty + source label + health).
``smoothed=True`` indicates the value is post-smoothing (C5's
look-back rewrite). Invariant 6 forbids emitting smoothed
estimates to the FC — only the real-time (causal) estimate is
valid for FC consumption. C8 outbound adapters MUST raise
:class:`FcEmitError` on ``smoothed=True``.
"""
frame_id: int
timestamp: datetime
pose_se3: Any
covariance_6x6: Any | None = None
source_label: str = "visual_propagated"
health: EstimatorHealth | None = None
smoothed: bool = False
extras: dict[str, Any] = field(default_factory=dict)
@dataclass(frozen=True)
class EstimatorHealth:
"""C5 estimator health flags."""
last_anchor_age_ms: int = 0
imu_bias_norm: float = 0.0
vio_drift_proxy: float = 0.0
is_spoof_promoted: bool = False
src/gps_denied_onboard/_types/state.py
+107
View File
@@ -0,0 +1,107 @@
"""C5 ``StateEstimator`` output DTOs + canonical pose-provenance enum.
AZ-381 owns this module per the C5 contract
(``_docs/02_document/contracts/c5_state/state_estimator_protocol.md`` v1.0.0).
C4 reuses the same ``Quat`` + ``PoseSourceLabel`` enums so the C4/C5
boundary stays single-typed (see the C4 contract
``_docs/02_document/contracts/c4_pose/pose_estimator_protocol.md`` §4 DTOs).
The dataclasses are ``frozen=True, slots=True`` per AC-2 — DTOs cross
component boundaries and mutation-through-aliasing has bitten this
codebase before (R05 in the C5 risk register).
"""
from __future__ import annotations
from dataclasses import dataclass
from enum import Enum
from typing import TYPE_CHECKING, Any
from uuid import UUID
if TYPE_CHECKING:
import numpy.typing as npt
from gps_denied_onboard._types.geo import LatLonAlt
__all__ = [
"EstimatorHealth",
"EstimatorOutput",
"IsamState",
"PoseSourceLabel",
"Quat",
]
@dataclass(frozen=True, slots=True)
class Quat:
"""Unit quaternion ``(w, x, y, z)``; scalar-first.
Used by C4/C5 to carry body→world orientation without committing
to a heavyweight SE(3) library at the DTO boundary.
"""
w: float
x: float
y: float
z: float
class PoseSourceLabel(Enum):
"""Canonical C5-emitted pose provenance label.
The three values map 1:1 to the C5 source-label state machine
(AZ-385): ``SATELLITE_ANCHORED`` requires the spoof-promotion
gate to be open; ``VISUAL_PROPAGATED`` is the steady-state
no-anchor path; ``DEAD_RECKONED`` is the IMU-only fallback.
"""
SATELLITE_ANCHORED = "satellite_anchored"
VISUAL_PROPAGATED = "visual_propagated"
DEAD_RECKONED = "dead_reckoned"
class IsamState(Enum):
"""C5 iSAM2 lifecycle state surfaced via :class:`EstimatorHealth`."""
INIT = "init"
TRACKING = "tracking"
DEGRADED = "degraded"
LOST = "lost"
@dataclass(frozen=True, slots=True)
class EstimatorOutput:
"""C5 state-estimator output.
Invariant 6 (the C5 contract) forbids emitting ``smoothed=True``
values to the FC; C8 outbound adapters MUST raise on it. The
composition root distinguishes the two paths by setting
``smoothed=True`` only on entries returned from
``smoothed_history(...)`` (history is for C13 / GCS observability,
not flight control).
"""
frame_id: UUID
position_wgs84: LatLonAlt
orientation_world_T_body: Quat
velocity_world_mps: tuple[float, float, float]
covariance_6x6: npt.NDArray[Any]
source_label: PoseSourceLabel
last_satellite_anchor_age_ms: int
smoothed: bool
emitted_at: int
@dataclass(frozen=True, slots=True)
class EstimatorHealth:
"""C5 iSAM2 health snapshot returned by ``health_snapshot()``.
Consumed by C8 telemetry, C13 FDR records, and the spoof-promotion
gate (AZ-385) for the ``cov_norm_growing_for_s`` / ``spoof_promotion_blocked``
decisions.
"""
isam2_state: IsamState
keyframe_count: int
cov_norm_growing_for_s: float
spoof_promotion_blocked: bool
src/gps_denied_onboard/components/c4_pose/__init__.py
+2 -2
View File
@@ -1,6 +1,6 @@
"""C4 Pose Estimator component — Public API."""
from gps_denied_onboard._types.pose import EstimatorOutput, PoseEstimate
from gps_denied_onboard._types.pose import PoseEstimate
from gps_denied_onboard.components.c4_pose.interface import PoseEstimator
__all__ = ["EstimatorOutput", "PoseEstimate", "PoseEstimator"]
__all__ = ["PoseEstimate", "PoseEstimator"]
src/gps_denied_onboard/components/c4_pose/interface.py
+2
View File
@@ -11,6 +11,8 @@ from typing import Protocol
from gps_denied_onboard._types.matching import MatchResult
from gps_denied_onboard._types.pose import PoseEstimate
__all__ = ["PoseEstimator"]
class PoseEstimator(Protocol):
"""Estimate a 6-DoF pose from a verified cross-domain match."""
src/gps_denied_onboard/components/c5_state/__init__.py
+50 -3
View File
@@ -1,6 +1,53 @@
"""C5 State Estimator component — Public API."""
"""C5 State Estimator component — public API.
from gps_denied_onboard._types.pose import EstimatorHealth, EstimatorOutput
Per the C5 contract (``state_estimator_protocol.md`` v1.0.0), the
public surface consists of:
- :class:`StateEstimator` Protocol
- :class:`EstimatorOutput`, :class:`EstimatorHealth`, :class:`IsamState`
DTOs (in ``_types/state.py``)
- :class:`PoseSourceLabel` enum (in ``_types/state.py``; shared with C4)
- :class:`C5StateConfig` config block (registered on import)
- Error hierarchy: :class:`StateEstimatorError` and three subclasses
The ``ISam2GraphHandle`` Protocol + ``ISam2GraphHandleImpl`` skeleton
live in the private ``_isam2_handle`` module — consumers import them
from the composition root, not from here.
"""
from gps_denied_onboard._types.state import (
EstimatorHealth,
EstimatorOutput,
IsamState,
PoseSourceLabel,
Quat,
)
from gps_denied_onboard.components.c5_state.config import C5StateConfig
from gps_denied_onboard.components.c5_state.errors import (
EstimatorDegradedError,
EstimatorFatalError,
StateEstimatorConfigError,
StateEstimatorError,
)
from gps_denied_onboard.components.c5_state.interface import StateEstimator
from gps_denied_onboard.config.schema import register_component_block
__all__ = ["EstimatorHealth", "EstimatorOutput", "StateEstimator"]
__all__ = [
"C5StateConfig",
"EstimatorDegradedError",
"EstimatorFatalError",
"EstimatorHealth",
"EstimatorOutput",
"IsamState",
"PoseSourceLabel",
"Quat",
"StateEstimator",
"StateEstimatorConfigError",
"StateEstimatorError",
]
# Register the c5_state config block on import. The composition root
# loads this module before `load_config(...)` so the block is in the
# registry by the time YAML/env overrides resolve.
register_component_block("c5_state", C5StateConfig)
src/gps_denied_onboard/components/c5_state/_isam2_handle.py
+84
View File
@@ -0,0 +1,84 @@
"""Concrete ``ISam2GraphHandle`` skeleton — AZ-381.
C4 (``OpenCVGtsamPoseEstimator``) calls ``add_factor`` / ``update`` /
``compute_marginals`` against this handle, NOT against C5 directly —
ADR-003 says C5 owns the graph; this handle is the typed seam C4 uses
to drive it without importing C5 internals.
AZ-381 ships the skeleton: every method raises
``NotImplementedError("Body owned by AZ-382 iSAM2 wiring task")``. The
``NotImplementedError`` messages name AZ-382 so the next task's
implementer can grep for them.
AZ-382 replaces the four method bodies with the real GTSAM calls
against the C5 estimator's ``_isam2`` + ``_smoother`` instances. The
Protocol surface is stable from AZ-381 onward.
"""
from __future__ import annotations
from typing import TYPE_CHECKING, Any, Protocol, runtime_checkable
if TYPE_CHECKING:
from gps_denied_onboard.components.c5_state.gtsam_isam2_estimator import (
GtsamIsam2StateEstimator,
)
__all__ = ["ISam2GraphHandle", "ISam2GraphHandleImpl"]
@runtime_checkable
class ISam2GraphHandle(Protocol):
"""C4 ↔ C5 seam over the shared iSAM2 graph (ADR-003).
Owned by C5; held by reference inside ``OpenCVGtsamPoseEstimator``.
The handle is passed during composition (``state_factory``
returns the tuple; ``pose_factory`` accepts it as a positional
argument) and never crosses thread boundaries — see Invariant 1
of both the C4 and C5 contracts.
"""
def add_factor(self, factor: Any) -> None: ...
def update(self, graph: Any, values: Any, timestamps: Any | None = None) -> None: ...
def compute_marginals(self) -> Any: ...
def last_anchor_age_ms(self) -> int: ...
class ISam2GraphHandleImpl(ISam2GraphHandle):
"""Skeleton — every method delegates to AZ-382 once that task lands.
The skeleton exists so AZ-381 can ship a runnable composition
root that produces a concrete handle reference for C4 to inject
against (per ADR-009). AZ-382 replaces every body with the real
GTSAM calls; the Protocol surface does not change.
"""
def __init__(self, estimator: GtsamIsam2StateEstimator) -> None:
self._estimator = estimator
def add_factor(self, factor: Any) -> None:
raise NotImplementedError(
"Body owned by AZ-382 iSAM2 wiring task — "
"this skeleton is intentionally inert until iSAM2 wiring lands."
)
def update(self, graph: Any, values: Any, timestamps: Any | None = None) -> None:
raise NotImplementedError(
"Body owned by AZ-382 iSAM2 wiring task — "
"this skeleton is intentionally inert until iSAM2 wiring lands."
)
def compute_marginals(self) -> Any:
raise NotImplementedError(
"Body owned by AZ-382 iSAM2 wiring task — "
"this skeleton is intentionally inert until iSAM2 wiring lands."
)
def last_anchor_age_ms(self) -> int:
raise NotImplementedError(
"Body owned by AZ-382 iSAM2 wiring task — "
"this skeleton is intentionally inert until iSAM2 wiring lands."
)
src/gps_denied_onboard/components/c5_state/config.py
+75
View File
@@ -0,0 +1,75 @@
"""C5 state estimator config block — AZ-381.
The block is registered into the global config registry via
``register_component_block("c5_state", C5StateConfig)``; the runtime
root reads ``config.components["c5_state"]`` and dispatches by
``strategy``. ADR-002 build-time-exclusion gating happens in
:mod:`gps_denied_onboard.runtime_root.state_factory`, not here.
"""
from __future__ import annotations
from dataclasses import dataclass
from typing import Final
from gps_denied_onboard.config.schema import ConfigError
__all__ = ["KNOWN_STATE_STRATEGIES", "C5StateConfig"]
KNOWN_STATE_STRATEGIES: Final[frozenset[str]] = frozenset({"gtsam_isam2", "eskf"})
_KEYFRAME_WINDOW_MIN: Final[int] = 10
_KEYFRAME_WINDOW_MAX: Final[int] = 20
@dataclass(frozen=True)
class C5StateConfig:
"""C5 state-estimator config block.
Fields per the C5 contract §"Config schema additions":
- ``strategy`` — selects between ``"gtsam_isam2"`` (production
default) and ``"eskf"`` (mandatory simple baseline per IT-12).
- ``keyframe_window_size`` — D-C5-3 K∈[10,20] for the
``IncrementalFixedLagSmoother`` window.
- ``spoof_promotion_min_stable_s`` — AC-NEW-2 minimum dwell time
in ``STABLE_NON_SPOOFED`` before the spoof-promotion gate opens.
- ``spoof_promotion_visual_consistency_tol_m`` — AC-NEW-8 visual
consistency tolerance on the next anchor.
- ``no_estimate_fallback_s`` — AC-5.2 timeout before the
runtime root drops to FC-IMU-only mode.
"""
strategy: str = "gtsam_isam2"
keyframe_window_size: int = 15
spoof_promotion_min_stable_s: float = 10.0
spoof_promotion_visual_consistency_tol_m: float = 30.0
no_estimate_fallback_s: float = 3.0
def __post_init__(self) -> None:
if self.strategy not in KNOWN_STATE_STRATEGIES:
raise ConfigError(
f"C5StateConfig.strategy={self.strategy!r} not in {sorted(KNOWN_STATE_STRATEGIES)}"
)
if not (_KEYFRAME_WINDOW_MIN <= self.keyframe_window_size <= _KEYFRAME_WINDOW_MAX):
raise ConfigError(
"C5StateConfig.keyframe_window_size must be in "
f"[{_KEYFRAME_WINDOW_MIN}, {_KEYFRAME_WINDOW_MAX}] (D-C5-3); "
f"got {self.keyframe_window_size}"
)
if self.spoof_promotion_min_stable_s <= 0.0:
raise ConfigError(
"C5StateConfig.spoof_promotion_min_stable_s must be > 0; "
f"got {self.spoof_promotion_min_stable_s}"
)
if self.spoof_promotion_visual_consistency_tol_m <= 0.0:
raise ConfigError(
"C5StateConfig.spoof_promotion_visual_consistency_tol_m must be > 0; "
f"got {self.spoof_promotion_visual_consistency_tol_m}"
)
if self.no_estimate_fallback_s <= 0.0:
raise ConfigError(
"C5StateConfig.no_estimate_fallback_s must be > 0; "
f"got {self.no_estimate_fallback_s}"
)
src/gps_denied_onboard/components/c5_state/errors.py
+55
View File
@@ -0,0 +1,55 @@
"""C5 ``StateEstimator`` error hierarchy — AZ-381.
Every C5-emitted exception inherits :class:`StateEstimatorError`
(AC-10) so callers can write a single ``except`` against the whole
component surface. Composition-root failures use
:class:`StateEstimatorConfigError`; runtime failures split into
``Degraded`` (recoverable; emit a degraded estimate + log) vs
``Fatal`` (unrecoverable; trigger the AC-5.2 IMU-only fallback path
in C8).
"""
from __future__ import annotations
__all__ = [
"EstimatorDegradedError",
"EstimatorFatalError",
"StateEstimatorConfigError",
"StateEstimatorError",
]
class StateEstimatorError(Exception):
"""Base class for every C5-emitted exception (AC-10)."""
class EstimatorDegradedError(StateEstimatorError):
"""Recoverable runtime degradation.
Examples: out-of-order ``add_*`` call (Invariant 2), failed factor
add against the graph (R05 mitigation surfaces via this), poor
convergence detected post-update. The estimator continues to
produce outputs but the next ``current_estimate()`` may carry a
degraded ``EstimatorHealth.isam2_state``.
"""
class EstimatorFatalError(StateEstimatorError):
"""Unrecoverable numerical failure.
Raised when iSAM2 / Marginals / the smoother enter a state from
which the run cannot continue: non-SPD posterior covariance after
update, NaN propagation, GTSAM exception bubbling. Triggers the
AC-5.2 path in C8 (IMU-only fallback) and the source-label state
machine transitions to ``DEAD_RECKONED``.
"""
class StateEstimatorConfigError(StateEstimatorError):
"""Composition-time configuration error.
Raised by :func:`build_state_estimator` when the requested
strategy is not registered (per ADR-002 build flag gating), when
the config schema fails validation, or when the runtime root
cannot wire the iSAM2 graph handle into C4.
"""
src/gps_denied_onboard/components/c5_state/interface.py
+58 -11
View File
@@ -1,23 +1,70 @@
"""C5 `StateEstimator` Protocol.
"""C5 ``StateEstimator`` Protocol — AZ-381.
Concrete impls: `GtsamIsam2StateEstimator` (production-default; iSAM2 +
IncrementalFixedLagSmoother), `EskfStateEstimator` (mandatory simple baseline).
See `_docs/02_document/components/07_c5_state/`.
The single typed handle C8 / C4 / runtime root hold for the state
estimator. Concrete implementations live in
``gps_denied_onboard.components.c5_state.gtsam_isam2_estimator`` (AZ-382
onward) and ``...eskf_baseline`` (AZ-386). Both are link-time exclusive
via the ``BUILD_STATE_<variant>`` flags per ADR-002.
The Protocol is ``runtime_checkable`` so test fakes pass
``isinstance(fake, StateEstimator)`` without depending on a concrete
parent class (ADR-009 — interface-first DI).
See the contract at
``_docs/02_document/contracts/c5_state/state_estimator_protocol.md``
for the complete invariant list (10 invariants total).
"""
from __future__ import annotations
from typing import Protocol
from typing import TYPE_CHECKING, Protocol, runtime_checkable
from gps_denied_onboard._types.pose import EstimatorOutput, PoseEstimate
from gps_denied_onboard._types.vio import VioOutput
if TYPE_CHECKING:
from gps_denied_onboard._types.fc import ImuTelemetrySample
from gps_denied_onboard._types.pose import PoseEstimate
from gps_denied_onboard._types.state import (
EstimatorHealth,
EstimatorOutput,
)
from gps_denied_onboard._types.vio import VioOutput
__all__ = ["StateEstimator"]
@runtime_checkable
class StateEstimator(Protocol):
"""Smoothed state estimator (fuses VIO + satellite anchors + IMU)."""
"""Smoothed state estimator (fuses VIO + satellite anchors + IMU).
def add_vio(self, vio: VioOutput) -> None: ...
All six methods run on the same ingest thread (Invariant 1 —
GTSAM iSAM2 is not thread-safe; composition root enforces). The
six methods correspond 1:1 to the contract surface; concrete
impls must implement every method.
"""
def add_pose_anchor(self, anchor: PoseEstimate) -> None: ...
def add_vio(self, vio: VioOutput) -> None:
"""Add a VIO output as a relative-pose factor to the iSAM2 graph."""
def latest_output(self) -> EstimatorOutput | None: ...
def add_pose_anchor(self, pose: PoseEstimate) -> None:
"""Add a C4 pose anchor.
Invariant 3 — ``pose.covariance_mode == "jacobian"`` MUST NOT
produce an iSAM2 factor; only the marginals path triggers the
factor + update cycle.
"""
def add_fc_imu(self, imu_window: ImuTelemetrySample) -> None:
"""Add an FC IMU sample / window to the iSAM2 preintegrator."""
def current_estimate(self) -> EstimatorOutput:
"""Return the latest (non-smoothed) estimate. Never returns ``None``."""
def smoothed_history(self, n_keyframes: int) -> list[EstimatorOutput]:
"""Return up to ``n_keyframes`` recent smoothed estimates.
Every entry has ``smoothed=True`` (Invariant 7); never emitted
to the FC (Invariant 6); bounded by the keyframe window K
(Invariant 6 of the contract; D-C5-3 K∈[10,20]).
"""
def health_snapshot(self) -> EstimatorHealth:
"""Return the current iSAM2 health snapshot."""
src/gps_denied_onboard/components/c8_fc_adapter/_covariance_projector.py
+5 -5
View File
@@ -26,11 +26,11 @@ from __future__ import annotations
import math
from datetime import datetime, timezone
from typing import Final
from typing import Any, Final
import numpy as np
from gps_denied_onboard._types.pose import EstimatorOutput
from gps_denied_onboard._types.state import EstimatorOutput
from gps_denied_onboard.components.c8_fc_adapter.errors import FcEmitError
from gps_denied_onboard.fdr_client.client import FdrClient
from gps_denied_onboard.fdr_client.records import FdrRecord
@@ -102,7 +102,7 @@ class CovarianceProjector:
"kv": {
"radius_mm_raw": radius_mm,
"clamped_to": _INAV_HPOS_MAX_MM,
"frame_id": output.frame_id,
"frame_id": str(output.frame_id),
},
},
)
@@ -151,10 +151,10 @@ class CovarianceProjector:
self,
*,
reason: str,
frame_id: int,
frame_id: Any,
extra: dict | None = None,
) -> None:
payload: dict = {"reason": reason, "frame_id": frame_id}
payload: dict = {"reason": reason, "frame_id": str(frame_id)}
if extra:
payload.update(extra)
# The FDR schema closes ``kind`` to the documented set; we
src/gps_denied_onboard/components/c8_fc_adapter/_outbound_provenance.py
+33 -19
View File
@@ -2,10 +2,11 @@
Two pieces shared by AP and iNav outbound paths:
1. :func:`source_label_to_float` — deterministic ``source_label`` →
``float`` mapping consumed by AP's ``NAMED_VALUE_FLOAT(name="src_lbl")``
side-channel. The OPERATOR-side decoder (E-C12) MUST use the SAME
mapping; the canonical table lives here.
1. :func:`source_label_to_float` — deterministic
:class:`PoseSourceLabel` → ``float`` mapping consumed by AP's
``NAMED_VALUE_FLOAT(name="src_lbl")`` side-channel. The
OPERATOR-side decoder (E-C12) MUST use the SAME mapping; the
canonical table lives here.
2. :class:`StatusTextTransitionRateLimiter` — emits ``STATUSTEXT(...)``
exactly once per ``source_label`` transition (AC-4 / AZ-393 AC-3 /
@@ -25,6 +26,7 @@ from collections.abc import Callable
from typing import Final
from gps_denied_onboard._types.fc import Severity
from gps_denied_onboard._types.state import PoseSourceLabel
__all__ = [
"SOURCE_LABEL_TO_FLOAT",
@@ -34,21 +36,27 @@ __all__ = [
# Canonical source-label-to-float mapping (AZ-393 AC-3 / D-C8-7).
# Operator-side decoder in C12 MUST mirror this table.
# Operator-side decoder in C12 MUST mirror this table. Keys are the
# string values of :class:`PoseSourceLabel`; the dict is materialised
# from the enum so the two cannot drift.
SOURCE_LABEL_TO_FLOAT: Final[dict[str, float]] = {
"unknown": 0.0,
"visual_propagated": 1.0,
"sat_anchored": 2.0,
"imu_only": 3.0,
"warm_start": 4.0,
"smoothed": 5.0,
"ac52_fallback": 6.0,
PoseSourceLabel.VISUAL_PROPAGATED.value: 1.0,
PoseSourceLabel.SATELLITE_ANCHORED.value: 2.0,
PoseSourceLabel.DEAD_RECKONED.value: 3.0,
}
_UNKNOWN_LABEL_FLOAT: Final[float] = 0.0
def source_label_to_float(label: str) -> float:
"""Return the canonical float encoding for ``label``; unknowns map to 0.0."""
return SOURCE_LABEL_TO_FLOAT.get(label, SOURCE_LABEL_TO_FLOAT["unknown"])
def source_label_to_float(label: PoseSourceLabel | str) -> float:
"""Return the canonical float encoding for ``label``.
Accepts :class:`PoseSourceLabel` (production path) or a raw
string (legacy / replay decoders). Unknown strings map to
``0.0``; unknown enum members can never happen because every
member is in the table by construction.
"""
key = label.value if isinstance(label, PoseSourceLabel) else label
return SOURCE_LABEL_TO_FLOAT.get(key, _UNKNOWN_LABEL_FLOAT)
class StatusTextTransitionRateLimiter:
@@ -72,12 +80,12 @@ class StatusTextTransitionRateLimiter:
self._min_interval_s = min_interval_s
self._clock = clock
self._lock = threading.Lock()
self._last_label: str | None = None
self._last_label: PoseSourceLabel | str | None = None
self._last_emit_at_by_sev: dict[Severity, float] = {}
def note_label_and_maybe_emit(
self,
new_label: str,
new_label: PoseSourceLabel | str,
*,
severity: Severity = Severity.INFO,
) -> bool:
@@ -96,12 +104,18 @@ class StatusTextTransitionRateLimiter:
if (now - last_emit) < self._min_interval_s:
return False
self._last_emit_at_by_sev[severity] = now
msg = f"src={new_label}" if previous is None else f"src {previous}->{new_label}"
new_text = _label_text(new_label)
prev_text = _label_text(previous) if previous is not None else None
msg = f"src={new_text}" if prev_text is None else f"src {prev_text}->{new_text}"
# Send OUTSIDE the lock — pymavlink statustext_send may block on UART.
self._send(msg, severity)
return True
@property
def last_label(self) -> str | None:
def last_label(self) -> PoseSourceLabel | str | None:
with self._lock:
return self._last_label
def _label_text(label: PoseSourceLabel | str) -> str:
return label.value if isinstance(label, PoseSourceLabel) else label
src/gps_denied_onboard/components/c8_fc_adapter/interface.py
+1 -1
View File
@@ -25,7 +25,7 @@ from gps_denied_onboard._types.fc import (
Subscription,
TelemetryCallback,
)
from gps_denied_onboard._types.pose import EstimatorOutput
from gps_denied_onboard._types.state import EstimatorOutput
__all__ = ["FcAdapter", "GcsAdapter", "ReplaySink"]
src/gps_denied_onboard/components/c8_fc_adapter/mavlink_gcs_adapter.py
+3 -4
View File
@@ -38,7 +38,7 @@ from gps_denied_onboard._types.fc import (
Subscription,
)
from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard._types.pose import EstimatorOutput
from gps_denied_onboard._types.state import EstimatorOutput
from gps_denied_onboard.components.c8_fc_adapter._covariance_projector import (
CovarianceProjector,
)
@@ -366,11 +366,10 @@ class QgcTelemetryAdapter:
)
def _extract_wgs84(self, output: EstimatorOutput) -> LatLonAlt:
wgs = output.extras.get("wgs84") if output.extras else None
wgs = output.position_wgs84
if not isinstance(wgs, LatLonAlt):
raise GcsEmitError(
"EstimatorOutput.extras['wgs84'] missing or not a LatLonAlt; "
"composition root must inject the ENU->WGS84 enricher"
f"EstimatorOutput.position_wgs84 must be a LatLonAlt; got {type(wgs).__name__}"
)
return wgs
src/gps_denied_onboard/components/c8_fc_adapter/msp2_inav_adapter.py
+6 -7
View File
@@ -28,7 +28,7 @@ from gps_denied_onboard._types.fc import (
TelemetryCallback,
)
from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard._types.pose import EstimatorOutput
from gps_denied_onboard._types.state import EstimatorOutput
from gps_denied_onboard.components.c8_fc_adapter._covariance_projector import (
CovarianceProjector,
)
@@ -184,7 +184,7 @@ class Msp2InavAdapter:
"c8.inav.first_emit",
extra={
"kind": "c8.inav.first_emit",
"kv": {"frame_id": output.frame_id, "seq": seq},
"kv": {"frame_id": str(output.frame_id), "seq": seq},
},
)
self._log.debug(
@@ -265,19 +265,18 @@ class Msp2InavAdapter:
)
def _extract_wgs84(self, output: EstimatorOutput) -> LatLonAlt:
wgs = output.extras.get("wgs84") if output.extras else None
wgs = output.position_wgs84
if not isinstance(wgs, LatLonAlt):
raise FcEmitError(
"EstimatorOutput.extras['wgs84'] missing or not a LatLonAlt; "
"composition root must inject the ENU->WGS84 enricher"
f"EstimatorOutput.position_wgs84 must be a LatLonAlt; got {type(wgs).__name__}"
)
return wgs
def _log_emit_failed(self, reason: str, frame_id: int) -> None:
def _log_emit_failed(self, reason: str, frame_id: Any) -> None:
self._log.error(
f"c8.inav.emit_failed: {reason}",
extra={
"kind": "c8.inav.emit_failed",
"kv": {"reason": reason, "frame_id": frame_id},
"kv": {"reason": reason, "frame_id": str(frame_id)},
},
)
src/gps_denied_onboard/components/c8_fc_adapter/pymavlink_ardupilot_adapter.py
+13 -13
View File
@@ -38,7 +38,7 @@ from gps_denied_onboard._types.fc import (
TelemetryCallback,
)
from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard._types.pose import EstimatorOutput
from gps_denied_onboard._types.state import EstimatorOutput
from gps_denied_onboard.components.c8_fc_adapter._covariance_projector import (
CovarianceProjector,
)
@@ -270,7 +270,7 @@ class PymavlinkArdupilotAdapter:
"c8.ap.first_emit",
extra={
"kind": "c8.ap.first_emit",
"kv": {"frame_id": output.frame_id, "seq": seq},
"kv": {"frame_id": str(output.frame_id), "seq": seq},
},
)
self._log.debug(
@@ -527,12 +527,12 @@ class PymavlinkArdupilotAdapter:
},
)
def _log_emit_failed(self, reason: str, frame_id: int) -> None:
def _log_emit_failed(self, reason: str, frame_id: Any) -> None:
self._log.error(
f"c8.ap.emit_failed: {reason}",
extra={
"kind": "c8.ap.emit_failed",
"kv": {"reason": reason, "frame_id": frame_id},
"kv": {"reason": reason, "frame_id": str(frame_id)},
},
)
@@ -588,19 +588,19 @@ class PymavlinkArdupilotAdapter:
pass
def _extract_wgs84(self, output: EstimatorOutput) -> LatLonAlt:
"""Pull the WGS84 fix the composition root pre-attached.
"""Pull the WGS84 fix that C5 produced.
C5 emits its estimate in the local ENU frame; the composition
root injects a WgsConverter-backed enricher that attaches the
WGS84 conversion to ``output.extras["wgs84"]`` BEFORE handing
the output to C8. If the enricher is missing the wgs84 key,
that is a composition bug — fail loudly rather than guess.
Per the C5 contract v1.0.0, the estimator emits
``EstimatorOutput.position_wgs84`` directly (the
composition root no longer injects an enricher; the
conversion happens inside C5's ``current_estimate`` path
using the shared :class:`WgsConverter`). A missing field
is a composition bug — fail loudly rather than guess.
"""
wgs = output.extras.get("wgs84") if output.extras else None
wgs = output.position_wgs84
if not isinstance(wgs, LatLonAlt):
raise FcEmitError(
"EstimatorOutput.extras['wgs84'] missing or not a LatLonAlt; "
"composition root must inject the ENU->WGS84 enricher"
f"EstimatorOutput.position_wgs84 must be a LatLonAlt; got {type(wgs).__name__}"
)
return wgs
src/gps_denied_onboard/runtime_root/__init__.py
+16
View File
@@ -40,6 +40,15 @@ from gps_denied_onboard.runtime_root.spoof_recovery_sink import (
SpoofRecoveryPublisher,
SpoofRecoverySink,
)
from gps_denied_onboard.runtime_root.state_factory import (
StateIngestThreadAlreadyBoundError,
bind_state_ingest_thread,
build_state_estimator,
clear_state_ingest_binding,
clear_state_registry,
list_registered_state_strategies,
register_state_estimator,
)
if TYPE_CHECKING:
from gps_denied_onboard.components.c13_fdr.headers import FlightHeader
@@ -55,13 +64,18 @@ __all__ = [
"RuntimeRoot",
"SpoofRecoveryPublisher",
"SpoofRecoverySink",
"StateIngestThreadAlreadyBoundError",
"StrategyNotLinkedError",
"StrategyTier",
"TakeoffResult",
"bind_outbound_emit_thread",
"bind_state_ingest_thread",
"build_fc_adapter",
"build_gcs_adapter",
"build_state_estimator",
"clear_outbound_thread_binding",
"clear_state_ingest_binding",
"clear_state_registry",
"clear_strategy_registries",
"clear_strategy_registry",
"compose_operator",
@@ -69,10 +83,12 @@ __all__ = [
"compose_root",
"list_registered_fc_strategies",
"list_registered_gcs_strategies",
"list_registered_state_strategies",
"list_registered_strategies",
"main",
"register_fc_adapter",
"register_gcs_adapter",
"register_state_estimator",
"register_strategy",
"take_off",
]
src/gps_denied_onboard/runtime_root/state_factory.py
+214
View File
@@ -0,0 +1,214 @@
"""Composition-root factory for C5 (AZ-381 / E-C5).
Mirrors the C8 factory shape (``runtime_root.fc_factory``): per-binary
bootstrap modules register concrete strategy factories under their
``BUILD_STATE_<variant>`` flag; :func:`build_state_estimator` resolves
the configured strategy, gates by build flag, constructs the
estimator, and returns the tuple ``(StateEstimator,
ISam2GraphHandle)`` so the runtime root can inject the handle into
C4.
Single-writer-thread binding for C5 + C4 is enforced via
:func:`bind_state_ingest_thread`; the second binding from a different
thread raises :class:`StateIngestThreadAlreadyBoundError`. The runtime
root binds C4 + C5 to the SAME thread.
ADR-002 build-flag gating: ``config.components["c5_state"].strategy ==
"gtsam_isam2"`` requires ``BUILD_STATE_GTSAM_ISAM2=ON``. Default is
``ON`` (most binaries link the production-default); the operator-side
binary may set ``OFF`` and only link the ESKF baseline.
"""
from __future__ import annotations
import os
import threading
from collections.abc import Callable
from typing import TYPE_CHECKING, Any, Final
from gps_denied_onboard.components.c5_state._isam2_handle import ISam2GraphHandle
from gps_denied_onboard.components.c5_state.config import (
KNOWN_STATE_STRATEGIES,
C5StateConfig,
)
from gps_denied_onboard.components.c5_state.errors import StateEstimatorConfigError
from gps_denied_onboard.components.c5_state.interface import StateEstimator
from gps_denied_onboard.config import Config
from gps_denied_onboard.logging import get_logger
if TYPE_CHECKING:
pass
__all__ = [
"StateEstimatorFactory",
"StateIngestThreadAlreadyBoundError",
"bind_state_ingest_thread",
"build_state_estimator",
"clear_state_ingest_binding",
"clear_state_registry",
"list_registered_state_strategies",
"register_state_estimator",
]
StateEstimatorFactory = Callable[..., tuple[StateEstimator, ISam2GraphHandle]]
_STATE_REGISTRY: dict[str, StateEstimatorFactory] = {}
_STATE_BUILD_FLAGS: Final[dict[str, str]] = {
"gtsam_isam2": "BUILD_STATE_GTSAM_ISAM2",
"eskf": "BUILD_STATE_ESKF",
}
def register_state_estimator(strategy: str, factory: StateEstimatorFactory) -> None:
"""Register a concrete `StateEstimator` strategy.
Called by per-binary bootstrap modules under the matching
``BUILD_STATE_<variant>`` flag. Duplicate registration with a
different factory raises :class:`StateEstimatorConfigError`.
"""
existing = _STATE_REGISTRY.get(strategy)
if existing is not None and existing is not factory:
raise StateEstimatorConfigError(
f"duplicate StateEstimator registration for strategy {strategy!r}"
)
_STATE_REGISTRY[strategy] = factory
def clear_state_registry() -> None:
"""Reset the state registry; unit-test isolation only."""
_STATE_REGISTRY.clear()
def list_registered_state_strategies() -> list[str]:
return sorted(_STATE_REGISTRY)
# ----------------------------------------------------------------------
# Single-writer state-ingest thread (Invariant 1; C4 + C5 share it).
class StateIngestThreadAlreadyBoundError(RuntimeError):
"""Raised on a second :func:`bind_state_ingest_thread` call from a different thread."""
_ingest_lock = threading.Lock()
_ingest_bound_thread: int | None = None
def bind_state_ingest_thread(thread_ident: int | None = None) -> int:
"""Bind ``thread_ident`` (defaults to the caller) as the sole state-ingest thread.
A second call from a different thread raises
:class:`StateIngestThreadAlreadyBoundError`. C4 + C5 + the shared
GTSAM substrate live on this thread per ADR-003. Repeated binding
from the SAME thread is permitted (idempotent for re-entrant
composition under tests).
"""
global _ingest_bound_thread
ident = thread_ident if thread_ident is not None else threading.get_ident()
with _ingest_lock:
if _ingest_bound_thread is not None and _ingest_bound_thread != ident:
raise StateIngestThreadAlreadyBoundError(
f"state ingest thread already bound to {_ingest_bound_thread}; "
f"refused to re-bind to {ident}"
)
_ingest_bound_thread = ident
return ident
def clear_state_ingest_binding() -> None:
"""Reset the state-ingest-thread binding; unit-test isolation only."""
global _ingest_bound_thread
with _ingest_lock:
_ingest_bound_thread = None
# ----------------------------------------------------------------------
# Public composition-root factory.
def build_state_estimator(
config: Config,
*,
imu_preintegrator: Any,
se3_utils: Any,
wgs_converter: Any,
fdr_client: Any,
) -> tuple[StateEstimator, ISam2GraphHandle]:
"""Resolve + build the configured state estimator.
Returns the ``(StateEstimator, ISam2GraphHandle)`` tuple so the
runtime root can inject the handle into C4 via
``build_pose_estimator``.
Validation order: config block lookup → build-flag gate → factory
lookup. The first failure surfaces a :class:`StateEstimatorConfigError`
with the offending strategy + flag name so the operator gets a
clear next step.
"""
block = _read_state_block(config)
strategy = block.strategy
if strategy not in KNOWN_STATE_STRATEGIES:
raise StateEstimatorConfigError(
f"C5StateConfig.strategy={strategy!r} not in {sorted(KNOWN_STATE_STRATEGIES)}"
)
flag_name = _STATE_BUILD_FLAGS.get(strategy)
if flag_name is None:
raise StateEstimatorConfigError(
f"state strategy {strategy!r} has no BUILD_STATE_* flag mapping"
)
if os.environ.get(flag_name, "ON").upper() == "OFF":
raise StateEstimatorConfigError(
f"{flag_name} is OFF — strategy {strategy!r} is not linked into this binary"
)
factory = _STATE_REGISTRY.get(strategy)
if factory is None:
raise StateEstimatorConfigError(
f"state strategy {strategy!r} selected by config but not registered; "
f"registered strategies: {list_registered_state_strategies()}"
)
estimator, handle = factory(
config=config,
imu_preintegrator=imu_preintegrator,
se3_utils=se3_utils,
wgs_converter=wgs_converter,
fdr_client=fdr_client,
)
_log_strategy_loaded(
strategy=strategy,
keyframe_window_size=block.keyframe_window_size,
)
return estimator, handle
def _read_state_block(config: Config) -> C5StateConfig:
"""Pull the c5_state block out of ``config.components`` (or fall back to defaults)."""
components = getattr(config, "components", None) or {}
block = components.get("c5_state") if isinstance(components, dict) else None
if block is None:
# Allow missing block to mean "documented defaults" — same shape
# as the cross-cutting blocks. Tests that exercise the factory
# without YAML/env see defaults.
return C5StateConfig()
if isinstance(block, C5StateConfig):
return block
raise StateEstimatorConfigError(
f"config.components['c5_state'] must be a C5StateConfig; got {type(block).__name__}"
)
def _log_strategy_loaded(*, strategy: str, keyframe_window_size: int) -> None:
log = get_logger("runtime_root.state_factory")
log.info(
f"c5.state.strategy_loaded: strategy={strategy} "
f"keyframe_window_size={keyframe_window_size}",
extra={
"kind": "c5.state.strategy_loaded",
"kv": {
"strategy": strategy,
"keyframe_window_size": keyframe_window_size,
},
},
)