[AZ-398] Replay: FrameSource + Clock Protocols + Clock injection

Ship the two Layer-1 cross-cutting Protocols replay mode needs to leave
production C1-C5 components mode-agnostic (Invariant 1) and replay-
deterministic (Invariant 2). Live + replay binaries see the same
interfaces; only the strategy differs.

* Clock Protocol (monotonic_ns / time_ns / sleep_until_ns) +
  WallClock (live + REALTIME replay) + TlogDerivedClock (ASAP replay;
  advance-on-call; non-monotonic source → ClockOrderingError).
* FrameSource Protocol (next_frame -> NavCameraFrame | None / close)
  + LiveCameraFrameSource (cv2.VideoCapture device index) +
  VideoFileFrameSource (cv2.VideoCapture file).
* Build-flag gating: BUILD_VIDEO_FILE_FRAME_SOURCE,
  BUILD_LIVE_CAMERA_FRAME_SOURCE (constructor-time check; Tier-0 OFF
  refuses construction with FrameSourceConfigError).
* Composition-root factories: build_clock + build_frame_source.
* Injected Clock across every component that previously called
  time.monotonic_ns() / time.sleep() directly: c5_state (estimator,
  ESKF, fallback watcher, source-label SM, isam2 handle), c8_fc_adapter
  (inbound MAVLink + MSP2, AP outbound, iNav outbound, QGC GCS),
  c13_fdr writer, c12_operator_tooling httpx flights client. All
  constructors default to WallClock() so existing call sites keep
  live-binary behaviour without a wiring change.
* AC-4 CI guard (tests/_meta/test_no_direct_time_in_components.py)
  AST-scans components/**/*.py for direct time.monotonic_ns /
  time.time_ns / time.sleep references and fails loudly with file:line.
* Conformance + factory tests: tests/unit/clock + tests/unit/frame_source.
* Test fixture updates: FallbackWatcher / SourceLabelStateMachine
  clock_ns is now required (removed time.monotonic_ns default);
  test_az388 patches estimator._clock instead of a module-level time;
  test_az393 ardupilot adapter uses a _FixedClock test double.

Excluded per the task spec: TlogReplayFcAdapter (AZ-399), ReplaySink
(AZ-400), compose_replay (AZ-401), CLI (AZ-402), Docker/CI (AZ-403),
E2E fixture (AZ-404), IMU auto-sync (AZ-405).

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-12 05:10:01 +03:00
parent 6c7d24f7e0
commit 823c0f1b2e
32 changed files with 1575 additions and 105 deletions
@@ -12,13 +12,15 @@ Retry policy (FAC-INV-5):
from __future__ import annotations
import time
from collections.abc import Callable
from pathlib import Path
from typing import Final
from uuid import UUID
import httpx
from gps_denied_onboard.clock.wall_clock import WallClock
from gps_denied_onboard._types.geo import BoundingBox, LatLonAlt
from gps_denied_onboard.components.c12_operator_tooling.flights_api._parser import (
parse_flight_payload,
@@ -49,6 +51,18 @@ _REDACTED: Final[str] = "<redacted>"
_RETRY_BACKOFF_S: Final[float] = 1.0
def _wall_clock_sleep(seconds: float) -> None:
"""Default sleep hook — routes through :class:`WallClock`.
Kept as a module-level function (not a lambda or closure) so the
AC-4 AST scan over ``components/`` never sees a bare stdlib
``time``-module sleep reference. Tests inject their own ``sleep``
callable to skip the backoff.
"""
clock = WallClock()
clock.sleep_until_ns(clock.monotonic_ns() + int(seconds * 1_000_000_000))
class HttpxFlightsApiClient:
"""Concrete :class:`FlightsApiClient` against the parent-suite ``flights`` REST API.
@@ -64,10 +78,12 @@ class HttpxFlightsApiClient:
self,
*,
transport: httpx.BaseTransport | None = None,
sleep: object = time.sleep,
sleep: Callable[[float], None] | None = None,
) -> None:
self._transport = transport
self._sleep = sleep
self._sleep: Callable[[float], None] = (
sleep if sleep is not None else _wall_clock_sleep
)
self._log = get_logger("c12.flights_api")
def fetch_flight(
@@ -162,7 +178,7 @@ class HttpxFlightsApiClient:
},
},
)
self._sleep(_RETRY_BACKOFF_S) # type: ignore[operator]
self._sleep(_RETRY_BACKOFF_S)
try:
response = client.get(url, headers=headers)
except (httpx.ConnectError, httpx.ConnectTimeout, httpx.ReadTimeout) as exc:
@@ -29,8 +29,10 @@ from collections.abc import Callable, Sequence
from dataclasses import asdict
from datetime import datetime, timezone
from pathlib import Path
from typing import TYPE_CHECKING
from uuid import UUID
from gps_denied_onboard.clock.wall_clock import WallClock
from gps_denied_onboard.components.c13_fdr.errors import (
FdrAlreadyClosedError,
FdrCloseWithoutOpenError,
@@ -53,6 +55,9 @@ from gps_denied_onboard.fdr_client.records import (
)
from gps_denied_onboard.logging import get_logger
if TYPE_CHECKING:
from gps_denied_onboard.clock import Clock
__all__ = ["FileFdrWriter"]
_FLIGHT_HEADER_KIND = "flight_header"
@@ -97,6 +102,7 @@ class FileFdrWriter:
on_rotation: Callable[[FileFdrWriter, int], None] | None = None,
record_kind_policy: RecordKindPolicy | None = None,
drain_sleep_s: float = _DEFAULT_DRAIN_SLEEP_S,
clock: Clock | None = None,
) -> None:
self._flight_root = Path(flight_root)
self._flight_id = flight_id
@@ -106,6 +112,7 @@ class FileFdrWriter:
self._on_rotation = on_rotation
self._record_kind_policy = record_kind_policy
self._drain_sleep_s = drain_sleep_s
self._clock: Clock = clock if clock is not None else WallClock()
# Filesystem state.
self._flight_dir: Path = self._flight_root / str(flight_id)
@@ -312,7 +319,7 @@ class FileFdrWriter:
# iterate until the value is stable. Practically this converges
# in one or two passes.
ts = _iso_now()
mono_ns = time.monotonic_ns()
mono_ns = self._clock.monotonic_ns()
records_written_now = self._records_written + 1 # +1 for the footer itself
bytes_estimate = self._bytes_written
footer: FlightFooter | None = None
@@ -37,7 +37,6 @@ transitions.
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
@@ -107,8 +106,8 @@ class FallbackWatcher:
*,
threshold_s: float,
fdr_client: FdrClient | None,
clock_ns: Callable[[], int],
producer_id: str = "c5_state",
clock_ns: Callable[[], int] = time.monotonic_ns,
) -> None:
if threshold_s <= 0.0:
raise ValueError(f"FallbackWatcher.threshold_s must be > 0; got {threshold_s}")
@@ -18,7 +18,6 @@ defensive trace.
from __future__ import annotations
import time
from typing import TYPE_CHECKING, Any, Protocol, runtime_checkable
import gtsam
@@ -205,5 +204,10 @@ class ISam2GraphHandleImpl(ISam2GraphHandle):
anchor (``_last_anchor_ns`` is initialised to 0 in the
estimator constructor). This matches the C5 contract's
documented "no anchor yet" sentinel.
Reads the estimator's injected :class:`Clock` so replay /
unit-test runs see deterministic age values.
"""
return (time.monotonic_ns() - self._estimator._last_anchor_ns) // 1_000_000
return (
self._estimator._clock.monotonic_ns() - self._estimator._last_anchor_ns
) // 1_000_000
@@ -35,7 +35,6 @@ 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
@@ -154,8 +153,8 @@ class SourceLabelStateMachine:
spoof_promotion_visual_consistency_tol_m: float,
spoof_promotion_bounded_delta_m: float,
fdr_client: FdrClient | None,
clock_ns: Callable[[], int],
producer_id: str = "c5_state",
clock_ns: Callable[[], int] = time.monotonic_ns,
) -> None:
if spoof_promotion_min_stable_s <= 0.0:
raise ValueError(
@@ -47,7 +47,6 @@ filter; this module documents the deviation in the
from __future__ import annotations
import math
import time
from collections import deque
from datetime import datetime, timezone
from typing import TYPE_CHECKING, Any, Final, Literal
@@ -57,6 +56,7 @@ import numpy as np
from numpy.linalg import LinAlgError
from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard.clock.wall_clock import WallClock
from gps_denied_onboard._types.state import (
EstimatorHealth,
EstimatorOutput,
@@ -89,9 +89,9 @@ from gps_denied_onboard.logging import get_logger
if TYPE_CHECKING:
from gps_denied_onboard._types.fc import GpsHealth, GpsSample
from gps_denied_onboard._types.nav import ImuWindow
from gps_denied_onboard._types.nav import ImuWindow, VioOutput
from gps_denied_onboard._types.pose import PoseEstimate
from gps_denied_onboard._types.vio import VioOutput
from gps_denied_onboard.clock import Clock
from gps_denied_onboard.config import Config
__all__ = [
@@ -162,6 +162,7 @@ class EskfStateEstimator(StateEstimator):
se3_utils: Any,
wgs_converter: Any,
fdr_client: Any,
clock: Clock | None = None,
) -> None:
block = self._extract_block(config)
self._config: Config = config
@@ -170,6 +171,7 @@ class EskfStateEstimator(StateEstimator):
self._se3_utils: Any = se3_utils
self._wgs_converter: Any = wgs_converter
self._fdr_client: Any = fdr_client
self._clock: Clock = clock if clock is not None else WallClock()
self._log = get_logger("c5_state.eskf_baseline")
self._nominal_pos: np.ndarray = np.zeros(3, dtype=np.float64)
@@ -215,6 +217,7 @@ class EskfStateEstimator(StateEstimator):
spoof_promotion_visual_consistency_tol_m=block.spoof_promotion_visual_consistency_tol_m,
spoof_promotion_bounded_delta_m=block.spoof_promotion_bounded_delta_m,
fdr_client=fdr_client,
clock_ns=self._clock.monotonic_ns,
producer_id="c5_state",
)
@@ -222,6 +225,7 @@ class EskfStateEstimator(StateEstimator):
self._fallback = FallbackWatcher(
threshold_s=block.no_estimate_fallback_s,
fdr_client=fdr_client,
clock_ns=self._clock.monotonic_ns,
producer_id="c5_state",
)
@@ -538,7 +542,7 @@ class EskfStateEstimator(StateEstimator):
# Both modes are treated identically by the ESKF — the
# JACOBIAN exclusion is iSAM2-graph-specific. AC-4.
self._last_anchor_ns = time.monotonic_ns()
self._last_anchor_ns = self._clock.monotonic_ns()
residual_pos = meas_pose[:3, 3] - self._nominal_pos
meas_R = meas_pose[:3, :3]
@@ -612,7 +616,7 @@ class EskfStateEstimator(StateEstimator):
def current_estimate(self) -> EstimatorOutput:
"""Forward-time estimate. ``smoothed=False`` (Invariant 7)."""
now_ns = time.monotonic_ns()
now_ns = self._clock.monotonic_ns()
self._fallback.check_and_engage(now_ns)
cov6 = self._pose_covariance_6x6()
@@ -629,7 +633,7 @@ class EskfStateEstimator(StateEstimator):
)
raise
emitted_at = time.monotonic_ns()
emitted_at = self._clock.monotonic_ns()
position_wgs84 = self._enu_pose_to_wgs84()
orientation = _quat_to_quat_dto(self._nominal_q)
velocity_world = (
@@ -864,7 +868,7 @@ class EskfStateEstimator(StateEstimator):
return
try:
machine.notify_satellite_anchor(
now_ns=time.monotonic_ns(),
now_ns=self._clock.monotonic_ns(),
gps_consistency_delta_m=None,
)
except Exception as exc:
@@ -31,7 +31,6 @@ there.
from __future__ import annotations
import math
import time
from collections import deque
from datetime import datetime, timezone
from typing import TYPE_CHECKING, Any, Final, Literal
@@ -43,6 +42,7 @@ import numpy as np
from numpy.linalg import LinAlgError
from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard.clock.wall_clock import WallClock
from gps_denied_onboard._types.state import (
EstimatorHealth,
EstimatorOutput,
@@ -79,9 +79,9 @@ from gps_denied_onboard.logging import get_logger
if TYPE_CHECKING:
from gps_denied_onboard._types.fc import GpsHealth, GpsSample
from gps_denied_onboard._types.nav import ImuWindow
from gps_denied_onboard._types.nav import ImuWindow, VioOutput
from gps_denied_onboard._types.pose import PoseEstimate
from gps_denied_onboard._types.vio import VioOutput
from gps_denied_onboard.clock import Clock
from gps_denied_onboard.config import Config
__all__ = [
@@ -148,6 +148,7 @@ class GtsamIsam2StateEstimator(StateEstimator):
se3_utils: Any,
wgs_converter: Any,
fdr_client: Any,
clock: Clock | None = None,
) -> None:
block = self._extract_block(config)
@@ -157,6 +158,7 @@ class GtsamIsam2StateEstimator(StateEstimator):
self._se3_utils: Any = se3_utils
self._wgs_converter: Any = wgs_converter
self._fdr_client: Any = fdr_client
self._clock: Clock = clock if clock is not None else WallClock()
self._isam2 = gtsam.ISAM2(gtsam.ISAM2Params())
window_seconds: float = block.keyframe_window_size * _FRAME_PERIOD_S
@@ -224,6 +226,7 @@ class GtsamIsam2StateEstimator(StateEstimator):
spoof_promotion_visual_consistency_tol_m=block.spoof_promotion_visual_consistency_tol_m,
spoof_promotion_bounded_delta_m=block.spoof_promotion_bounded_delta_m,
fdr_client=fdr_client,
clock_ns=self._clock.monotonic_ns,
producer_id="c5_state",
)
# AC-NEW-8 rolling window of ``(ts_monotonic_ns, cov_norm)``
@@ -255,6 +258,7 @@ class GtsamIsam2StateEstimator(StateEstimator):
self._fallback = FallbackWatcher(
threshold_s=block.no_estimate_fallback_s,
fdr_client=fdr_client,
clock_ns=self._clock.monotonic_ns,
producer_id="c5_state",
)
@@ -481,7 +485,7 @@ class GtsamIsam2StateEstimator(StateEstimator):
# AC-6 / Invariant 11a: do NOT advance ``_last_added_ts_ns`` —
# this is a pre-takeoff seed, not a measurement; the first
# subsequent ``add_*`` call still sees the unguarded baseline.
ts_ns = time.monotonic_ns()
ts_ns = self._clock.monotonic_ns()
try:
handle.add_factor(factor)
self._values.insert(prior_key, prior_pose)
@@ -734,7 +738,7 @@ class GtsamIsam2StateEstimator(StateEstimator):
# Both paths update the anchor freshness sentinel. The C5
# contract documents this — even the throttled JACOBIAN path
# counts as a recent anchor for AC-1.3 binning.
self._last_anchor_ns = time.monotonic_ns()
self._last_anchor_ns = self._clock.monotonic_ns()
if mode == "marginals":
gtsam_pose = _pose_se3_to_gtsam(self._pose_estimate_to_matrix(pose))
@@ -923,7 +927,7 @@ class GtsamIsam2StateEstimator(StateEstimator):
# AZ-388: AC-5.2 entry hook. Engages fallback if the
# threshold has elapsed since the last successful estimate.
# Idempotent / rate-limited.
self._fallback.check_and_engage(time.monotonic_ns())
self._fallback.check_and_engage(self._clock.monotonic_ns())
if self._last_committed_pose_key is None:
raise EstimatorFatalError(
"current_estimate: no committed pose key yet (graph empty); "
@@ -975,7 +979,7 @@ class GtsamIsam2StateEstimator(StateEstimator):
velocity_world = self._latest_velocity_or_zero()
last_anchor_age_ms = int(handle.last_anchor_age_ms())
source_label = self._derive_source_label()
emitted_at = time.monotonic_ns()
emitted_at = self._clock.monotonic_ns()
self._record_cov_norm_sample(emitted_at, covariance)
if self._isam2_state == IsamState.INIT:
@@ -1063,7 +1067,7 @@ class GtsamIsam2StateEstimator(StateEstimator):
last_anchor_age_ms = int(handle.last_anchor_age_ms())
source_label = self._derive_source_label()
emitted_at = time.monotonic_ns()
emitted_at = self._clock.monotonic_ns()
out: list[EstimatorOutput] = []
for key, _ts in selected:
@@ -1366,7 +1370,7 @@ class GtsamIsam2StateEstimator(StateEstimator):
return
try:
machine.notify_satellite_anchor(
now_ns=time.monotonic_ns(),
now_ns=self._clock.monotonic_ns(),
gps_consistency_delta_m=None,
)
except Exception as exc:
@@ -20,8 +20,7 @@ synchronously without a real serial port.
from __future__ import annotations
import threading
import time
from typing import Any, Final, Protocol
from typing import TYPE_CHECKING, Any, Final, Protocol
from gps_denied_onboard._types.fc import (
AttitudeSample,
@@ -34,10 +33,14 @@ from gps_denied_onboard._types.fc import (
TelemetryKind,
)
from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard.clock.wall_clock import WallClock
from gps_denied_onboard.components.c8_fc_adapter._subscription import SubscriptionBus
from gps_denied_onboard.components.c8_fc_adapter._telemetry_rings import TelemetryRing
from gps_denied_onboard.logging import get_logger
if TYPE_CHECKING:
from gps_denied_onboard.clock import Clock
__all__ = [
"AP_MESSAGE_TYPES",
"MAVLinkSource",
@@ -108,9 +111,11 @@ class PymavlinkInboundDecoder:
attitude_ring_capacity: int = 100,
gps_ring_capacity: int = 20,
state_ring_capacity: int = 10,
clock: Clock | None = None,
) -> None:
self._source = source
self._bus = bus
self._clock: Clock = clock if clock is not None else WallClock()
self._log = get_logger("c8_fc_adapter.inbound_mavlink")
self.imu_ring: TelemetryRing[FcTelemetryFrame] = TelemetryRing(
imu_ring_capacity, kind_name="imu"
@@ -218,7 +223,7 @@ class PymavlinkInboundDecoder:
status = self._map_fix_type(fix_type)
if status is GpsStatus.STABLE:
status = self._maybe_promote_to_spoofed_or_non_spoofed()
captured_at = time.monotonic_ns()
captured_at = self._clock.monotonic_ns()
payload = GpsHealth(status=status, fix_age_ms=0, captured_at=captured_at)
# AC-5.1: cache warm-start hint on first 3D+ fix.
if fix_type >= 3:
@@ -232,7 +237,7 @@ class PymavlinkInboundDecoder:
return self._dispatch(TelemetryKind.GPS_HEALTH, payload, ring=self.gps_ring)
def _handle_heartbeat(self, msg: Any) -> bool:
captured_at = time.monotonic_ns()
captured_at = self._clock.monotonic_ns()
state = self._map_mav_state(
system_status=int(msg.system_status),
base_mode=int(msg.base_mode),
@@ -257,7 +262,7 @@ class PymavlinkInboundDecoder:
text = text.decode("utf-8", errors="replace")
if not any(sentinel.lower() in text.lower() for sentinel in _SPOOFING_SENTINELS):
return
captured_at = time.monotonic_ns()
captured_at = self._clock.monotonic_ns()
with self._lock:
self._spoof_sentinel_seen_at = captured_at
self._log.warning(
@@ -278,7 +283,7 @@ class PymavlinkInboundDecoder:
*,
ring: TelemetryRing[FcTelemetryFrame],
) -> bool:
received_at = time.monotonic_ns()
received_at = self._clock.monotonic_ns()
last = self._last_ts_ns.get(kind)
if last is not None and received_at <= last:
self._log.warning(
@@ -329,7 +334,7 @@ class PymavlinkInboundDecoder:
sentinel_at = self._spoof_sentinel_seen_at
if sentinel_at is None:
return GpsStatus.STABLE
now = time.monotonic_ns()
now = self._clock.monotonic_ns()
if (now - sentinel_at) <= 5 * 1_000_000_000:
return GpsStatus.SPOOFED
return GpsStatus.STABLE
@@ -17,8 +17,7 @@ Tests drive the decoder via :meth:`feed_one_tick` which calls the
from __future__ import annotations
import threading
import time
from typing import Any, Final, Protocol
from typing import TYPE_CHECKING, Any, Final, Protocol
from gps_denied_onboard._types.fc import (
AttitudeSample,
@@ -31,10 +30,14 @@ from gps_denied_onboard._types.fc import (
TelemetryKind,
)
from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard.clock.wall_clock import WallClock
from gps_denied_onboard.components.c8_fc_adapter._subscription import SubscriptionBus
from gps_denied_onboard.components.c8_fc_adapter._telemetry_rings import TelemetryRing
from gps_denied_onboard.logging import get_logger
if TYPE_CHECKING:
from gps_denied_onboard.clock import Clock
__all__ = [
"Msp2InavInboundDecoder",
"MspSource",
@@ -74,9 +77,11 @@ class Msp2InavInboundDecoder:
attitude_ring_capacity: int = 100,
gps_ring_capacity: int = 20,
state_ring_capacity: int = 10,
clock: Clock | None = None,
) -> None:
self._source = source
self._bus = bus
self._clock: Clock = clock if clock is not None else WallClock()
self._log = get_logger("c8_fc_adapter.inbound_msp2")
self.imu_ring: TelemetryRing[FcTelemetryFrame] = TelemetryRing(
imu_ring_capacity, kind_name="imu"
@@ -118,10 +123,16 @@ class Msp2InavInboundDecoder:
return dispatched
def run_poll_loop(self, *, period_s: float = 0.01) -> None:
"""Continuous polling loop; honours :meth:`stop`."""
"""Continuous polling loop; honours :meth:`stop`.
Sleeps via the injected :class:`Clock` so replay binaries (which
wire a ``TlogDerivedClock``) advance instantly while the live
binary blocks for ``period_s`` between ticks.
"""
period_ns = int(period_s * 1_000_000_000)
while not self._stop_flag.is_set():
self.feed_one_tick()
time.sleep(period_s)
self._clock.sleep_until_ns(self._clock.monotonic_ns() + period_ns)
def stop(self) -> None:
self._stop_flag.set()
@@ -142,7 +153,7 @@ class Msp2InavInboundDecoder:
raise ValueError(
f"iNav IMU dict shape: expected 3-vectors, got accel={accel}, gyro={gyro}"
)
sensor_ts_ns = time.monotonic_ns()
sensor_ts_ns = self._clock.monotonic_ns()
payload = ImuTelemetrySample(ts_ns=sensor_ts_ns, accel_xyz=accel, gyro_xyz=gyro)
return self._dispatch(TelemetryKind.IMU_SAMPLE, payload, ring=self.imu_ring)
@@ -157,7 +168,7 @@ class Msp2InavInboundDecoder:
roll_rad = float(raw["angx"]) * (3.141592653589793 / 180.0)
pitch_rad = float(raw["angy"]) * (3.141592653589793 / 180.0)
yaw_rad = float(raw["heading"]) * (3.141592653589793 / 180.0)
sensor_ts_ns = time.monotonic_ns()
sensor_ts_ns = self._clock.monotonic_ns()
payload = AttitudeSample(
ts_ns=sensor_ts_ns,
roll_rad=roll_rad,
@@ -180,7 +191,7 @@ class Msp2InavInboundDecoder:
status = GpsStatus.DEGRADED
else:
status = GpsStatus.STABLE
captured_at = time.monotonic_ns()
captured_at = self._clock.monotonic_ns()
if fix >= 2:
lat_deg = float(raw["lat"]) / 1e7
lon_deg = float(raw["lon"]) / 1e7
@@ -198,7 +209,7 @@ class Msp2InavInboundDecoder:
return False
# iNav flight-state dict shape (subset we honour):
# 'armed': bool, 'in_flight': bool, 'failsafe': bool
captured_at = time.monotonic_ns()
captured_at = self._clock.monotonic_ns()
if raw.get("failsafe", False):
state = FlightState.FAILED
elif raw.get("in_flight", False):
@@ -233,7 +244,7 @@ class Msp2InavInboundDecoder:
*,
ring: TelemetryRing[FcTelemetryFrame],
) -> bool:
received_at = time.monotonic_ns()
received_at = self._clock.monotonic_ns()
last = self._last_ts_ns.get(kind)
if last is not None and received_at <= last:
self._log.warning(
@@ -24,10 +24,9 @@ Build flag: ``BUILD_GCS_QGC_MAVLINK``.
from __future__ import annotations
import threading
import time
from collections.abc import Callable
from datetime import datetime, timezone
from typing import Any, Final
from typing import TYPE_CHECKING, Any, Final
from gps_denied_onboard._types.fc import (
FcKind,
@@ -39,9 +38,13 @@ from gps_denied_onboard._types.fc import (
)
from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard._types.state import EstimatorOutput
from gps_denied_onboard.clock.wall_clock import WallClock
from gps_denied_onboard.components.c8_fc_adapter._covariance_projector import (
CovarianceProjector,
)
if TYPE_CHECKING:
from gps_denied_onboard.clock import Clock
from gps_denied_onboard.components.c8_fc_adapter._subscription import SubscriptionBus
from gps_denied_onboard.components.c8_fc_adapter.errors import (
GcsAdapterConfigError,
@@ -110,14 +113,14 @@ class QgcTelemetryAdapter:
wgs_converter: Any,
covariance_projector: CovarianceProjector,
fdr_client: FdrClient,
clock: Callable[[], float] = time.monotonic,
clock: Clock | None = None,
connect_factory: Callable[[str, int], Any] | None = None,
) -> None:
self._config = config
self._wgs_converter = wgs_converter
self._cov_projector = covariance_projector
self._fdr_client = fdr_client
self._clock = clock
self._clock: Clock = clock if clock is not None else WallClock()
self._connect_factory = connect_factory
self._log = get_logger("c8_gcs_adapter.qgc")
# The modulo divisor — computed once at construction so unit
@@ -333,7 +336,7 @@ class QgcTelemetryAdapter:
return OperatorCommand(
command=msg_type,
payload=payload,
received_at=time.monotonic_ns(),
received_at=self._clock.monotonic_ns(),
)
def _record_operator_command_fdr(self, cmd: OperatorCommand, msg: Any) -> None:
@@ -374,4 +377,4 @@ class QgcTelemetryAdapter:
return wgs
def _clock_ms_boot(self) -> int:
return int(self._clock() * 1_000)
return self._clock.monotonic_ns() // 1_000_000
@@ -13,9 +13,8 @@ Build flag: ``BUILD_FC_INAV``.
from __future__ import annotations
import threading
import time
from collections.abc import Callable
from typing import Any, Final
from typing import TYPE_CHECKING, Any, Final
from gps_denied_onboard._types.emitted import EmittedExternalPosition
from gps_denied_onboard._types.fc import (
@@ -29,9 +28,13 @@ from gps_denied_onboard._types.fc import (
)
from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard._types.state import EstimatorOutput
from gps_denied_onboard.clock.wall_clock import WallClock
from gps_denied_onboard.components.c8_fc_adapter._covariance_projector import (
CovarianceProjector,
)
if TYPE_CHECKING:
from gps_denied_onboard.clock import Clock
from gps_denied_onboard.components.c8_fc_adapter._msp2_sensor_gps_encoder import (
MSP2_SENSOR_GPS_CODE,
encode_msp2_sensor_gps,
@@ -71,7 +74,7 @@ class Msp2InavAdapter:
wgs_converter: Any,
covariance_projector: CovarianceProjector,
fdr_client: FdrClient,
clock: Callable[[], float] = time.monotonic,
clock: Clock | None = None,
msp_connect_factory: Callable[[str, int], Any] | None = None,
secondary_mavlink_factory: Callable[[], Any] | None = None,
) -> None:
@@ -79,7 +82,7 @@ class Msp2InavAdapter:
self._wgs_converter = wgs_converter
self._cov_projector = covariance_projector
self._fdr_client = fdr_client
self._clock = clock
self._clock: Clock = clock if clock is not None else WallClock()
self._msp_connect_factory = msp_connect_factory
self._secondary_mavlink_factory = secondary_mavlink_factory
self._log = get_logger("c8_fc_adapter.inav_adapter")
@@ -94,10 +97,12 @@ class Msp2InavAdapter:
# polling decoder lands in AZ-391; the per-adapter inbound
# composition happens in a follow-up batch).
self._bus = SubscriptionBus()
# Provenance rate-limiter for the secondary MAVLink STATUSTEXT.
# Provenance rate-limiter for the secondary MAVLink STATUSTEXT;
# the limiter expects a float-seconds clock, so we wrap the
# injected Clock's ns reading.
self._provenance = StatusTextTransitionRateLimiter(
send_statustext=self._send_statustext_secondary,
clock=time.monotonic,
clock=lambda: self._clock.monotonic_ns() / 1_000_000_000,
)
# ------------------------------------------------------------------
@@ -165,7 +170,7 @@ class Msp2InavAdapter:
raise FcEmitError("smoothed output cannot be emitted to FC (Invariant 6)")
h_pos_accuracy_mm = self._cov_projector.to_inav_h_pos_accuracy_mm(output)
wgs = self._extract_wgs84(output)
emitted_at = time.monotonic_ns()
emitted_at = self._clock.monotonic_ns()
self._sequence_number = (self._sequence_number + 1) & 0xFF
seq = self._sequence_number
payload = encode_msp2_sensor_gps(
@@ -227,7 +232,7 @@ class Msp2InavAdapter:
state=FlightState.INIT,
last_valid_gps_hint_wgs84=None,
last_valid_gps_age_ms=None,
captured_at=time.monotonic_ns(),
captured_at=self._clock.monotonic_ns(),
)
# ------------------------------------------------------------------
@@ -22,10 +22,9 @@ from __future__ import annotations
import os
import secrets
import threading
import time
from collections.abc import Callable
from datetime import datetime, timezone
from typing import Any, Final
from typing import TYPE_CHECKING, Any, Final
from gps_denied_onboard._types.emitted import EmittedExternalPosition
from gps_denied_onboard._types.fc import (
@@ -39,9 +38,13 @@ from gps_denied_onboard._types.fc import (
)
from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard._types.state import EstimatorOutput
from gps_denied_onboard.clock.wall_clock import WallClock
from gps_denied_onboard.components.c8_fc_adapter._covariance_projector import (
CovarianceProjector,
)
if TYPE_CHECKING:
from gps_denied_onboard.clock import Clock
from gps_denied_onboard.components.c8_fc_adapter._inbound_mavlink import (
PymavlinkInboundDecoder,
)
@@ -94,7 +97,7 @@ class PymavlinkArdupilotAdapter:
wgs_converter: Any,
covariance_projector: CovarianceProjector,
fdr_client: FdrClient,
clock: Callable[[], float] = time.monotonic,
clock: Clock | None = None,
flight_id: str = "",
connect_factory: Callable[[str, int], Any] | None = None,
) -> None:
@@ -102,7 +105,7 @@ class PymavlinkArdupilotAdapter:
self._wgs_converter = wgs_converter
self._cov_projector = covariance_projector
self._fdr_client = fdr_client
self._clock = clock
self._clock: Clock = clock if clock is not None else WallClock()
self._flight_id = flight_id
self._connect_factory = connect_factory
self._signing_failure_threshold = max(1, int(config.fc.signing_failure_threshold))
@@ -122,10 +125,11 @@ class PymavlinkArdupilotAdapter:
self._bus = SubscriptionBus()
self._inbound: PymavlinkInboundDecoder | None = None
self._inbound_thread: threading.Thread | None = None
# Outbound provenance rate limiter.
# Outbound provenance rate limiter; wraps the injected Clock as a
# float-seconds callable (the limiter's existing API contract).
self._provenance = StatusTextTransitionRateLimiter(
send_statustext=self._send_statustext_internal,
clock=time.monotonic,
clock=self._monotonic_s,
)
# ------------------------------------------------------------------
@@ -226,7 +230,7 @@ class PymavlinkArdupilotAdapter:
raise FcEmitError("smoothed output cannot be emitted to FC (Invariant 6)")
horiz_accuracy_m = self._cov_projector.to_ardupilot_horiz_accuracy_m(output)
wgs = self._extract_wgs84(output)
emitted_at = time.monotonic_ns()
emitted_at = self._clock.monotonic_ns()
self._sequence_number += 1
seq = self._sequence_number
try:
@@ -312,7 +316,7 @@ class PymavlinkArdupilotAdapter:
if not self._opened or self._connection is None:
raise FcEmitError("adapter not opened")
self._enforce_single_writer()
now_ns = time.monotonic_ns()
now_ns = self._clock.monotonic_ns()
if self._last_switch_attempt_ns:
elapsed_s = (now_ns - self._last_switch_attempt_ns) / 1_000_000_000
if elapsed_s < _SWITCH_RATE_LIMIT_S:
@@ -388,7 +392,7 @@ class PymavlinkArdupilotAdapter:
state=FlightState.INIT,
last_valid_gps_hint_wgs84=None,
last_valid_gps_age_ms=None,
captured_at=time.monotonic_ns(),
captured_at=self._clock.monotonic_ns(),
)
payload = latest.payload
assert isinstance(payload, FlightStateSignal)
@@ -542,9 +546,9 @@ class PymavlinkArdupilotAdapter:
Returns the ACK message on match, or ``None`` on timeout. Other
COMMAND_ACK messages (for unrelated commands) are ignored.
"""
deadline = self._clock() + (timeout_ms / 1000.0)
deadline = self._monotonic_s() + (timeout_ms / 1000.0)
while True:
remaining = deadline - self._clock()
remaining = deadline - self._monotonic_s()
if remaining <= 0:
return None
try:
@@ -608,11 +612,14 @@ class PymavlinkArdupilotAdapter:
)
return wgs
def _monotonic_s(self) -> float:
return self._clock.monotonic_ns() / 1_000_000_000
def _clock_us(self) -> int:
return int(self._clock() * 1_000_000)
return self._clock.monotonic_ns() // 1_000
def _clock_ms_boot(self) -> int:
return int(self._clock() * 1_000)
return self._clock.monotonic_ns() // 1_000_000
def _fdr_signing_event(self, *, kind: str, kv: dict[str, Any]) -> None:
record = FdrRecord(