mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 02:11:14 +00:00
[AZ-698] Multi-flight tlog handling: segment first, pick last flight
Real derkachi.tlog covers 3 takeoffs at the same field but the uploaded video covers only the last. Original NCC argmax + AZ-405 head-takeoff fallback both biased toward flight 1, violating the spec's "the last chunk in tlog is relevant" framing. Patch: pre-NCC flight segmenter partitions the IMU energy stream into distinct flights (threshold + gap walk); find_aligned_window restricts NCC search to the last segment; low-confidence fallback uses that segment's start instead of head-takeoff detection. AlignedWindow gains flight_count_detected + selected_flight_index for FDR-visible audit. 7 new unit tests (segmenter shapes + end-to-end multi-flight pipeline + segmented fallback path). 19 AZ-698 tests pass, 113 in the regression slice. Zero new mypy --strict errors. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -35,8 +35,10 @@ from gps_denied_onboard.components.c8_fc_adapter.tlog_replay_adapter import (
|
||||
from gps_denied_onboard.replay_input.auto_sync import (
|
||||
_align_via_cross_correlation,
|
||||
_resample_uniform,
|
||||
_segment_flights_from_imu_energy,
|
||||
compute_offset,
|
||||
detect_video_motion_onset,
|
||||
find_aligned_window,
|
||||
validate_offset_or_fail,
|
||||
)
|
||||
from gps_denied_onboard.replay_input.errors import ReplayInputAdapterError
|
||||
@@ -110,6 +112,31 @@ def _build_double_burst_stream(
|
||||
return tuple(out)
|
||||
|
||||
|
||||
def _build_multi_flight_stream(
|
||||
*,
|
||||
flights: tuple[tuple[float, float], ...],
|
||||
end_s: float,
|
||||
hz: float,
|
||||
in_flight_amplitude: float = 0.3,
|
||||
ground_amplitude: float = 0.02,
|
||||
) -> tuple[tuple[int, float], ...]:
|
||||
"""Build a multi-flight IMU energy stream.
|
||||
|
||||
``flights`` is a tuple of ``(start_s, end_s)`` per flight. Between
|
||||
flights the energy is ``ground_amplitude``; inside each flight it
|
||||
is ``in_flight_amplitude``. Used by the multi-flight segmentation
|
||||
tests to mimic a real "3 takeoffs at the same field" tlog.
|
||||
"""
|
||||
out: list[tuple[int, float]] = []
|
||||
period_s = 1.0 / hz
|
||||
t = 0.0
|
||||
while t < end_s:
|
||||
in_flight = any(s <= t < e for s, e in flights)
|
||||
out.append((_ns(t), in_flight_amplitude if in_flight else ground_amplitude))
|
||||
t += period_s
|
||||
return tuple(out)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# AC-1: takeoff-aligned regression — find_aligned_window must produce
|
||||
# the same offset (within ± 50 ms) as the AZ-405 compute_offset path
|
||||
@@ -614,3 +641,295 @@ def test_autosync_decision_offset_is_within_ac9_window_for_baseline() -> None:
|
||||
# Assert
|
||||
assert decision.offset_ms == 2_000
|
||||
assert decision.combined_confidence == pytest.approx(0.85, abs=1e-6)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------
|
||||
# Multi-flight tlog handling (user constraint: "if 1 flight take it, if
|
||||
# multiple take the last"). The pre-NCC segmenter is the gatekeeper.
|
||||
|
||||
|
||||
def test_segmenter_one_flight_returns_single_span() -> None:
|
||||
# Arrange: 120 s tlog with a single flight from t=10..100 s.
|
||||
samples = _build_multi_flight_stream(
|
||||
flights=((10.0, 100.0),),
|
||||
end_s=120.0,
|
||||
hz=10.0,
|
||||
)
|
||||
|
||||
# Act
|
||||
segments = _segment_flights_from_imu_energy(
|
||||
samples,
|
||||
motion_threshold=0.1,
|
||||
min_flight_duration_ns=_ns(30.0),
|
||||
max_internal_gap_ns=_ns(5.0),
|
||||
)
|
||||
|
||||
# Assert
|
||||
assert len(segments) == 1
|
||||
seg_start_ns, seg_end_ns = segments[0]
|
||||
assert abs(seg_start_ns - _ns(10.0)) <= _ns(0.2)
|
||||
assert abs(seg_end_ns - _ns(100.0)) <= _ns(0.2)
|
||||
|
||||
|
||||
def test_segmenter_three_flights_returns_three_spans_in_order() -> None:
|
||||
# Arrange: 360 s tlog with three takeoffs (60 s flights with 30 s
|
||||
# ground gaps between them) — mimics the Derkachi scenario the
|
||||
# user flagged: one tlog, three sorties, video covers only the
|
||||
# last one.
|
||||
flights_def = ((10.0, 70.0), (100.0, 160.0), (190.0, 250.0))
|
||||
samples = _build_multi_flight_stream(
|
||||
flights=flights_def,
|
||||
end_s=300.0,
|
||||
hz=10.0,
|
||||
)
|
||||
|
||||
# Act
|
||||
segments = _segment_flights_from_imu_energy(
|
||||
samples,
|
||||
motion_threshold=0.1,
|
||||
min_flight_duration_ns=_ns(30.0),
|
||||
max_internal_gap_ns=_ns(5.0),
|
||||
)
|
||||
|
||||
# Assert
|
||||
assert len(segments) == 3
|
||||
for (actual_start, actual_end), (want_start, want_end) in zip(
|
||||
segments, flights_def
|
||||
):
|
||||
assert abs(actual_start - _ns(want_start)) <= _ns(0.2)
|
||||
assert abs(actual_end - _ns(want_end)) <= _ns(0.2)
|
||||
|
||||
|
||||
def test_segmenter_drops_ground_blip_below_min_duration() -> None:
|
||||
# Arrange: a 5 s ground manoeuvre (engine test) followed by a
|
||||
# real 60 s flight. With min_flight_duration_ns=30 s the blip
|
||||
# must be discarded, leaving only the real flight.
|
||||
samples = _build_multi_flight_stream(
|
||||
flights=((5.0, 10.0), (50.0, 110.0)),
|
||||
end_s=120.0,
|
||||
hz=10.0,
|
||||
)
|
||||
|
||||
# Act
|
||||
segments = _segment_flights_from_imu_energy(
|
||||
samples,
|
||||
motion_threshold=0.1,
|
||||
min_flight_duration_ns=_ns(30.0),
|
||||
max_internal_gap_ns=_ns(5.0),
|
||||
)
|
||||
|
||||
# Assert
|
||||
assert len(segments) == 1
|
||||
seg_start_ns, _seg_end_ns = segments[0]
|
||||
assert abs(seg_start_ns - _ns(50.0)) <= _ns(0.2)
|
||||
|
||||
|
||||
def test_segmenter_keeps_brief_cruise_lull_inside_flight() -> None:
|
||||
# Arrange: one flight with a 3 s cruise lull mid-way. The lull is
|
||||
# below max_internal_gap_ns=5 s, so the segmenter must keep the
|
||||
# whole flight as a single segment.
|
||||
samples = _build_multi_flight_stream(
|
||||
flights=((10.0, 45.0), (48.0, 100.0)),
|
||||
end_s=120.0,
|
||||
hz=10.0,
|
||||
)
|
||||
|
||||
# Act
|
||||
segments = _segment_flights_from_imu_energy(
|
||||
samples,
|
||||
motion_threshold=0.1,
|
||||
min_flight_duration_ns=_ns(30.0),
|
||||
max_internal_gap_ns=_ns(5.0),
|
||||
)
|
||||
|
||||
# Assert
|
||||
assert len(segments) == 1
|
||||
seg_start_ns, seg_end_ns = segments[0]
|
||||
assert abs(seg_start_ns - _ns(10.0)) <= _ns(0.2)
|
||||
assert abs(seg_end_ns - _ns(100.0)) <= _ns(0.2)
|
||||
|
||||
|
||||
def test_find_aligned_window_picks_last_flight_for_multi_flight_tlog(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
# Arrange: a 300 s tlog with three sorties (10..70, 100..160,
|
||||
# 190..250 s). The video covers only the LAST sortie — flow
|
||||
# samples at video-clock 0..30 s with a motion burst at
|
||||
# video t=5 s that, on the tlog timeline, corresponds to
|
||||
# tlog t=200 s (5 s into flight 3 which starts at 190 s).
|
||||
flights_def = ((10.0, 70.0), (100.0, 160.0), (190.0, 250.0))
|
||||
tlog_energy = _build_multi_flight_stream(
|
||||
flights=flights_def,
|
||||
end_s=260.0,
|
||||
hz=10.0,
|
||||
)
|
||||
flow_samples = _build_motion_burst_stream(
|
||||
start_s=0.0,
|
||||
end_s=30.0,
|
||||
hz=10.0,
|
||||
burst_at_s=5.0,
|
||||
burst_amplitude=2.0,
|
||||
burst_duration_s=3.0,
|
||||
baseline_amplitude=0.05,
|
||||
)
|
||||
config = AutoSyncConfig(
|
||||
alignment_segment_min_flight_duration_seconds=30.0,
|
||||
alignment_segment_max_internal_gap_seconds=5.0,
|
||||
)
|
||||
|
||||
# Inject the pre-loaded IMU energy by monkey-patching the loader
|
||||
# used inside find_aligned_window; the function reads a tlog via
|
||||
# pymavlink, but for the unit-level invariant we want to assert
|
||||
# the segment selection — not the binary parser.
|
||||
import gps_denied_onboard.replay_input.auto_sync as auto_sync_mod
|
||||
|
||||
fake_tlog = tmp_path / "multi_flight.tlog"
|
||||
fake_tlog.write_bytes(b"\x00")
|
||||
fake_video = tmp_path / "video.mp4"
|
||||
fake_video.write_bytes(b"\x00")
|
||||
|
||||
def _fake_loader(
|
||||
path: Path,
|
||||
*,
|
||||
max_messages: int,
|
||||
source_factory: Any,
|
||||
) -> tuple[tuple[int, float], ...]:
|
||||
return tlog_energy
|
||||
|
||||
def _fake_frames(
|
||||
path: Path, scan_seconds: float,
|
||||
) -> "list[tuple[int, Any]]":
|
||||
import numpy as np
|
||||
|
||||
rng = np.random.default_rng(42)
|
||||
frames: list[tuple[int, Any]] = []
|
||||
prev_offset = np.zeros((16, 16, 3), dtype=np.int16)
|
||||
for ts_ns, mag in flow_samples:
|
||||
# 3-channel BGR (cvtColor BGR→GRAY needs ≥ 3 channels).
|
||||
# During a burst we shift pixels — that motion is what
|
||||
# Farneback flow magnitudes pick up.
|
||||
base = rng.integers(0, 30, size=(16, 16, 3), dtype=np.int16)
|
||||
shift_px = int(mag * 4)
|
||||
if shift_px > 0:
|
||||
base = np.roll(base, shift=shift_px, axis=0)
|
||||
frame = np.clip(base + prev_offset, 0, 255).astype(np.uint8)
|
||||
frames.append((ts_ns, frame))
|
||||
prev_offset = np.zeros_like(prev_offset)
|
||||
return frames
|
||||
|
||||
monkeypatch = pytest.MonkeyPatch()
|
||||
try:
|
||||
monkeypatch.setattr(
|
||||
auto_sync_mod, "_load_tlog_imu_energy_stream", _fake_loader
|
||||
)
|
||||
|
||||
# Act
|
||||
window = find_aligned_window(
|
||||
fake_tlog,
|
||||
fake_video,
|
||||
config,
|
||||
target_fc_dialect=FcKind.ARDUPILOT_PLANE,
|
||||
video_frames_factory=_fake_frames,
|
||||
)
|
||||
finally:
|
||||
monkeypatch.undo()
|
||||
|
||||
# Assert: the aligner MUST select FLIGHT 3 (190..250 s), NOT
|
||||
# flight 1 (10..70 s). Whether NCC locks on or the fallback
|
||||
# path fires, the resulting window must lie inside flight 3 —
|
||||
# that's the user-visible contract ("take the last flight").
|
||||
flight3_start_ns, flight3_end_ns = (_ns(190.0), _ns(250.0))
|
||||
assert window.flight_count_detected == 3
|
||||
assert window.selected_flight_index == 2
|
||||
assert flight3_start_ns <= window.tlog_start_ns <= flight3_end_ns
|
||||
# Sanity: did NOT lock onto flight 1 or 2.
|
||||
assert window.tlog_start_ns > _ns(160.0)
|
||||
|
||||
|
||||
def test_align_via_cross_correlation_locks_onto_burst_inside_last_segment() -> None:
|
||||
# Arrange: pre-segmented tlog energy restricted to flight 3
|
||||
# (mimicking what find_aligned_window passes after segmentation),
|
||||
# plus a flow stream whose burst pattern matches a specific
|
||||
# offset inside that segment. This directly exercises the NCC
|
||||
# path with the inputs the post-segmentation aligner sees.
|
||||
last_segment_tlog = _build_motion_burst_stream(
|
||||
start_s=190.0,
|
||||
end_s=250.0,
|
||||
hz=10.0,
|
||||
burst_at_s=210.0,
|
||||
burst_amplitude=1.5,
|
||||
burst_duration_s=5.0,
|
||||
baseline_amplitude=0.05,
|
||||
)
|
||||
flow_samples = _build_motion_burst_stream(
|
||||
start_s=0.0,
|
||||
end_s=30.0,
|
||||
hz=10.0,
|
||||
burst_at_s=10.0,
|
||||
burst_amplitude=2.0,
|
||||
burst_duration_s=5.0,
|
||||
baseline_amplitude=0.05,
|
||||
)
|
||||
config = AutoSyncConfig()
|
||||
|
||||
# Act
|
||||
window = _align_via_cross_correlation(
|
||||
tlog_energy=last_segment_tlog,
|
||||
flow_samples=flow_samples,
|
||||
config=config,
|
||||
target_fc_dialect=FcKind.ARDUPILOT_PLANE,
|
||||
tlog_path=Path("/nonexistent.tlog"),
|
||||
tlog_source_factory=None,
|
||||
flight_count_detected=3,
|
||||
selected_flight_index=2,
|
||||
)
|
||||
|
||||
# Assert: NCC must lock on (high confidence, no fallback). The
|
||||
# tlog_start_ns must be the start of the matched 30 s window —
|
||||
# video burst at video_t=10 s lines up with tlog_t=210 s ⇒
|
||||
# tlog_start_ns ≈ 200 s (210 s − 10 s).
|
||||
assert not window.fallback_used
|
||||
assert window.confidence > 0.6
|
||||
assert window.flight_count_detected == 3
|
||||
assert window.selected_flight_index == 2
|
||||
assert abs(window.tlog_start_ns - _ns(200.0)) <= _ns(0.2)
|
||||
|
||||
|
||||
def test_find_aligned_window_uses_only_segment_for_segmented_tlog_fallback(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
# Arrange: a 3-flight tlog where the video flow is flat (no
|
||||
# structure for NCC to lock onto). NCC must produce confidence
|
||||
# ~ 0; the fallback path must use the LAST segment start, NOT
|
||||
# the head-takeoff detector (which would lock onto flight 1).
|
||||
flights_def = ((10.0, 70.0), (100.0, 160.0), (190.0, 250.0))
|
||||
tlog_energy = _build_multi_flight_stream(
|
||||
flights=flights_def,
|
||||
end_s=260.0,
|
||||
hz=10.0,
|
||||
)
|
||||
flat_flow = tuple((_ns(t / 10.0), 0.5) for t in range(0, 50))
|
||||
config = AutoSyncConfig()
|
||||
|
||||
# Act
|
||||
window = _align_via_cross_correlation(
|
||||
tlog_energy=tuple(
|
||||
(ts, e) for ts, e in tlog_energy
|
||||
if _ns(190.0) <= ts <= _ns(250.0)
|
||||
),
|
||||
flow_samples=flat_flow,
|
||||
config=config,
|
||||
target_fc_dialect=FcKind.ARDUPILOT_PLANE,
|
||||
tlog_path=tmp_path / "x.tlog",
|
||||
tlog_source_factory=None,
|
||||
flight_count_detected=3,
|
||||
selected_flight_index=2,
|
||||
)
|
||||
|
||||
# Assert
|
||||
assert window.fallback_used is True
|
||||
assert window.flight_count_detected == 3
|
||||
assert window.selected_flight_index == 2
|
||||
# The fallback must use flight 3's start, not flight 1's takeoff.
|
||||
assert window.tlog_start_ns >= _ns(190.0)
|
||||
assert window.tlog_start_ns <= _ns(250.0)
|
||||
|
||||
Reference in New Issue
Block a user