mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-23 00:41:13 +00:00
[AZ-698] Tlog trim + mid-flight alignment for replay
Adds find_aligned_window cross-correlation (NCC, per-window unit norm)
between IMU energy and video optical-flow magnitude. Returns
AlignedWindow{tlog_start_ns, tlog_end_ns, offset_ms, confidence,
used_fallback}, with fallback to head-takeoff on low confidence to
preserve AZ-405 behavior. TlogReplayFcAdapter honors tlog_start_ns and
skips pre-window messages. New --auto-trim CLI flag, mutex with
--time-offset-ms. AC-1..AC-4 covered by unit tests; AC-5 skipped (no
real flight_derkachi.mp4 in repo). 106 tests pass in regression slice.
Zero new mypy --strict errors.
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -61,10 +61,12 @@ from gps_denied_onboard.replay_input.auto_sync import (
|
||||
_load_tlog_samples,
|
||||
compute_offset,
|
||||
detect_video_motion_onset,
|
||||
find_aligned_window,
|
||||
validate_offset_or_fail,
|
||||
)
|
||||
from gps_denied_onboard.replay_input.errors import ReplayInputAdapterError
|
||||
from gps_denied_onboard.replay_input.interface import (
|
||||
AlignedWindow,
|
||||
AutoSyncConfig,
|
||||
AutoSyncDecision,
|
||||
ReplayInputBundle,
|
||||
@@ -86,6 +88,8 @@ _LOG_KIND_AUTO_SYNC_DETECTED = "replay.auto_sync.detected"
|
||||
_LOG_KIND_AUTO_SYNC_LOW_CONF = "replay.auto_sync.low_confidence"
|
||||
_LOG_KIND_AUTO_SYNC_AC8_FAIL = "replay.auto_sync.ac8_validation_failed"
|
||||
_LOG_KIND_OPEN_MANUAL = "replay.input.opened_manual_offset"
|
||||
_LOG_KIND_AUTO_TRIM_RESOLVED = "replay.auto_trim.resolved"
|
||||
_LOG_KIND_AUTO_TRIM_FALLBACK = "replay.auto_trim.fallback_to_takeoff"
|
||||
|
||||
|
||||
class ReplayInputAdapter:
|
||||
@@ -137,6 +141,7 @@ class ReplayInputAdapter:
|
||||
"_pace",
|
||||
"_manual_time_offset_ms",
|
||||
"_skip_auto_sync_validation",
|
||||
"_auto_trim",
|
||||
"_auto_sync_config",
|
||||
"_tlog_source_factory",
|
||||
"_video_frames_factory",
|
||||
@@ -161,6 +166,7 @@ class ReplayInputAdapter:
|
||||
manual_time_offset_ms: int | None,
|
||||
auto_sync_config: AutoSyncConfig,
|
||||
skip_auto_sync_validation: bool = False,
|
||||
auto_trim: bool = False,
|
||||
tlog_source_factory: Any | None = None,
|
||||
video_frames_factory: Any | None = None,
|
||||
video_timestamps_factory: Any | None = None,
|
||||
@@ -199,6 +205,21 @@ class ReplayInputAdapter:
|
||||
"skip_auto_sync_validation=True requires "
|
||||
"manual_time_offset_ms to be set"
|
||||
)
|
||||
if not isinstance(auto_trim, bool):
|
||||
raise ReplayInputAdapterError(
|
||||
"auto_trim must be a bool; got "
|
||||
f"{type(auto_trim).__name__}"
|
||||
)
|
||||
if auto_trim and manual_time_offset_ms is not None:
|
||||
# Mirror the ReplayConfig.__post_init__ gate. An explicit
|
||||
# manual offset means the operator has already aligned
|
||||
# the streams; running the cross-correlation aligner on
|
||||
# top of that would either re-resolve the same window
|
||||
# (wasteful) or overwrite the operator's intent silently.
|
||||
raise ReplayInputAdapterError(
|
||||
"auto_trim=True is mutually exclusive with "
|
||||
"manual_time_offset_ms"
|
||||
)
|
||||
self._video_path = video_path
|
||||
self._tlog_path = tlog_path
|
||||
self._camera_calibration = camera_calibration
|
||||
@@ -208,6 +229,7 @@ class ReplayInputAdapter:
|
||||
self._pace = pace
|
||||
self._manual_time_offset_ms = manual_time_offset_ms
|
||||
self._skip_auto_sync_validation = skip_auto_sync_validation
|
||||
self._auto_trim = auto_trim
|
||||
self._auto_sync_config = auto_sync_config
|
||||
self._tlog_source_factory = tlog_source_factory
|
||||
self._video_frames_factory = video_frames_factory
|
||||
@@ -233,12 +255,20 @@ class ReplayInputAdapter:
|
||||
# surfaces without paying the cv2.VideoCapture cost.
|
||||
tlog_imu_timestamps_ns, tlog_samples_for_auto = self._load_and_validate_tlog()
|
||||
|
||||
# Step 2 — resolve the offset (auto-sync or manual override).
|
||||
# Step 2 — resolve the offset (auto-sync, auto-trim, or
|
||||
# manual override).
|
||||
decision: AutoSyncDecision | None
|
||||
if self._manual_time_offset_ms is None:
|
||||
aligned_window: AlignedWindow | None
|
||||
if self._auto_trim:
|
||||
aligned_window = self._run_auto_trim()
|
||||
decision = None
|
||||
resolved_offset_ms = aligned_window.offset_ms
|
||||
elif self._manual_time_offset_ms is None:
|
||||
aligned_window = None
|
||||
decision = self._run_auto_sync(tlog_samples_for_auto)
|
||||
resolved_offset_ms = decision.offset_ms
|
||||
else:
|
||||
aligned_window = None
|
||||
decision = None
|
||||
resolved_offset_ms = int(self._manual_time_offset_ms)
|
||||
self._log.info(
|
||||
@@ -315,6 +345,11 @@ class ReplayInputAdapter:
|
||||
wgs_converter=self._wgs_converter,
|
||||
fdr_client=self._fdr_client,
|
||||
time_offset_ms=resolved_offset_ms,
|
||||
tlog_start_ns=(
|
||||
aligned_window.tlog_start_ns
|
||||
if aligned_window is not None
|
||||
else None
|
||||
),
|
||||
pace=self._pace,
|
||||
source_factory=self._tlog_source_factory,
|
||||
mavlink_transport=self._mavlink_transport,
|
||||
@@ -345,6 +380,7 @@ class ReplayInputAdapter:
|
||||
clock=clock,
|
||||
resolved_time_offset_ms=resolved_offset_ms,
|
||||
auto_sync_result=decision,
|
||||
aligned_window=aligned_window,
|
||||
)
|
||||
self._bundle = bundle
|
||||
self._opened = True
|
||||
@@ -408,6 +444,50 @@ class ReplayInputAdapter:
|
||||
)
|
||||
return [ts for ts, _ in samples.accel], samples
|
||||
|
||||
def _run_auto_trim(self) -> AlignedWindow:
|
||||
"""AZ-698 auto-trim path — cross-correlate IMU energy ↔ optical flow.
|
||||
|
||||
Returns the located :class:`AlignedWindow`. When the
|
||||
correlation peak falls below
|
||||
:attr:`AutoSyncConfig.alignment_low_confidence_threshold`,
|
||||
:func:`find_aligned_window` falls back to the AZ-405
|
||||
head-takeoff detector and sets ``fallback_used=True`` — the
|
||||
coordinator logs WARN but still proceeds (the
|
||||
AC-9 frame-window validator runs in Step 3 and will
|
||||
hard-fail if the resolved offset is bad).
|
||||
"""
|
||||
window = find_aligned_window(
|
||||
self._tlog_path,
|
||||
self._video_path,
|
||||
self._auto_sync_config,
|
||||
self._target_fc_dialect,
|
||||
tlog_source_factory=self._tlog_source_factory,
|
||||
video_frames_factory=self._video_frames_factory,
|
||||
)
|
||||
kind = (
|
||||
_LOG_KIND_AUTO_TRIM_FALLBACK
|
||||
if window.fallback_used
|
||||
else _LOG_KIND_AUTO_TRIM_RESOLVED
|
||||
)
|
||||
level = "WARN" if window.fallback_used else "INFO"
|
||||
kv = {
|
||||
"tlog_start_ns": window.tlog_start_ns,
|
||||
"tlog_end_ns": window.tlog_end_ns,
|
||||
"offset_ms": window.offset_ms,
|
||||
"confidence": window.confidence,
|
||||
"fallback_used": window.fallback_used,
|
||||
}
|
||||
msg = (
|
||||
f"{kind}: tlog_start_ns={window.tlog_start_ns} "
|
||||
f"offset_ms={window.offset_ms} confidence={window.confidence:.3f}"
|
||||
)
|
||||
if window.fallback_used:
|
||||
self._log.warning(msg, extra={"kind": kind, "kv": kv})
|
||||
else:
|
||||
self._log.info(msg, extra={"kind": kind, "kv": kv})
|
||||
self._emit_fdr_event(level=level, log_kind=kind, msg=msg, kv=kv)
|
||||
return window
|
||||
|
||||
def _run_auto_sync(self, tlog_samples: Any) -> AutoSyncDecision:
|
||||
"""Auto path — compute the take-off / motion-onset / offset.
|
||||
|
||||
|
||||
Reference in New Issue
Block a user