fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ReplayEKF: reduce effect of IMU time slip

Browse Source
This commit is contained in:
bresch 2025-03-12 14:52:22 +01:00 committed by Mathieu Bresciani
parent fe5d90301d
commit 30a6a854c1
2 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/modules/replay/ReplayEkf2.cpp
View File

@ -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<uint32_t>(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;

1
src/modules/replay/ReplayEkf2.hpp
View File

@ -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