azaion/gps-denied-onboard
1
0
Fork 0
mirror of https://github.com/azaion/gps-denied-onboard.git synced 2026-06-22 20:41:12 +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
tests/unit/c8_fc_adapter/test_az390_adapter_protocol.py
+1 -1
View File
@@ -38,7 +38,7 @@ from gps_denied_onboard._types.fc import (
Subscription,
TelemetryKind,
)
from gps_denied_onboard._types.pose import EstimatorOutput
from gps_denied_onboard._types.state import EstimatorOutput
from gps_denied_onboard.components.c8_fc_adapter import (
FcAdapter,
GcsAdapter,
tests/unit/c8_fc_adapter/test_az392_covariance_projector.py
+37 -12
View File
@@ -14,28 +14,53 @@ from __future__ import annotations
import logging
import math
from datetime import datetime, timezone
from unittest import mock
from uuid import UUID
import numpy as np
import pytest
from gps_denied_onboard._types.pose import EstimatorHealth, EstimatorOutput
from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard._types.state import (
EstimatorOutput,
PoseSourceLabel,
Quat,
)
from gps_denied_onboard.components.c8_fc_adapter._covariance_projector import (
CovarianceProjector,
)
from gps_denied_onboard.components.c8_fc_adapter.errors import FcEmitError
from gps_denied_onboard.fdr_client.records import FdrRecord
_TEST_FRAME_ID = UUID("00000000-0000-0000-0000-000000000007")
def _output(cov: np.ndarray | None, frame_id: int = 7) -> EstimatorOutput:
def _output(cov: np.ndarray | None, frame_id: UUID = _TEST_FRAME_ID) -> EstimatorOutput:
return EstimatorOutput(
frame_id=frame_id,
timestamp=datetime.now(tz=timezone.utc),
pose_se3=np.eye(4),
covariance_6x6=cov,
source_label="visual_propagated",
health=EstimatorHealth(),
position_wgs84=LatLonAlt(lat_deg=50.0, lon_deg=30.0, alt_m=100.0),
orientation_world_T_body=Quat(w=1.0, x=0.0, y=0.0, z=0.0),
velocity_world_mps=(0.0, 0.0, 0.0),
covariance_6x6=cov if cov is not None else np.eye(6), # placeholder; tests override
source_label=PoseSourceLabel.VISUAL_PROPAGATED,
last_satellite_anchor_age_ms=0,
smoothed=False,
emitted_at=0,
)
def _output_with_cov(cov: np.ndarray | None) -> EstimatorOutput:
"""Variant that allows None covariance for missing-cov tests."""
return EstimatorOutput(
frame_id=_TEST_FRAME_ID,
position_wgs84=LatLonAlt(lat_deg=50.0, lon_deg=30.0, alt_m=100.0),
orientation_world_T_body=Quat(w=1.0, x=0.0, y=0.0, z=0.0),
velocity_world_mps=(0.0, 0.0, 0.0),
covariance_6x6=cov, # type: ignore[arg-type]
source_label=PoseSourceLabel.VISUAL_PROPAGATED,
last_satellite_anchor_age_ms=0,
smoothed=False,
emitted_at=0,
)
@@ -150,7 +175,7 @@ def test_ac4_non_spd_raises_fc_emit_error_and_logs_fdr() -> None:
record: FdrRecord = fdr.enqueue.call_args.args[0]
assert record.kind == "log"
assert record.payload["kv"]["reason"] == "non_spd"
assert record.payload["kv"]["frame_id"] == 7
assert record.payload["kv"]["frame_id"] == str(_TEST_FRAME_ID)
def test_ac4_asymmetric_2x2_rejected() -> None:
@@ -196,7 +221,7 @@ def test_ac5_missing_covariance_rejected() -> None:
# Arrange
fdr = _fake_fdr_client()
proj = CovarianceProjector(fdr_client=fdr)
out = _output(cov=None)
out = _output_with_cov(cov=None)
# Act + Assert
with pytest.raises(FcEmitError, match=r"missing"):
@@ -208,7 +233,7 @@ def test_ac5_wrong_shape_rejected() -> None:
# Arrange
fdr = _fake_fdr_client()
proj = CovarianceProjector(fdr_client=fdr)
out = _output(cov=np.eye(5))
out = _output_with_cov(cov=np.eye(5))
# Act + Assert
with pytest.raises(FcEmitError, match=r"6x6"):
@@ -266,7 +291,7 @@ def test_ac7_inav_clamps_at_uint16_max(caplog: pytest.LogCaptureFixture) -> None
]
assert len(clamp_records) == 1
assert clamp_records[0].kv["clamped_to"] == 65535
assert clamp_records[0].kv["frame_id"] == 7
assert clamp_records[0].kv["frame_id"] == str(_TEST_FRAME_ID)
def test_ac7_inav_exact_at_uint16_max_not_clamped() -> None:
tests/unit/c8_fc_adapter/test_az393_ardupilot_outbound.py
+20 -10
View File
@@ -9,16 +9,20 @@ from __future__ import annotations
import logging
import threading
from datetime import datetime, timezone
from typing import Any
from unittest import mock
from uuid import UUID, uuid4
import numpy as np
import pytest
from gps_denied_onboard._types.fc import FcKind, PortConfig
from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard._types.pose import EstimatorOutput
from gps_denied_onboard._types.state import (
EstimatorOutput,
PoseSourceLabel,
Quat,
)
from gps_denied_onboard.components.c8_fc_adapter._covariance_projector import (
CovarianceProjector,
)
@@ -108,25 +112,31 @@ def _config_for_ap(tmp_path, *, signing_key_source: str = "none"):
def _make_output(
*,
source_label: str = "visual_propagated",
source_label: PoseSourceLabel = PoseSourceLabel.VISUAL_PROPAGATED,
smoothed: bool = False,
cov: np.ndarray | None = None,
wgs: LatLonAlt | None = None,
frame_id: int = 1,
frame_id: UUID | int | None = None,
) -> EstimatorOutput:
if cov is None:
cov = np.eye(6, dtype=np.float64) * 0.25
if wgs is None:
wgs = LatLonAlt(lat_deg=50.0, lon_deg=30.0, alt_m=100.0)
if frame_id is None:
frame_id = uuid4()
elif isinstance(frame_id, int):
# Deterministic UUID for legacy int-keyed tests.
frame_id = UUID(int=frame_id)
return EstimatorOutput(
frame_id=frame_id,
timestamp=datetime.now(tz=timezone.utc),
pose_se3=np.eye(4),
position_wgs84=wgs,
orientation_world_T_body=Quat(w=1.0, x=0.0, y=0.0, z=0.0),
velocity_world_mps=(0.0, 0.0, 0.0),
covariance_6x6=cov,
source_label=source_label,
health=None,
last_satellite_anchor_age_ms=0,
smoothed=smoothed,
extras={"wgs84": wgs},
emitted_at=0,
)
@@ -184,7 +194,7 @@ def test_ac3_named_value_float_every_frame(
assert len(conn.mav.named_value_float_calls) == 100
for _, name, value in conn.mav.named_value_float_calls:
assert name == b"src_lbl"
assert value == pytest.approx(source_label_to_float("visual_propagated"))
assert value == pytest.approx(source_label_to_float(PoseSourceLabel.VISUAL_PROPAGATED))
# ----------------------------------------------------------------------
@@ -199,7 +209,7 @@ def test_ac4_statustext_only_on_transition(
# rate-limiter's min_interval_s — AC-4 measures the transition
# behaviour, not the secondary 1 Hz spam-defence cap.
adapter._provenance._min_interval_s = 0.0 # type: ignore[attr-defined]
labels = ["visual_propagated", "sat_anchored"]
labels = [PoseSourceLabel.VISUAL_PROPAGATED, PoseSourceLabel.SATELLITE_ANCHORED]
for i in range(100):
label = labels[(i // 10) % 2]
adapter.emit_external_position(_make_output(source_label=label, frame_id=i))
tests/unit/c8_fc_adapter/test_az394_inav_outbound.py
+18 -8
View File
@@ -4,16 +4,20 @@ from __future__ import annotations
import logging
import threading
from datetime import datetime, timezone
from typing import Any
from unittest import mock
from uuid import UUID, uuid4
import numpy as np
import pytest
from gps_denied_onboard._types.fc import FcKind, PortConfig
from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard._types.pose import EstimatorOutput
from gps_denied_onboard._types.state import (
EstimatorOutput,
PoseSourceLabel,
Quat,
)
from gps_denied_onboard.components.c8_fc_adapter._covariance_projector import (
CovarianceProjector,
)
@@ -89,24 +93,30 @@ def _inav_config(tmp_path) -> Any:
def _make_output(
*,
source_label: str = "visual_propagated",
source_label: PoseSourceLabel = PoseSourceLabel.VISUAL_PROPAGATED,
smoothed: bool = False,
cov: np.ndarray | None = None,
wgs: LatLonAlt | None = None,
frame_id: int = 1,
frame_id: UUID | int | None = None,
) -> EstimatorOutput:
if cov is None:
cov = np.eye(6, dtype=np.float64) * 0.25
if wgs is None:
wgs = LatLonAlt(lat_deg=50.0, lon_deg=30.0, alt_m=100.0)
if frame_id is None:
frame_id = uuid4()
elif isinstance(frame_id, int):
frame_id = UUID(int=frame_id)
return EstimatorOutput(
frame_id=frame_id,
timestamp=datetime.now(tz=timezone.utc),
pose_se3=np.eye(4),
position_wgs84=wgs,
orientation_world_T_body=Quat(w=1.0, x=0.0, y=0.0, z=0.0),
velocity_world_mps=(0.0, 0.0, 0.0),
covariance_6x6=cov,
source_label=source_label,
last_satellite_anchor_age_ms=0,
smoothed=smoothed,
extras={"wgs84": wgs},
emitted_at=0,
)
@@ -179,7 +189,7 @@ def test_ac3_statustext_secondary_only_on_transitions(
adapter: Msp2InavAdapter, msp: _MspStub, secondary: _SecondaryMavStub
) -> None:
adapter._provenance._min_interval_s = 0.0 # type: ignore[attr-defined]
labels = ["visual_propagated", "sat_anchored"]
labels = [PoseSourceLabel.VISUAL_PROPAGATED, PoseSourceLabel.SATELLITE_ANCHORED]
for i in range(100):
label = labels[(i // 10) % 2]
adapter.emit_external_position(_make_output(source_label=label, frame_id=i))
tests/unit/c8_fc_adapter/test_az395_mavlink_signing.py
+17 -7
View File
@@ -4,17 +4,21 @@ from __future__ import annotations
import logging
import re
from datetime import datetime, timezone
from types import SimpleNamespace
from typing import Any
from unittest import mock
from uuid import UUID, uuid4
import numpy as np
import pytest
from gps_denied_onboard._types.fc import FcKind, PortConfig, Severity
from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard._types.pose import EstimatorOutput
from gps_denied_onboard._types.state import (
EstimatorOutput,
PoseSourceLabel,
Quat,
)
from gps_denied_onboard.components.c8_fc_adapter._covariance_projector import (
CovarianceProjector,
)
@@ -96,15 +100,21 @@ def _port() -> PortConfig:
return PortConfig(fc_kind=FcKind.ARDUPILOT_PLANE, device="/dev/null", baud=921600)
def _make_output(frame_id: int = 1) -> EstimatorOutput:
def _make_output(frame_id: UUID | int | None = None) -> EstimatorOutput:
if frame_id is None:
frame_id = uuid4()
elif isinstance(frame_id, int):
frame_id = UUID(int=frame_id)
return EstimatorOutput(
frame_id=frame_id,
timestamp=datetime.now(tz=timezone.utc),
pose_se3=np.eye(4),
position_wgs84=LatLonAlt(lat_deg=50.0, lon_deg=30.0, alt_m=100.0),
orientation_world_T_body=Quat(w=1.0, x=0.0, y=0.0, z=0.0),
velocity_world_mps=(0.0, 0.0, 0.0),
covariance_6x6=np.eye(6, dtype=np.float64) * 0.25,
source_label="visual_propagated",
source_label=PoseSourceLabel.VISUAL_PROPAGATED,
last_satellite_anchor_age_ms=0,
smoothed=False,
extras={"wgs84": LatLonAlt(lat_deg=50.0, lon_deg=30.0, alt_m=100.0)},
emitted_at=0,
)
tests/unit/c8_fc_adapter/test_az397_qgc_telemetry.py
+27 -11
View File
@@ -4,10 +4,10 @@ from __future__ import annotations
import logging
import threading
from datetime import datetime, timezone
from types import SimpleNamespace
from typing import Any
from unittest import mock
from uuid import UUID, uuid4
import numpy as np
import pytest
@@ -19,7 +19,11 @@ from gps_denied_onboard._types.fc import (
Severity,
)
from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard._types.pose import EstimatorOutput
from gps_denied_onboard._types.state import (
EstimatorOutput,
PoseSourceLabel,
Quat,
)
from gps_denied_onboard.components.c8_fc_adapter._covariance_projector import (
CovarianceProjector,
)
@@ -76,15 +80,25 @@ def _config(*, summary_rate_hz: float = 2.0) -> Any:
return load_config(env=env, paths=(), require_env=False)
def _make_output(*, source_label: str = "visual_propagated", frame_id: int = 1) -> EstimatorOutput:
def _make_output(
*,
source_label: PoseSourceLabel = PoseSourceLabel.VISUAL_PROPAGATED,
frame_id: UUID | int | None = None,
) -> EstimatorOutput:
if frame_id is None:
frame_id = uuid4()
elif isinstance(frame_id, int):
frame_id = UUID(int=frame_id)
return EstimatorOutput(
frame_id=frame_id,
timestamp=datetime.now(tz=timezone.utc),
pose_se3=np.eye(4),
position_wgs84=LatLonAlt(lat_deg=50.0, lon_deg=30.0, alt_m=100.0),
orientation_world_T_body=Quat(w=1.0, x=0.0, y=0.0, z=0.0),
velocity_world_mps=(0.0, 0.0, 0.0),
covariance_6x6=np.eye(6, dtype=np.float64) * 0.25,
source_label=source_label,
last_satellite_anchor_age_ms=0,
smoothed=False,
extras={"wgs84": LatLonAlt(lat_deg=50.0, lon_deg=30.0, alt_m=100.0)},
emitted_at=0,
)
@@ -168,13 +182,15 @@ def test_ac3_summary_frame_fields() -> None:
try:
wgs = LatLonAlt(lat_deg=50.45, lon_deg=30.52, alt_m=180.0)
output = EstimatorOutput(
frame_id=1,
timestamp=datetime.now(tz=timezone.utc),
pose_se3=np.eye(4),
frame_id=UUID(int=1),
position_wgs84=wgs,
orientation_world_T_body=Quat(w=1.0, x=0.0, y=0.0, z=0.0),
velocity_world_mps=(0.0, 0.0, 0.0),
covariance_6x6=np.diag([0.25, 0.25, 9.0, 1.0, 1.0, 1.0]).astype(np.float64),
source_label="visual_propagated",
source_label=PoseSourceLabel.VISUAL_PROPAGATED,
last_satellite_anchor_age_ms=0,
smoothed=False,
extras={"wgs84": wgs},
emitted_at=0,
)
a.emit_summary(output)
assert len(conn.mav.global_position_int_calls) == 1