UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-09-23 10:18:30 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix(replay): keep the imu stream aligned with the sensor topics

Browse Source 
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 <brescianimathieu@gmail.com>
This commit is contained in:
bresch
2026-09-01 10:17:38 +02:00
committed by Mathieu Bresciani
parent 15b0a24149
commit 4aeaafef8e
2 changed files with 42 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

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