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

[AZ-405] Replay — replay_input/ coordinator + IMU take-off auto-sync

Browse Source 
Adds the Layer-4 cross-cutting `replay_input/` module per ADR-011:
ReplayInputAdapter converges (video, tlog) into the standard
FrameSource + FcAdapter + Clock surfaces the airborne composition
root consumes. Owns time-alignment between video frames and tlog
IMU/attitude ticks (manual via --time-offset-ms or auto via the
AZ-405 IMU-take-off detector + Farneback motion-onset detector).

Auto-sync algorithm (auto_sync.py):
- Tlog take-off detector: sustained vertical-accel excess > 0.5 g for
  >= 0.5 s + sustained attitude-rate magnitude > 1 rad/s.
- Video motion-onset detector: dense Farneback flow magnitude > 1.5 px
  sustained >= 0.5 s (deterministic per AC-10).
- compute_offset combines the two; confidence = min(tlog, video).
- validate_offset_or_fail implements the AC-9 95 % frame-window match
  validator with configurable threshold + window.

ReplayInputAdapter.open() ordering (AC-13):
1. Load tlog samples + fail-fast on missing RAW_IMU/SCALED_IMU2 or
   ATTITUDE BEFORE any video read.
2. Resolve offset (auto-sync OR manual override; manual bypasses the
   detectors entirely per AC-8).
3. Run AC-9 validator on resolved offset; raise auto-sync hard-fail
   for AC-7 (CLI exit 2 mapping).
4. Build single Clock instance per pace (TlogDerived/ASAP, Wall/REAL).
5. Construct VideoFileFrameSource and TlogReplayFcAdapter with the
   resolved offset baked in (replay protocol Invariant 8).

Structured log + FDR records on auto-sync detected / low-confidence /
AC-8 hard-fail kinds. Idempotent close (AC-12).

Tests: 25 unit tests across tests/unit/replay_input/ covering all 13
ACs (kernel-level synthetic fixtures for AC-1..AC-10; coordinator-
level OpenCV synthetic videos + faked pymavlink for AC-6..AC-13).

Contract update: replay_protocol.md v2.0.0 added fdr_client to the
ReplayInputAdapter __init__ signature (was missing in the prose; the
task spec already listed it in the allowed-imports section).

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-14 09:50:51 +03:00
parent f9b4241d3a
commit 8149083cac
14 changed files with 2979 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tests/unit/replay_input/test_az405_auto_sync.py
+483
View File
@@ -0,0 +1,483 @@
"""AZ-405 — auto-sync detector + offset-compute + AC-9 validator.
Covers AC-1..AC-10 of ``_docs/02_tasks/todo/AZ-405_replay_auto_sync.md``.
Tests run against the pure compute kernels in
:mod:`gps_denied_onboard.replay_input.auto_sync` (no disk IO, no real
pymavlink, no real OpenCV) so the suite is fast + deterministic.
Style: every test follows the Arrange / Act / Assert pattern.
"""
from __future__ import annotations
import math
from typing import Any
import pytest
from gps_denied_onboard.replay_input.auto_sync import (
TlogSamples,
_compute_tlog_takeoff_from_samples,
_compute_video_onset_from_samples,
compute_offset,
validate_offset_or_fail,
)
from gps_denied_onboard.replay_input.errors import ReplayInputAdapterError
from gps_denied_onboard.replay_input.interface import AutoSyncConfig
# ---------------------------------------------------------------------
# Synthetic-fixture helpers
def _ns(seconds: float) -> int:
return int(seconds * 1_000_000_000)
def _flat_accel_samples(
*, start_s: float, end_s: float, hz: int, total_g: float
) -> list[tuple[int, float]]:
out: list[tuple[int, float]] = []
period = 1.0 / hz
t = start_s
while t < end_s:
out.append((_ns(t), total_g))
t += period
return out
def _flat_attitude_samples(
*, start_s: float, end_s: float, hz: int, roll: float, pitch: float, yaw: float
) -> list[tuple[int, float, float, float]]:
out: list[tuple[int, float, float, float]] = []
period = 1.0 / hz
t = start_s
while t < end_s:
out.append((_ns(t), roll, pitch, yaw))
t += period
return out
def _ramp_attitude_samples(
*,
start_s: float,
end_s: float,
hz: int,
base_roll: float,
base_pitch: float,
base_yaw: float,
rate_rad_s: float,
) -> list[tuple[int, float, float, float]]:
"""Attitude that ramps in pitch at ``rate_rad_s`` rad/s."""
out: list[tuple[int, float, float, float]] = []
period = 1.0 / hz
t = start_s
while t < end_s:
dt = t - start_s
pitch = base_pitch + rate_rad_s * dt
out.append((_ns(t), base_roll, pitch, base_yaw))
t += period
return out
def _build_takeoff_samples() -> TlogSamples:
"""AC-1 fixture: 2 s flat hover, then 1.5 s sustained 2.2 g + 1.5 rad/s.
Take-off onset is at t = 2.0 s (the first sample with the
elevated acceleration). Body-frame accelerometer convention: at
hover the proper-acceleration magnitude is 1 g (gravity reaction);
during a 1.2 g thrust climb it is 2.2 g, so the take-off excess
above 1 g rest is 1.2 g — well above the 0.5 g threshold.
"""
accel_pre = _flat_accel_samples(start_s=0.0, end_s=2.0, hz=200, total_g=1.0)
accel_post = _flat_accel_samples(start_s=2.0, end_s=3.5, hz=200, total_g=2.2)
accel = accel_pre + accel_post
attitude_pre = _flat_attitude_samples(
start_s=0.0, end_s=2.0, hz=100, roll=0.0, pitch=0.0, yaw=0.0
)
attitude_post = _ramp_attitude_samples(
start_s=2.0,
end_s=3.5,
hz=100,
base_roll=0.0,
base_pitch=0.0,
base_yaw=0.0,
rate_rad_s=1.5,
)
attitude = attitude_pre + attitude_post
return TlogSamples(
accel=tuple(accel),
attitude=tuple(attitude),
imu_count_by_type={
"RAW_IMU": len(accel),
"ATTITUDE": len(attitude),
},
)
def _build_low_amplitude_vibration_samples() -> TlogSamples:
"""AC-2 fixture: 5 s of 0.3 g body-frame vibration (no take-off).
Total proper-acceleration during vibration = 1.3 g (0.3 g excess
above the 1 g rest baseline) — strictly below the 0.5 g detector
threshold so the sustained-event search rejects every window.
"""
accel = _flat_accel_samples(start_s=0.0, end_s=5.0, hz=200, total_g=1.3)
attitude = _flat_attitude_samples(
start_s=0.0, end_s=5.0, hz=100, roll=0.0, pitch=0.0, yaw=0.0
)
return TlogSamples(
accel=tuple(accel),
attitude=tuple(attitude),
imu_count_by_type={
"RAW_IMU": len(accel),
"ATTITUDE": len(attitude),
},
)
def _build_hand_launch_samples() -> TlogSamples:
"""AC-3 fixture: 0.8 g impulse for 100 ms; not sustained for 0.5 s.
Body-frame accelerometer convention (see ``_build_takeoff_samples``):
a 0.8 g impulse becomes 1.8 g total proper-acceleration during the
impulse window.
"""
accel_pre = _flat_accel_samples(start_s=0.0, end_s=2.0, hz=200, total_g=1.0)
accel_impulse = _flat_accel_samples(
start_s=2.0, end_s=2.1, hz=200, total_g=1.8
)
accel_post = _flat_accel_samples(start_s=2.1, end_s=3.0, hz=200, total_g=1.0)
accel = accel_pre + accel_impulse + accel_post
attitude = _flat_attitude_samples(
start_s=0.0, end_s=3.0, hz=100, roll=0.0, pitch=0.0, yaw=0.0
)
return TlogSamples(
accel=tuple(accel),
attitude=tuple(attitude),
imu_count_by_type={
"RAW_IMU": len(accel),
"ATTITUDE": len(attitude),
},
)
def _flow_samples_from_frames(
*, n_stationary: int, n_moving: int, fps: int = 30, motion_px: float = 4.0
) -> tuple[tuple[int, float], ...]:
"""Synthesise the flow-magnitude series the video detector consumes.
The detector emits a ``(ts_ns, mean_magnitude_px)`` tuple for each
consecutive frame pair (skipping the first frame's pair). For
AC-4 we pretend frames 1..10 are stationary (mag ≈ 0) and frames
11..60 are moving (mag = motion_px).
"""
out: list[tuple[int, float]] = []
period_ns = int(1_000_000_000 / fps)
for i in range(1, n_stationary):
out.append((i * period_ns, 0.05))
for j in range(n_stationary, n_stationary + n_moving):
out.append((j * period_ns, motion_px))
return tuple(out)
# ---------------------------------------------------------------------
# AC-1 — tlog take-off detector (positive)
def test_ac1_tlog_takeoff_detector_positive_within_50ms_and_high_confidence() -> None:
# Arrange
config = AutoSyncConfig()
samples = _build_takeoff_samples()
# Act
result = _compute_tlog_takeoff_from_samples(samples, config)
# Assert
expected_onset_ns = _ns(2.0)
assert abs(result.onset_ns - expected_onset_ns) <= _ns(0.05), (
f"detected onset {result.onset_ns / 1e9}s deviates >50ms from expected 2.0s"
)
assert result.confidence >= 0.85, (
f"confidence {result.confidence} below AC-1 minimum of 0.85"
)
# ---------------------------------------------------------------------
# AC-2 — tlog take-off detector (ambiguous)
def test_ac2_tlog_takeoff_detector_low_amplitude_vibration_low_confidence() -> None:
# Arrange
config = AutoSyncConfig()
samples = _build_low_amplitude_vibration_samples()
# Act
result = _compute_tlog_takeoff_from_samples(samples, config)
# Assert
assert result.confidence < 0.50, (
f"confidence {result.confidence} should be < 0.50 for ambiguous vibration"
)
# ---------------------------------------------------------------------
# AC-3 — tlog take-off detector (hand launch)
def test_ac3_tlog_takeoff_detector_hand_launch_warn_regime() -> None:
# Arrange
config = AutoSyncConfig()
samples = _build_hand_launch_samples()
# Act
result = _compute_tlog_takeoff_from_samples(samples, config)
# Assert
assert result.confidence < 0.80, (
f"confidence {result.confidence} should be < 0.80 for unsustained hand-launch"
)
# ---------------------------------------------------------------------
# AC-4 — video motion-onset detector
def test_ac4_video_motion_onset_detected_within_one_frame() -> None:
# Arrange
config = AutoSyncConfig()
flow_samples = _flow_samples_from_frames(n_stationary=10, n_moving=50, fps=30)
period_ns = int(1_000_000_000 / 30)
expected_onset_ns = 10 * period_ns
# Act
result = _compute_video_onset_from_samples(flow_samples, config)
# Assert
assert abs(result.onset_ns - expected_onset_ns) <= period_ns, (
f"detected motion onset {result.onset_ns} ns deviates >1 frame "
f"from expected {expected_onset_ns} ns"
)
assert result.confidence > 0.80, (
f"confidence {result.confidence} too low for clear motion onset"
)
# ---------------------------------------------------------------------
# AC-5 — combined offset within ± 200 ms
def test_ac5_combined_offset_within_200ms_of_ground_truth() -> None:
# Arrange
config = AutoSyncConfig()
tlog_samples = _build_takeoff_samples()
tlog_result = _compute_tlog_takeoff_from_samples(tlog_samples, config)
flow_samples = _flow_samples_from_frames(n_stationary=10, n_moving=50, fps=30)
video_result = _compute_video_onset_from_samples(flow_samples, config)
# Ground-truth offset = tlog take-off (2.0 s) − video onset (10/30 s)
period_ns = int(1_000_000_000 / 30)
ground_truth_offset_ms = (_ns(2.0) - 10 * period_ns) // 1_000_000
# Act
decision = compute_offset(tlog_result, video_result)
# Assert
assert abs(decision.offset_ms - ground_truth_offset_ms) <= 200, (
f"offset {decision.offset_ms} ms deviates >200 ms from ground truth "
f"{ground_truth_offset_ms} ms"
)
# ---------------------------------------------------------------------
# AC-6 — low combined confidence (verified via the coordinator test
# in test_az405_replay_input_adapter.py; here we only verify the
# combined-confidence aggregator picks min())
def test_ac6_combined_confidence_takes_minimum_of_inputs() -> None:
# Arrange
from gps_denied_onboard.replay_input.auto_sync import _DetectorResult
high = _DetectorResult(onset_ns=_ns(1.0), confidence=0.95)
low = _DetectorResult(onset_ns=_ns(2.0), confidence=0.50)
# Act
decision = compute_offset(high, low)
# Assert
assert decision.combined_confidence == pytest.approx(0.50)
assert decision.offset_ms == (_ns(1.0) - _ns(2.0)) // 1_000_000
# ---------------------------------------------------------------------
# AC-7 — AC-9 validator hard-fail (the coordinator-level raise is
# covered in test_az405_replay_input_adapter.py)
def test_ac7_validator_hard_fail_returns_2_for_offset_outside_window() -> None:
# Arrange
fps = 30
period_ns = int(1_000_000_000 / fps)
video_ts = [i * period_ns for i in range(60)]
# IMU sampled at 200 Hz from t=0 to t=2 (mismatch deliberate; the
# bad offset shifts everything outside the window).
imu_ts = [int(i / 200 * 1_000_000_000) for i in range(400)]
bad_offset_ms = 60_000
# Act
code = validate_offset_or_fail(
bad_offset_ms,
imu_ts,
video_ts,
threshold_pct=95.0,
window_ms=100,
)
# Assert
assert code == 2
# ---------------------------------------------------------------------
# AC-9 — frame-window match-percentage validator (positive)
def test_ac9_validator_passes_for_well_matched_offset() -> None:
# Arrange
fps = 30
period_ns = int(1_000_000_000 / fps)
video_ts = [i * period_ns for i in range(60)]
# IMU samples densely spanning the same time range — every video
# frame has an IMU sample within ± 100 ms.
imu_ts = [int(i / 200 * 1_000_000_000) for i in range(60 * 200 // 30)]
# Act
code = validate_offset_or_fail(
0, imu_ts, video_ts, threshold_pct=95.0, window_ms=100
)
# Assert
assert code == 0
def test_ac9_threshold_configurable() -> None:
# Arrange — set up a series where exactly 80% of frames match.
fps = 30
period_ns = int(1_000_000_000 / fps)
video_ts = [i * period_ns for i in range(50)]
# IMU only covers the first 80% of the video timeline; the last
# 10 frames will be far outside the window.
imu_ts = [
int(i / 200 * 1_000_000_000) for i in range(int(40 / 30 * 200))
]
# Act / Assert
# Default 95% threshold → fail (80% < 95%).
assert validate_offset_or_fail(
0, imu_ts, video_ts, threshold_pct=95.0, window_ms=100
) == 2
# Lowered to 75% → pass.
assert validate_offset_or_fail(
0, imu_ts, video_ts, threshold_pct=75.0, window_ms=100
) == 0
# ---------------------------------------------------------------------
# AC-10 — confidence determinism
def test_ac10_confidence_score_deterministic_across_two_runs() -> None:
# Arrange
config = AutoSyncConfig()
samples = _build_takeoff_samples()
# Act
first = _compute_tlog_takeoff_from_samples(samples, config)
second = _compute_tlog_takeoff_from_samples(samples, config)
# Assert
assert first.onset_ns == second.onset_ns
assert math.isclose(first.confidence, second.confidence, abs_tol=1e-9)
def test_ac10_video_onset_deterministic_across_two_runs() -> None:
# Arrange
config = AutoSyncConfig()
flow_samples = _flow_samples_from_frames(n_stationary=5, n_moving=20, fps=30)
# Act
first = _compute_video_onset_from_samples(flow_samples, config)
second = _compute_video_onset_from_samples(flow_samples, config)
# Assert
assert first.onset_ns == second.onset_ns
assert math.isclose(first.confidence, second.confidence, abs_tol=1e-9)
# ---------------------------------------------------------------------
# R-DEMO-3 fail-fast on the pure compute path
def test_pure_takeoff_kernel_raises_on_no_imu_samples() -> None:
# Arrange
config = AutoSyncConfig()
samples = TlogSamples(
accel=(),
attitude=(),
imu_count_by_type={"ATTITUDE": 100},
)
# Act / Assert
with pytest.raises(ReplayInputAdapterError, match="tlog missing required"):
_compute_takeoff_or_propagate(samples, config)
def test_pure_takeoff_kernel_raises_on_no_attitude_samples() -> None:
# Arrange
config = AutoSyncConfig()
accel = _flat_accel_samples(start_s=0.0, end_s=1.0, hz=200, total_g=1.0)
samples = TlogSamples(
accel=tuple(accel),
attitude=(),
imu_count_by_type={"RAW_IMU": len(accel)},
)
# Act / Assert
with pytest.raises(ReplayInputAdapterError, match="tlog missing required"):
_compute_takeoff_or_propagate(samples, config)
def _compute_takeoff_or_propagate(samples: TlogSamples, config: AutoSyncConfig) -> Any:
"""Local trampoline so the assertions are explicit even if the
underscore-named helper migrates."""
return _compute_tlog_takeoff_from_samples(samples, config)
# ---------------------------------------------------------------------
# AC-9 edge cases
def test_validator_returns_2_on_empty_video_or_tlog() -> None:
# Arrange
imu_ts = [0, 1_000_000, 2_000_000]
video_ts: list[int] = []
# Act / Assert — empty video
assert (
validate_offset_or_fail(
0, imu_ts, video_ts, threshold_pct=95.0, window_ms=100
)
== 2
)
# Empty tlog
assert (
validate_offset_or_fail(
0, [], [0, 1_000_000], threshold_pct=95.0, window_ms=100
)
== 2
)