[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:
Oleksandr Bezdieniezhnykh
2026-05-20 16:29:59 +03:00
parent 64d961f60c
commit 87fe98858f
13 changed files with 1360 additions and 7 deletions
@@ -202,6 +202,7 @@ class TlogReplayFcAdapter:
"_clock",
"_wgs_converter",
"_time_offset_ns",
"_tlog_start_ns",
"_pace",
"_fdr_client",
"_log",
@@ -218,6 +219,7 @@ class TlogReplayFcAdapter:
"_latest_flight_state",
"_last_received_at_ns",
"_dispatched_count",
"_skipped_pre_window_count",
"_mavlink_transport",
"_outbound_mav",
"_sequence_number",
@@ -234,6 +236,7 @@ class TlogReplayFcAdapter:
wgs_converter: "WgsConverter",
fdr_client: "FdrClient",
time_offset_ms: int = 0,
tlog_start_ns: int | None = None,
pace: ReplayPace = ReplayPace.ASAP,
source_factory: Any | None = None,
mavlink_transport: "MavlinkTransport | None" = None,
@@ -254,12 +257,23 @@ class TlogReplayFcAdapter:
f"target_fc_dialect must be ARDUPILOT_PLANE or INAV; "
f"got {target_fc_dialect!r}"
)
if tlog_start_ns is not None and not isinstance(tlog_start_ns, int):
raise FcAdapterConfigError(
"tlog_start_ns must be int or None; "
f"got {type(tlog_start_ns).__name__}"
)
self._tlog_path = tlog_path
self._target_fc_dialect = target_fc_dialect
self._clock = clock
self._wgs_converter = wgs_converter
self._fdr_client = fdr_client
self._time_offset_ns: int = int(time_offset_ms) * 1_000_000
# AZ-698: pre-window seek bound. Messages with raw
# ``_timestamp`` (NOT offset-shifted) below this value are
# silently skipped by ``feed_one_message`` so the runtime
# loop only sees the mid-flight slice the aligner located.
# ``None`` preserves the historical "stream from t=0" behaviour.
self._tlog_start_ns: int | None = tlog_start_ns
self._pace = pace
self._log = get_logger("c8_fc_adapter.tlog_replay")
self._bus = SubscriptionBus()
@@ -275,6 +289,7 @@ class TlogReplayFcAdapter:
self._latest_flight_state: FlightStateSignal | None = None
self._last_received_at_ns: int = -1
self._dispatched_count: int = 0
self._skipped_pre_window_count: int = 0
# AZ-558: outbound MAVLink seam. When ``mavlink_transport`` is
# injected (replay branch wires NoopMavlinkTransport in), every
# ``emit_external_position`` / ``emit_status_text`` call routes
@@ -634,9 +649,24 @@ class TlogReplayFcAdapter:
Test-friendly entrypoint mirroring AZ-391's
:meth:`PymavlinkInboundDecoder.feed_one_message`. Production
replay uses :meth:`_run_decode_loop`.
AZ-698: when ``tlog_start_ns`` was set at construction, every
message with a raw ``_timestamp`` below that bound is silently
skipped before its type-specific handler runs — the runtime
loop only sees the trimmed window.
"""
if msg is None:
return False
if self._tlog_start_ns is not None:
try:
raw_ts_ns = _msg_timestamp_ns(msg)
except FcOpenError:
# Malformed timestamp — let the handler raise so the
# error path matches the no-trim case verbatim.
raw_ts_ns = None
if raw_ts_ns is not None and raw_ts_ns < self._tlog_start_ns:
self._skipped_pre_window_count += 1
return False
try:
msg_type = self._safe_msg_type(msg)
if msg_type in ("RAW_IMU", "SCALED_IMU2"):