mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 18:41:13 +00:00
[AZ-390] [AZ-392] C8 FC/GCS adapter foundation + covariance projector
Adds the C8 foundation: - FcAdapter / GcsAdapter / ReplaySink Protocols + contract DTOs in _types/fc.py (PortConfig, FcKind, FlightState, GpsStatus, Severity, TelemetryKind, FcTelemetryFrame, FlightStateSignal, GpsHealth, OperatorCommand, Subscription, Imu/Attitude samples). - Disjoint FcAdapterError / GcsAdapterError trees with SourceSetSwitchNotSupportedError <: SourceSetSwitchError per AC-9. - FcConfig + GcsConfig cross-cutting Config blocks with config-load validation (unknown strategy rejected at __post_init__). - runtime_root/fc_factory.py: build_fc_adapter / build_gcs_adapter with BUILD_FC_*/BUILD_GCS_* flag gating + INFO log on load + single-writer outbound-thread binding. - CovarianceProjector (helper, AZ-392): 6x6 -> 3x3 -> 2x2 -> sqrt(lambda_max) reduction; AP returns float m, iNav returns int mm with uint16 clamp + WARN + FDR record. Non-SPD / NaN / wrong-shape raise FcEmitError and emit an FDR ERROR record carrying frame_id. Contracts: - composition_root_protocol.md 1.1.0 -> 1.2.0 (added fc/gcs blocks + build_fc_adapter / build_gcs_adapter + outbound-thread binding). - fc_adapter_protocol.md unchanged (this batch implements v1.0.0). Tests: 410 pass / 2 skip / 0 fail (+53 new tests in batch 8). AZ-391 (inbound subscription) deferred to batch 9 — pulls YAMSPy as a new external dependency (iNav MSP2 decode). Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -1,4 +1,4 @@
|
||||
"""C8 FC + GCS Adapter component — Public API."""
|
||||
"""C8 FC + GCS Adapter component — Public API (AZ-390 / E-C8)."""
|
||||
|
||||
from gps_denied_onboard._types.emitted import EmittedExternalPosition
|
||||
from gps_denied_onboard.components.c8_fc_adapter.interface import (
|
||||
|
||||
@@ -0,0 +1,188 @@
|
||||
"""`CovarianceProjector` — honest 6x6 -> 2x2 -> equivalent_radius (AZ-392 / E-C8).
|
||||
|
||||
The single source of truth for converting a 6x6 GTSAM ``Marginals``
|
||||
covariance from C5's ``EstimatorOutput`` into the per-FC scalar
|
||||
horizontal-accuracy field. Lives inside C8 (helper-only; not in the
|
||||
public API per ``module-layout.md``).
|
||||
|
||||
Steps per Invariant 4 + AC-4.3:
|
||||
|
||||
1. ``cov_6x6 -> cov_3x3`` — top-left 3x3 block (position sub-matrix
|
||||
in the SE(3) parameterisation [x, y, z, rx, ry, rz]).
|
||||
2. ``cov_3x3 -> cov_2x2`` — top-left 2x2 (horizontal) sub-matrix.
|
||||
3. ``equivalent_radius = sqrt(largest_eigenvalue(cov_2x2))`` with the
|
||||
closed-form solution ``sqrt(0.5 * (sigma_xx + sigma_yy + sqrt(
|
||||
(sigma_xx - sigma_yy)**2 + 4 * sigma_xy**2)))`` (bit-stable; no
|
||||
numpy ``eigvalsh`` round-off drift).
|
||||
|
||||
Non-SPD / NaN inputs raise :class:`FcEmitError` BEFORE any per-FC
|
||||
unit conversion runs (AC-6 / AC-7) and emit a single FDR record
|
||||
``kind="c8.cov_projector.spd_violation"`` carrying the offending
|
||||
``frame_id`` so C13 post-mortem tooling can correlate emit drops
|
||||
with the upstream C5 frame.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from datetime import datetime, timezone
|
||||
from typing import Final
|
||||
|
||||
import numpy as np
|
||||
|
||||
from gps_denied_onboard._types.pose 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
|
||||
from gps_denied_onboard.logging import get_logger
|
||||
|
||||
__all__ = ["CovarianceProjector"]
|
||||
|
||||
|
||||
_SPD_VIOLATION_KIND: Final[str] = "c8.cov_projector.spd_violation"
|
||||
_INAV_CLAMPED_KIND: Final[str] = "c8.cov_projector.inav_clamped"
|
||||
# iNav MSP2_SENSOR_GPS.hPosAccuracy is a uint16 in millimetres.
|
||||
_INAV_HPOS_MAX_MM: Final[int] = 65535
|
||||
|
||||
|
||||
def _is_spd_2x2(m: np.ndarray) -> bool:
|
||||
"""Strict 2x2 SPD check: symmetric, positive determinant, positive trace."""
|
||||
if not np.allclose(m, m.T, atol=1e-9):
|
||||
return False
|
||||
a, b = float(m[0, 0]), float(m[0, 1])
|
||||
d = float(m[1, 1])
|
||||
det = a * d - b * b
|
||||
trace = a + d
|
||||
return det > 0.0 and trace > 0.0
|
||||
|
||||
|
||||
def _largest_eigenvalue_2x2(m: np.ndarray) -> float:
|
||||
"""Closed-form largest eigenvalue of a 2x2 symmetric SPD matrix.
|
||||
|
||||
The 2x2 SPD case is dominated by the closed-form
|
||||
``0.5 * (a + d + sqrt((a - d)^2 + 4 * b^2))``; numpy's
|
||||
``eigvalsh`` has the same answer up to floating-point round-off
|
||||
but allocates + dispatches to LAPACK. We prefer the closed-form
|
||||
for bit-stability on per-emit calls (AC-9 cross-FC equality
|
||||
relies on identical intermediate arithmetic).
|
||||
"""
|
||||
a = float(m[0, 0])
|
||||
b = float(m[0, 1])
|
||||
d = float(m[1, 1])
|
||||
return 0.5 * (a + d + math.sqrt((a - d) * (a - d) + 4.0 * b * b))
|
||||
|
||||
|
||||
class CovarianceProjector:
|
||||
"""Honest 6x6 -> 2x2 -> equivalent_radius projector for C8 outbound emit."""
|
||||
|
||||
def __init__(self, fdr_client: FdrClient) -> None:
|
||||
self._fdr_client = fdr_client
|
||||
self._log = get_logger("c8_fc_adapter.cov_projector")
|
||||
|
||||
def to_ardupilot_horiz_accuracy_m(self, output: EstimatorOutput) -> float:
|
||||
"""Project a 6x6 covariance to meters for AP ``GPS_INPUT.horiz_accuracy``."""
|
||||
radius_m = self._equivalent_radius_m(output)
|
||||
return radius_m
|
||||
|
||||
def to_inav_h_pos_accuracy_mm(self, output: EstimatorOutput) -> int:
|
||||
"""Project a 6x6 covariance to millimeters for iNav ``MSP2_SENSOR_GPS.hPosAccuracy``.
|
||||
|
||||
Clamps at ``_INAV_HPOS_MAX_MM`` (uint16); emits a single WARN
|
||||
log per clamp event (AC-5).
|
||||
"""
|
||||
radius_m = self._equivalent_radius_m(output)
|
||||
# Round half-up to int; built-in round() uses banker's rounding,
|
||||
# so add 0.5 + math.floor for the AC-4 spec.
|
||||
radius_mm = math.floor(radius_m * 1000.0 + 0.5)
|
||||
if radius_mm > _INAV_HPOS_MAX_MM:
|
||||
self._log.warning(
|
||||
f"c8.cov_projector.inav_clamped: {radius_mm} -> {_INAV_HPOS_MAX_MM}",
|
||||
extra={
|
||||
"kind": _INAV_CLAMPED_KIND,
|
||||
"kv": {
|
||||
"radius_mm_raw": radius_mm,
|
||||
"clamped_to": _INAV_HPOS_MAX_MM,
|
||||
"frame_id": output.frame_id,
|
||||
},
|
||||
},
|
||||
)
|
||||
return _INAV_HPOS_MAX_MM
|
||||
return radius_mm
|
||||
|
||||
def _equivalent_radius_m(self, output: EstimatorOutput) -> float:
|
||||
"""Shared 6x6 -> 3x3 -> 2x2 -> sqrt(lambda_max) reduction."""
|
||||
cov_6x6 = output.covariance_6x6
|
||||
if cov_6x6 is None:
|
||||
self._fdr_log_violation(reason="missing", frame_id=output.frame_id)
|
||||
raise FcEmitError("missing covariance from C5; refusing emit")
|
||||
cov_arr = np.asarray(cov_6x6, dtype=np.float64)
|
||||
if cov_arr.shape != (6, 6):
|
||||
self._fdr_log_violation(
|
||||
reason="bad_shape",
|
||||
frame_id=output.frame_id,
|
||||
extra={"shape": list(cov_arr.shape)},
|
||||
)
|
||||
raise FcEmitError(f"covariance_6x6 must be 6x6; got shape={cov_arr.shape}")
|
||||
if not np.all(np.isfinite(cov_arr)):
|
||||
self._fdr_log_violation(reason="nan_or_inf", frame_id=output.frame_id)
|
||||
raise FcEmitError("NaN covariance from C5; refusing emit")
|
||||
cov_3x3 = cov_arr[:3, :3]
|
||||
cov_2x2 = cov_3x3[:2, :2]
|
||||
if not _is_spd_2x2(cov_2x2):
|
||||
self._fdr_log_violation(
|
||||
reason="non_spd",
|
||||
frame_id=output.frame_id,
|
||||
extra={
|
||||
"cov_2x2": cov_2x2.tolist(),
|
||||
},
|
||||
)
|
||||
raise FcEmitError("non-SPD covariance from C5; refusing emit")
|
||||
lam = _largest_eigenvalue_2x2(cov_2x2)
|
||||
if lam <= 0.0 or not math.isfinite(lam):
|
||||
self._fdr_log_violation(
|
||||
reason="degenerate_eigenvalue",
|
||||
frame_id=output.frame_id,
|
||||
extra={"lambda": lam},
|
||||
)
|
||||
raise FcEmitError("degenerate covariance eigenvalue; refusing emit")
|
||||
return math.sqrt(lam)
|
||||
|
||||
def _fdr_log_violation(
|
||||
self,
|
||||
*,
|
||||
reason: str,
|
||||
frame_id: int,
|
||||
extra: dict | None = None,
|
||||
) -> None:
|
||||
payload: dict = {"reason": reason, "frame_id": frame_id}
|
||||
if extra:
|
||||
payload.update(extra)
|
||||
# The FDR schema closes ``kind`` to the documented set; we
|
||||
# use the ``log`` kind which carries an arbitrary ``kv`` per the
|
||||
# AZ-272 contract so this projector's WARN can survive
|
||||
# roundtrip without a schema bump.
|
||||
record = FdrRecord(
|
||||
schema_version=1,
|
||||
ts=datetime.now(tz=timezone.utc).isoformat(),
|
||||
producer_id="c8_fc_adapter",
|
||||
kind="log",
|
||||
payload={
|
||||
"level": "ERROR",
|
||||
"component": "c8_fc_adapter",
|
||||
"frame_id": frame_id,
|
||||
"kind": _SPD_VIOLATION_KIND,
|
||||
"msg": "covariance projector rejected emit",
|
||||
"kv": payload,
|
||||
},
|
||||
)
|
||||
try:
|
||||
self._fdr_client.enqueue(record)
|
||||
except Exception as exc:
|
||||
# FDR enqueue failure must not mask the SPD failure path.
|
||||
self._log.error(
|
||||
"cov_projector.fdr_enqueue_failed",
|
||||
extra={
|
||||
"kind": "c8.cov_projector.fdr_enqueue_failed",
|
||||
"kv": {"error": repr(exc), "reason": reason},
|
||||
},
|
||||
)
|
||||
@@ -0,0 +1,98 @@
|
||||
"""C8 FcAdapter / GcsAdapter error hierarchy (AZ-390 / E-C8).
|
||||
|
||||
Two disjoint trees so consumers can ``except FcAdapterError`` to catch
|
||||
every flight-controller adapter failure without also catching ground
|
||||
station failures (and vice versa). Sub-classes carry the specific
|
||||
contract semantics (e.g. ``SourceSetSwitchNotSupportedError`` is a
|
||||
subclass of ``SourceSetSwitchError`` so iNav's rejection is catchable
|
||||
as either form per AC-9).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
__all__ = [
|
||||
"FcAdapterConfigError",
|
||||
"FcAdapterError",
|
||||
"FcEmitError",
|
||||
"FcOpenError",
|
||||
"GcsAdapterConfigError",
|
||||
"GcsAdapterError",
|
||||
"GcsEmitError",
|
||||
"SigningHandshakeError",
|
||||
"SigningKeyExpiredError",
|
||||
"SourceSetSwitchError",
|
||||
"SourceSetSwitchNotSupportedError",
|
||||
]
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# FC adapter tree
|
||||
|
||||
|
||||
class FcAdapterError(Exception):
|
||||
"""Base class for every `FcAdapter` failure (Invariant catch-all)."""
|
||||
|
||||
|
||||
class FcOpenError(FcAdapterError):
|
||||
"""`FcAdapter.open()` failed (port unavailable, signing missing, etc.)."""
|
||||
|
||||
|
||||
class FcEmitError(FcAdapterError):
|
||||
"""`emit_external_position` / `emit_status_text` failed.
|
||||
|
||||
Raised on non-SPD / NaN covariance (Invariant 4),
|
||||
`output.smoothed == True` (Invariant 6), wire-encode failure, and
|
||||
write-side OS errors.
|
||||
"""
|
||||
|
||||
|
||||
class SigningHandshakeError(FcAdapterError):
|
||||
"""MAVLink 2.0 per-flight signing handshake failed (AP only)."""
|
||||
|
||||
|
||||
class SigningKeyExpiredError(FcAdapterError):
|
||||
"""The current per-flight signing key has rotated out under us."""
|
||||
|
||||
|
||||
class SourceSetSwitchError(FcAdapterError):
|
||||
"""`request_source_set_switch()` failed (ACK timeout, REJECTED, etc.)."""
|
||||
|
||||
|
||||
class SourceSetSwitchNotSupportedError(SourceSetSwitchError):
|
||||
"""`request_source_set_switch()` is not implementable for this FC variant.
|
||||
|
||||
Raised by `Msp2InavAdapter` (iNav has no equivalent of
|
||||
`MAV_CMD_SET_EKF_SOURCE_SET`). Sub-classes `SourceSetSwitchError`
|
||||
so callers can either catch the specific case or treat it as a
|
||||
generic switch failure (AC-9).
|
||||
"""
|
||||
|
||||
|
||||
class FcAdapterConfigError(FcAdapterError):
|
||||
"""Bad / mismatched config for an FC adapter.
|
||||
|
||||
Raised at config-load for unknown strategy names (AC-5) and at
|
||||
factory build for build-flag-OFF strategies (AC-4); also by
|
||||
`Msp2InavAdapter.open(...)` when a non-None `signing_key` is
|
||||
passed (Invariant 2 — iNav has no signing per RESTRICT-COMM-2).
|
||||
"""
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# GCS adapter tree
|
||||
|
||||
|
||||
class GcsAdapterError(Exception):
|
||||
"""Base class for every `GcsAdapter` failure."""
|
||||
|
||||
|
||||
class GcsEmitError(GcsAdapterError):
|
||||
"""`GcsAdapter.emit_summary` / `emit_status_text` failed."""
|
||||
|
||||
|
||||
class GcsAdapterConfigError(GcsAdapterError):
|
||||
"""Bad / mismatched config for a GCS adapter.
|
||||
|
||||
Raised at config-load for unknown strategy names and at factory
|
||||
build for build-flag-OFF strategies.
|
||||
"""
|
||||
@@ -1,47 +1,85 @@
|
||||
"""C8 Adapter Protocols: `FcAdapter`, `GcsAdapter`, `ReplaySink`.
|
||||
"""C8 `FcAdapter` + `GcsAdapter` Protocols (AZ-390 / E-C8).
|
||||
|
||||
Concrete impls: `PymavlinkArdupilotAdapter`, `Msp2InavAdapter`,
|
||||
`MavlinkGcsAdapter`, `TlogReplayFcAdapter`, `JsonlReplaySink`. See
|
||||
`_docs/02_document/components/10_c8_fc_adapter/`.
|
||||
Concrete strategies (linked at build time per ADR-002):
|
||||
- AP: `PymavlinkArdupilotAdapter` (AZ-393 outbound, AZ-391 inbound, AZ-395 signing)
|
||||
- iNav: `Msp2InavAdapter` (AZ-394 outbound, AZ-391 inbound)
|
||||
- GCS: `QgcTelemetryAdapter` (AZ-397)
|
||||
|
||||
Replay extensions (`TlogReplayFcAdapter`, `JsonlReplaySink`) implement
|
||||
the same Protocols and live under separate build flags (E-DEMO-REPLAY).
|
||||
|
||||
Public-API restriction: only `FcAdapter`, `GcsAdapter`, `ReplaySink`,
|
||||
plus the contract DTOs in `_types/fc.py` and `_types/emitted.py`.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from collections.abc import Iterator
|
||||
from typing import Protocol
|
||||
from typing import Protocol, runtime_checkable
|
||||
|
||||
from gps_denied_onboard._types.emitted import EmittedExternalPosition
|
||||
from gps_denied_onboard._types.nav import (
|
||||
AttitudeWindow,
|
||||
from gps_denied_onboard._types.fc import (
|
||||
FlightStateSignal,
|
||||
GpsHealth,
|
||||
ImuSample,
|
||||
OperatorCommandCallback,
|
||||
PortConfig,
|
||||
Severity,
|
||||
Subscription,
|
||||
TelemetryCallback,
|
||||
)
|
||||
from gps_denied_onboard._types.pose import EstimatorOutput
|
||||
|
||||
__all__ = ["FcAdapter", "GcsAdapter", "ReplaySink"]
|
||||
|
||||
|
||||
@runtime_checkable
|
||||
class FcAdapter(Protocol):
|
||||
"""Bidirectional flight-controller adapter."""
|
||||
"""Per-FC flight-controller adapter (inbound telemetry + outbound emit).
|
||||
|
||||
def outbound(self, position: EmittedExternalPosition) -> None: ...
|
||||
Invariants enforced by concrete implementations (see contract):
|
||||
- Single open per instance; re-open raises `FcOpenError`.
|
||||
- `close()` is idempotent.
|
||||
- `emit_external_position` rejects `output.smoothed == True`.
|
||||
- Outbound methods are called from one dedicated emit thread.
|
||||
- Inbound subscribe-callbacks fire on the decoder thread.
|
||||
"""
|
||||
|
||||
def inbound_imu(self) -> Iterator[ImuSample]: ...
|
||||
def open(self, port: PortConfig, signing_key: bytes | None) -> None: ...
|
||||
|
||||
def inbound_attitude(self) -> Iterator[AttitudeWindow]: ...
|
||||
def close(self) -> None: ...
|
||||
|
||||
def inbound_gps_health(self) -> Iterator[GpsHealth]: ...
|
||||
def subscribe_telemetry(self, callback: TelemetryCallback) -> Subscription: ...
|
||||
|
||||
def inbound_flight_state(self) -> Iterator[FlightStateSignal]: ...
|
||||
def emit_external_position(self, output: EstimatorOutput) -> EmittedExternalPosition: ...
|
||||
|
||||
def emit_status_text(self, msg: str, severity: Severity) -> None: ...
|
||||
|
||||
def request_source_set_switch(self) -> None: ...
|
||||
|
||||
def current_flight_state(self) -> FlightStateSignal: ...
|
||||
|
||||
|
||||
@runtime_checkable
|
||||
class GcsAdapter(Protocol):
|
||||
"""Ground-control-station adapter (telemetry + operator commands)."""
|
||||
"""Ground-control-station adapter (downsampled summary + operator commands)."""
|
||||
|
||||
def emit_summary(self, summary: dict) -> None: ...
|
||||
def open(self, port: PortConfig) -> None: ...
|
||||
|
||||
def operator_commands(self) -> Iterator[dict]: ...
|
||||
def close(self) -> None: ...
|
||||
|
||||
def emit_summary(self, output: EstimatorOutput) -> None: ...
|
||||
|
||||
def subscribe_operator_commands(self, callback: OperatorCommandCallback) -> Subscription: ...
|
||||
|
||||
def emit_status_text(self, msg: str, severity: Severity) -> None: ...
|
||||
|
||||
|
||||
@runtime_checkable
|
||||
class ReplaySink(Protocol):
|
||||
"""Replay-mode estimate sink (e.g. JSONL writer)."""
|
||||
"""Replay-mode estimate sink (e.g. JSONL writer).
|
||||
|
||||
def write(self, estimate: dict) -> None: ...
|
||||
Lives in the same module so the replay binary's composition root
|
||||
can wire `JsonlReplaySink` alongside the production adapters.
|
||||
Excluded from `__init__.__all__` in production-only builds via the
|
||||
`BUILD_REPLAY_SINK_JSONL` flag.
|
||||
"""
|
||||
|
||||
def write(self, output: EstimatorOutput) -> None: ...
|
||||
|
||||
Reference in New Issue
Block a user