mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 21:21:14 +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:
@@ -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"):
|
||||
|
||||
Reference in New Issue
Block a user