From 30a6a854c165b33feb43e9be71a484d724b89da9 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 12 Mar 2025 14:52:22 +0100 Subject: [PATCH] ReplayEKF: reduce effect of IMU time slip --- src/modules/replay/ReplayEkf2.cpp | 9 +++++++++ src/modules/replay/ReplayEkf2.hpp | 1 + 2 files changed, 10 insertions(+) diff --git a/src/modules/replay/ReplayEkf2.cpp b/src/modules/replay/ReplayEkf2.cpp index 7a0a077a16..c9785b5ef0 100644 --- a/src/modules/replay/ReplayEkf2.cpp +++ b/src/modules/replay/ReplayEkf2.cpp @@ -160,6 +160,15 @@ ReplayEkf2::publishEkf2Topics(sensor_combined_s &sensor_combined, std::ifstream findTimestampAndPublish(sensor_combined.timestamp, _aux_global_position_msg_id, replay_file); // sensor_combined: publish last because ekf2 is polling on this + if (_last_sensor_combined_timestamp > 0) { + // Some samples might be missing so compensate for this by holding the last value + const uint32_t true_dt = static_cast(sensor_combined.timestamp - _last_sensor_combined_timestamp); + sensor_combined.gyro_integral_dt = true_dt; + sensor_combined.accelerometer_integral_dt = true_dt; + } + + _last_sensor_combined_timestamp = sensor_combined.timestamp; + publishTopic(*_subscriptions[_sensor_combined_msg_id], &sensor_combined); return true; diff --git a/src/modules/replay/ReplayEkf2.hpp b/src/modules/replay/ReplayEkf2.hpp index b8890f3e47..bbf478c73e 100644 --- a/src/modules/replay/ReplayEkf2.hpp +++ b/src/modules/replay/ReplayEkf2.hpp @@ -100,6 +100,7 @@ private: uint16_t _vehicle_attitude_groundtruth_msg_id = msg_id_invalid; bool _ekf2_timestamps_exists{false}; + uint64_t _last_sensor_combined_timestamp{0}; }; } //namespace px4