mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 20:11:14 +00:00
[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:
@@ -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(
|
||||
|
||||
Reference in New Issue
Block a user