From fa0ba690839bc225e1feda72ac98e4276192e77d Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 14 Mar 2025 11:35:52 +0100 Subject: [PATCH] ReplayEkf: correctly add all subscriptions Logged data and subscription messages can be mixed, so don't stop adding subscriptions after finding the first logged message --- src/modules/replay/Replay.cpp | 10 ++++++---- src/modules/replay/Replay.hpp | 3 +-- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/modules/replay/Replay.cpp b/src/modules/replay/Replay.cpp index 1f5c835b21..e05d50a3e0 100644 --- a/src/modules/replay/Replay.cpp +++ b/src/modules/replay/Replay.cpp @@ -902,6 +902,7 @@ Replay::run() PX4_INFO("Replay in progress..."); + // Find and add all subscriptions ulog_message_header_s message_header; replay_file.seekg(_data_section_start); @@ -910,22 +911,23 @@ Replay::run() replay_file.read((char *)&message_header, ULOG_MSG_HEADER_LEN); if (!replay_file) { + // end of file break; } if (message_header.msg_type == (int)ULogMessageType::ADD_LOGGED_MSG) { readAndAddSubscription(replay_file, message_header.msg_size); - } else if (message_header.msg_type == (int)ULogMessageType::DATA) { - // End of Definition & Data Section Message Header section - break; - } else { // Not important for now, skip replay_file.seekg(message_header.msg_size, ios::cur); } } + // Rewind back to the begining of the data section + replay_file.seekg(_data_section_start); + replay_file.clear(); + const uint64_t timestamp_offset = getTimestampOffset(); uint32_t nr_published_messages = 0; streampos last_additional_message_pos = _data_section_start; diff --git a/src/modules/replay/Replay.hpp b/src/modules/replay/Replay.hpp index 6d85b44040..865bb88561 100644 --- a/src/modules/replay/Replay.hpp +++ b/src/modules/replay/Replay.hpp @@ -201,8 +201,7 @@ protected: /** * Find next data message for this subscription, starting with the stored file offset. * Skip the first message, and if found, read the timestamp and store the new file offset. - * This also takes care of new subscriptions and parameter updates. When reaching EOF, - * the subscription is set to invalid. + * When reaching EOF, the subscription is set to invalid. * File seek position is arbitrary after this call. * @return false on file error */