fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 12:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ReplayEkf: correctly add all subscriptions

Browse Source 
Logged data and subscription messages can be mixed, so don't stop adding
subscriptions after finding the first logged message
This commit is contained in:
bresch
2025-03-17 10:01:48 +01:00
committed by Mathieu Bresciani
parent 30a6a854c1
commit fa0ba69083
2 changed files with 7 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/replay/Replay.cpp
+6 -4
View File
@@ -902,6 +902,7 @@ Replay::run()
PX4_INFO("Replay in progress..."); PX4_INFO("Replay in progress...");
// Find and add all subscriptions
ulog_message_header_s message_header; ulog_message_header_s message_header;
replay_file.seekg(_data_section_start); replay_file.seekg(_data_section_start);
@@ -910,22 +911,23 @@ Replay::run()
replay_file.read((char *)&message_header, ULOG_MSG_HEADER_LEN); replay_file.read((char *)&message_header, ULOG_MSG_HEADER_LEN);
if (!replay_file) { if (!replay_file) {
// end of file
break; break;
} }
if (message_header.msg_type == (int)ULogMessageType::ADD_LOGGED_MSG) { if (message_header.msg_type == (int)ULogMessageType::ADD_LOGGED_MSG) {
readAndAddSubscription(replay_file, message_header.msg_size); 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 { } else {
// Not important for now, skip // Not important for now, skip
replay_file.seekg(message_header.msg_size, ios::cur); 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(); const uint64_t timestamp_offset = getTimestampOffset();
uint32_t nr_published_messages = 0; uint32_t nr_published_messages = 0;
streampos last_additional_message_pos = _data_section_start; streampos last_additional_message_pos = _data_section_start;
src/modules/replay/Replay.hpp
+1 -2
View File
@@ -201,8 +201,7 @@ protected:
/** /**
* Find next data message for this subscription, starting with the stored file offset. * 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. * 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, * When reaching EOF, the subscription is set to invalid.
* the subscription is set to invalid.
* File seek position is arbitrary after this call. * File seek position is arbitrary after this call.
* @return false on file error * @return false on file error
*/ */