mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 14:41:15 +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:
@@ -198,6 +198,9 @@ _REPLAY_AUTO_SYNC_TYPES: Final[dict[str, type]] = {
|
||||
"alignment_resample_hz": float,
|
||||
"alignment_video_scan_seconds": float,
|
||||
"alignment_low_confidence_threshold": float,
|
||||
"alignment_segment_motion_threshold_g": float,
|
||||
"alignment_segment_min_flight_duration_seconds": float,
|
||||
"alignment_segment_max_internal_gap_seconds": float,
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -323,6 +323,9 @@ class ReplayAutoSyncConfig:
|
||||
alignment_resample_hz: float = 10.0
|
||||
alignment_video_scan_seconds: float = 30.0
|
||||
alignment_low_confidence_threshold: float = 0.60
|
||||
alignment_segment_motion_threshold_g: float = 0.10
|
||||
alignment_segment_min_flight_duration_seconds: float = 30.0
|
||||
alignment_segment_max_internal_gap_seconds: float = 30.0
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
|
||||
@@ -727,15 +727,53 @@ def find_aligned_window(
|
||||
if not video_path.is_file():
|
||||
raise ReplayInputAdapterError(f"video file not found: {video_path}")
|
||||
|
||||
tlog_energy = _load_tlog_imu_energy_stream(
|
||||
tlog_energy_full = _load_tlog_imu_energy_stream(
|
||||
tlog_path,
|
||||
max_messages=config.prescan_max_messages * 10,
|
||||
source_factory=tlog_source_factory,
|
||||
)
|
||||
if len(tlog_energy_full) < 2:
|
||||
raise ReplayInputAdapterError(
|
||||
f"tlog yielded {len(tlog_energy_full)} IMU sample(s); "
|
||||
"need ≥ 2 for cross-correlation alignment"
|
||||
)
|
||||
|
||||
# Multi-flight handling: a tlog may cover several takeoffs at the
|
||||
# same field (engine starts between sorties); the uploaded video
|
||||
# only covers ONE of them, conventionally the LAST. Segment the
|
||||
# tlog first and restrict NCC to the last detected flight so the
|
||||
# peak cannot lock onto an earlier sortie.
|
||||
flight_segments = _segment_flights_from_imu_energy(
|
||||
tlog_energy_full,
|
||||
motion_threshold=config.alignment_segment_motion_threshold_g,
|
||||
min_flight_duration_ns=int(
|
||||
config.alignment_segment_min_flight_duration_seconds * 1_000_000_000
|
||||
),
|
||||
max_internal_gap_ns=int(
|
||||
config.alignment_segment_max_internal_gap_seconds * 1_000_000_000
|
||||
),
|
||||
)
|
||||
if flight_segments:
|
||||
seg_start_ns, seg_end_ns = flight_segments[-1]
|
||||
tlog_energy = tuple(
|
||||
(ts, e) for ts, e in tlog_energy_full
|
||||
if seg_start_ns <= ts <= seg_end_ns
|
||||
)
|
||||
flight_count_detected = len(flight_segments)
|
||||
selected_flight_index = len(flight_segments) - 1
|
||||
else:
|
||||
# No clear flight pattern detected (degenerate tlog: very
|
||||
# short, all-quiet, or thresholds badly tuned). Fall through
|
||||
# to whole-tlog NCC so we keep the AZ-405-equivalent
|
||||
# behavior; surface this via flight_count_detected=0.
|
||||
tlog_energy = tlog_energy_full
|
||||
flight_count_detected = 0
|
||||
selected_flight_index = -1
|
||||
|
||||
if len(tlog_energy) < 2:
|
||||
raise ReplayInputAdapterError(
|
||||
f"tlog yielded {len(tlog_energy)} IMU sample(s); "
|
||||
"need ≥ 2 for cross-correlation alignment"
|
||||
f"selected flight segment yielded {len(tlog_energy)} IMU "
|
||||
"sample(s); need ≥ 2 for cross-correlation alignment"
|
||||
)
|
||||
|
||||
if video_frames_factory is None:
|
||||
@@ -765,6 +803,8 @@ def find_aligned_window(
|
||||
target_fc_dialect=target_fc_dialect,
|
||||
tlog_path=tlog_path,
|
||||
tlog_source_factory=tlog_source_factory,
|
||||
flight_count_detected=flight_count_detected,
|
||||
selected_flight_index=selected_flight_index,
|
||||
)
|
||||
|
||||
|
||||
@@ -776,6 +816,8 @@ def _align_via_cross_correlation(
|
||||
target_fc_dialect: FcKind,
|
||||
tlog_path: Path,
|
||||
tlog_source_factory: Callable[[str], Any] | None,
|
||||
flight_count_detected: int = 0,
|
||||
selected_flight_index: int = -1,
|
||||
) -> AlignedWindow:
|
||||
"""Pure compute kernel: turn pre-loaded streams into an :class:`AlignedWindow`.
|
||||
|
||||
@@ -844,6 +886,8 @@ def _align_via_cross_correlation(
|
||||
video_origin_ns=video_origin_ns,
|
||||
video_flow_duration_ns=video_duration_ns,
|
||||
confidence=confidence,
|
||||
flight_count_detected=flight_count_detected,
|
||||
selected_flight_index=selected_flight_index,
|
||||
)
|
||||
|
||||
# Absolute tlog timeline value where video t=0 aligns. The
|
||||
@@ -862,6 +906,8 @@ def _align_via_cross_correlation(
|
||||
offset_ms=offset_ms,
|
||||
confidence=confidence,
|
||||
fallback_used=False,
|
||||
flight_count_detected=flight_count_detected,
|
||||
selected_flight_index=selected_flight_index,
|
||||
)
|
||||
|
||||
|
||||
@@ -883,28 +929,49 @@ def _fallback_to_head_takeoff(
|
||||
video_origin_ns: int,
|
||||
video_flow_duration_ns: int,
|
||||
confidence: float,
|
||||
flight_count_detected: int = 0,
|
||||
selected_flight_index: int = -1,
|
||||
) -> AlignedWindow:
|
||||
"""Low-confidence path: use AZ-405 head-takeoff detector.
|
||||
"""Low-confidence fallback path.
|
||||
|
||||
Returns an :class:`AlignedWindow` whose ``offset_ms`` and
|
||||
``tlog_start_ns`` come from the takeoff onset; ``fallback_used``
|
||||
is ``True`` so callers + FDR audit can record the divergence.
|
||||
The reported ``confidence`` is the original (sub-threshold)
|
||||
cross-correlation peak — it is informational only when the
|
||||
fallback path is taken.
|
||||
Two modes:
|
||||
|
||||
* **Segmented tlog** (``flight_count_detected > 0``): the
|
||||
pre-NCC segmenter already chose the LAST flight. We use
|
||||
``tlog_energy[0][0]`` — the start of that segment — as the
|
||||
``tlog_start_ns`` rather than re-running the AZ-405
|
||||
head-takeoff detector (which would lock onto FLIGHT 1's
|
||||
takeoff on a multi-flight tlog and silently throw away the
|
||||
segmenter's correct answer). This is the AZ-698-after-user-
|
||||
feedback contract: "if 1 flight take it, if multiple take
|
||||
the last" applies to the fallback path too.
|
||||
|
||||
* **Un-segmented tlog** (``flight_count_detected == 0``): no
|
||||
flight pattern fired in the segmenter (degenerate / very
|
||||
short tlog). Fall back to the AZ-405 head-takeoff detector
|
||||
as before — this preserves the single-flight behavior that
|
||||
existed before AZ-698's segmentation stage.
|
||||
|
||||
``fallback_used`` is ``True`` in either case so callers + FDR
|
||||
audit can record the divergence. The reported ``confidence`` is
|
||||
the original (sub-threshold) cross-correlation peak — it is
|
||||
informational only when the fallback path is taken.
|
||||
"""
|
||||
takeoff = detect_tlog_takeoff(
|
||||
tlog_path,
|
||||
target_fc_dialect,
|
||||
config,
|
||||
source_factory=tlog_source_factory,
|
||||
)
|
||||
if takeoff.confidence > 0.0:
|
||||
tlog_start_ns = takeoff.onset_ns
|
||||
elif tlog_energy:
|
||||
if flight_count_detected > 0 and tlog_energy:
|
||||
tlog_start_ns = tlog_energy[0][0]
|
||||
else:
|
||||
tlog_start_ns = 0
|
||||
takeoff = detect_tlog_takeoff(
|
||||
tlog_path,
|
||||
target_fc_dialect,
|
||||
config,
|
||||
source_factory=tlog_source_factory,
|
||||
)
|
||||
if takeoff.confidence > 0.0:
|
||||
tlog_start_ns = takeoff.onset_ns
|
||||
elif tlog_energy:
|
||||
tlog_start_ns = tlog_energy[0][0]
|
||||
else:
|
||||
tlog_start_ns = 0
|
||||
tlog_end_ns = tlog_start_ns + video_flow_duration_ns
|
||||
offset_ms = (tlog_start_ns - video_origin_ns) // 1_000_000
|
||||
return AlignedWindow(
|
||||
@@ -913,6 +980,8 @@ def _fallback_to_head_takeoff(
|
||||
offset_ms=offset_ms,
|
||||
confidence=confidence,
|
||||
fallback_used=True,
|
||||
flight_count_detected=flight_count_detected,
|
||||
selected_flight_index=selected_flight_index,
|
||||
)
|
||||
|
||||
|
||||
@@ -969,6 +1038,60 @@ def _zero_mean_normalise(
|
||||
return result
|
||||
|
||||
|
||||
def _segment_flights_from_imu_energy(
|
||||
samples: tuple[tuple[int, float], ...],
|
||||
*,
|
||||
motion_threshold: float,
|
||||
min_flight_duration_ns: int,
|
||||
max_internal_gap_ns: int,
|
||||
) -> list[tuple[int, int]]:
|
||||
"""Partition an IMU energy stream into distinct flight segments.
|
||||
|
||||
A flight is a contiguous span where energy stayed ``>=`` the
|
||||
threshold, with no sub-threshold run longer than
|
||||
``max_internal_gap_ns`` (cruise lulls don't split a flight).
|
||||
Spans shorter than ``min_flight_duration_ns`` are discarded as
|
||||
ground-startup noise. Returns ``(start_ns, end_ns)`` per flight,
|
||||
in chronological order.
|
||||
|
||||
AZ-698 / AZ-697 user constraint: a single tlog often spans
|
||||
multiple takeoffs at the same field, but the uploaded video only
|
||||
covers the **last** one. The aligner uses this segmenter to find
|
||||
every flight, then restricts NCC search to the last segment so
|
||||
the trim is unambiguous. ``_find_sustained_event`` (AZ-405)
|
||||
returns only the FIRST qualifying window by design; partitioning
|
||||
all flights needs this fresh one-pass walk.
|
||||
"""
|
||||
if not samples:
|
||||
return []
|
||||
segments: list[tuple[int, int]] = []
|
||||
in_flight = False
|
||||
flight_start_ns = 0
|
||||
last_above_ns = 0
|
||||
last_below_ns: int | None = None
|
||||
for ts, energy in samples:
|
||||
if energy >= motion_threshold:
|
||||
if not in_flight:
|
||||
in_flight = True
|
||||
flight_start_ns = ts
|
||||
last_above_ns = ts
|
||||
last_below_ns = None
|
||||
else:
|
||||
if in_flight:
|
||||
if last_below_ns is None:
|
||||
last_below_ns = ts
|
||||
if (ts - last_below_ns) >= max_internal_gap_ns:
|
||||
if (
|
||||
last_above_ns - flight_start_ns
|
||||
) >= min_flight_duration_ns:
|
||||
segments.append((flight_start_ns, last_above_ns))
|
||||
in_flight = False
|
||||
last_below_ns = None
|
||||
if in_flight and (last_above_ns - flight_start_ns) >= min_flight_duration_ns:
|
||||
segments.append((flight_start_ns, last_above_ns))
|
||||
return segments
|
||||
|
||||
|
||||
def _load_tlog_imu_energy_stream(
|
||||
tlog_path: Path,
|
||||
*,
|
||||
|
||||
@@ -91,6 +91,21 @@ class AutoSyncConfig:
|
||||
confidence below which :func:`find_aligned_window` falls
|
||||
back to the head-takeoff detector (AZ-405 path).
|
||||
Default 0.60.
|
||||
alignment_segment_motion_threshold_g: Minimum IMU energy
|
||||
(``|a| - 1g`` in g-units) for a sample to count as
|
||||
"in-flight" during segmentation. A 3-flight tlog has 3
|
||||
spans where this threshold is exceeded with gaps below
|
||||
``alignment_segment_max_internal_gap_seconds``. Default
|
||||
0.10 — captures cruise oscillation while ignoring
|
||||
stationary sensor noise (~ 0.02 g).
|
||||
alignment_segment_min_flight_duration_seconds: Minimum span
|
||||
length (in seconds) for a candidate segment to be
|
||||
classified as a flight. Discards short ground-startup
|
||||
blips. Default 30.
|
||||
alignment_segment_max_internal_gap_seconds: Sub-threshold
|
||||
spans shorter than this stay inside a flight (cruise
|
||||
lulls don't split a flight); spans equal-or-longer end
|
||||
the current flight. Default 30.
|
||||
"""
|
||||
|
||||
takeoff_accel_threshold_g: float = 0.5
|
||||
@@ -105,6 +120,9 @@ class AutoSyncConfig:
|
||||
alignment_resample_hz: float = 10.0
|
||||
alignment_video_scan_seconds: float = 30.0
|
||||
alignment_low_confidence_threshold: float = 0.60
|
||||
alignment_segment_motion_threshold_g: float = 0.10
|
||||
alignment_segment_min_flight_duration_seconds: float = 30.0
|
||||
alignment_segment_max_internal_gap_seconds: float = 30.0
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
@@ -163,6 +181,18 @@ class AlignedWindow:
|
||||
fallback_used: ``True`` when cross-correlation confidence
|
||||
dropped below the threshold and the result was built
|
||||
from the head-takeoff detector instead.
|
||||
flight_count_detected: Number of distinct flight segments the
|
||||
pre-NCC segmenter found in the tlog. ``1`` for a clean
|
||||
single-flight tlog. ``> 1`` means the tlog covers
|
||||
multiple flights — the aligner always selects the
|
||||
**last** one (per AZ-698 spec line 23: "the last chunk
|
||||
in tlog is relevant").
|
||||
selected_flight_index: Zero-based index of the segment the
|
||||
aligner restricted NCC to. Always
|
||||
``flight_count_detected - 1`` when ``flight_count_detected
|
||||
> 0``; ``-1`` when segmentation returned no candidate and
|
||||
the aligner fell through to whole-tlog NCC (single-flight
|
||||
edge case where the segmenter's thresholds didn't fire).
|
||||
"""
|
||||
|
||||
tlog_start_ns: int
|
||||
@@ -170,6 +200,8 @@ class AlignedWindow:
|
||||
offset_ms: int
|
||||
confidence: float
|
||||
fallback_used: bool
|
||||
flight_count_detected: int = 0
|
||||
selected_flight_index: int = -1
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
|
||||
@@ -476,10 +476,14 @@ class ReplayInputAdapter:
|
||||
"offset_ms": window.offset_ms,
|
||||
"confidence": window.confidence,
|
||||
"fallback_used": window.fallback_used,
|
||||
"flight_count_detected": window.flight_count_detected,
|
||||
"selected_flight_index": window.selected_flight_index,
|
||||
}
|
||||
msg = (
|
||||
f"{kind}: tlog_start_ns={window.tlog_start_ns} "
|
||||
f"offset_ms={window.offset_ms} confidence={window.confidence:.3f}"
|
||||
f"offset_ms={window.offset_ms} confidence={window.confidence:.3f} "
|
||||
f"flights_detected={window.flight_count_detected} "
|
||||
f"selected_flight={window.selected_flight_index}"
|
||||
)
|
||||
if window.fallback_used:
|
||||
self._log.warning(msg, extra={"kind": kind, "kv": kv})
|
||||
|
||||
@@ -271,6 +271,15 @@ def _build_auto_sync_config(config: Config) -> AutoSyncConfig:
|
||||
alignment_resample_hz=block.alignment_resample_hz,
|
||||
alignment_video_scan_seconds=block.alignment_video_scan_seconds,
|
||||
alignment_low_confidence_threshold=block.alignment_low_confidence_threshold,
|
||||
alignment_segment_motion_threshold_g=(
|
||||
block.alignment_segment_motion_threshold_g
|
||||
),
|
||||
alignment_segment_min_flight_duration_seconds=(
|
||||
block.alignment_segment_min_flight_duration_seconds
|
||||
),
|
||||
alignment_segment_max_internal_gap_seconds=(
|
||||
block.alignment_segment_max_internal_gap_seconds
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user