From 4aeaafef8e9d679a99e569ae2ef36355ce78ccae Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 1 Sep 2026 10:17:38 +0200 Subject: [PATCH] fix(replay): keep the imu stream aligned with the sensor topics sensor_combined carries the same timestamp as ekf2_timestamps and won the main loop's tie, so it was consumed without being published and every barometer, magnetometer and range sample reached ekf2 one update late. Assisted-by: Claude:claude-opus-5[1m] Signed-off-by: bresch --- src/modules/replay/ReplayEkf2.cpp | 38 ++++++++++++++++++++++++++++--- src/modules/replay/ReplayEkf2.hpp | 7 ++++++ 2 files changed, 42 insertions(+), 3 deletions(-) diff --git a/src/modules/replay/ReplayEkf2.cpp b/src/modules/replay/ReplayEkf2.cpp index fc7d137cae9..17158f3dc8f 100644 --- a/src/modules/replay/ReplayEkf2.cpp +++ b/src/modules/replay/ReplayEkf2.cpp @@ -150,11 +150,18 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id) } else if (sub.orb_meta == ORB_ID(ekf2_timestamps)) { _ekf2_timestamps_exists = true; + + if (_sensor_combined_msg_id != msg_id_invalid) { + _subscriptions[_sensor_combined_msg_id]->ignored = true; + } } - // the main loop should only handle publication of the following topics, everything ekf2 - // consumes is published from within the lockstep barrier in publishEkf2Topics() - sub.ignored = sub.orb_meta != ORB_ID(ekf2_timestamps) && sub.orb_meta != ORB_ID(sensor_combined); + // The main loop only drives ekf2_timestamps, everything ekf2 consumes is published from within + // the lockstep barrier in publishEkf2Topics(). sensor_combined carries the same timestamp as + // ekf2_timestamps, so leaving it to the main loop would let it win the tie and consume the + // sample without publishing it, shifting the whole IMU stream one update ahead of the sensors. + sub.ignored = sub.orb_meta != ORB_ID(ekf2_timestamps) + && !(sub.orb_meta == ORB_ID(sensor_combined) && !_ekf2_timestamps_exists); } bool @@ -219,6 +226,8 @@ ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifs findTimestampAndPublish(ekf2_timestamps.timestamp, _vehicle_land_detected_msg_id, replay_file); findTimestampAndPublish(ekf2_timestamps.timestamp, _vehicle_status_msg_id, replay_file); + publishUnmatchedImuSamples(ekf2_timestamps.timestamp, replay_file); + // sensor_combined: publish last because ekf2 is polling on this if (!findTimestampAndPublish(ekf2_timestamps.timestamp, _sensor_combined_msg_id, replay_file)) { if (_sensor_combined_msg_id == msg_id_invalid) { @@ -238,6 +247,29 @@ ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifs return true; } +void +ReplayEkf2::publishUnmatchedImuSamples(uint64_t timestamp, std::ifstream &replay_file) +{ + if (_sensor_combined_msg_id == msg_id_invalid) { + return; + } + + Subscription &sub = *_subscriptions[_sensor_combined_msg_id]; + + // A sample without a matching ekf2_timestamps entry (the log started before ekf2 was being + // logged, or an ekf2_timestamps message was lost) would be overwritten by the one below before + // ekf2 gets to run, so give it a lockstep cycle of its own instead of dropping it. + while (sub.orb_meta && sub.next_timestamp < timestamp) { + if (!sub.published) { + readTopicDataToBuffer(sub, replay_file); + publishTopic(sub, _read_buffer.data()); + px4_lockstep_wait_for_components(); + } + + nextDataMessage(replay_file, sub, _sensor_combined_msg_id); + } +} + bool ReplayEkf2::findTimestampAndPublish(uint64_t timestamp, uint16_t msg_id, std::ifstream &replay_file) { diff --git a/src/modules/replay/ReplayEkf2.hpp b/src/modules/replay/ReplayEkf2.hpp index 9362371086b..6a8caff7405 100644 --- a/src/modules/replay/ReplayEkf2.hpp +++ b/src/modules/replay/ReplayEkf2.hpp @@ -84,6 +84,13 @@ private: */ bool findTimestampAndPublish(uint64_t timestamp, uint16_t msg_id, std::ifstream &replay_file); + /** + * publish the sensor_combined samples preceding @a timestamp, each in its own lockstep cycle + * @param timestamp of the ekf2 update these samples precede, in microseconds + * @param replay_file file currently replayed (file seek position should be considered arbitrary after this call) + */ + void publishUnmatchedImuSamples(uint64_t timestamp, std::ifstream &replay_file); + static constexpr uint16_t msg_id_invalid = 0xffff; uint16_t _airspeed_msg_id = msg_id_invalid;