fix(replay): synthesize IMU samples missing from the log instead of shifting the stream

Signed-off-by: Marco Hauswirth <marco.hauswirth@auterion.com>

src/modules/replay/ReplayEkf2.cpp

@@ -245,30 +245,49 @@ ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifs
 	findTimestampAndPublish(ekf2_timestamps.timestamp, _sensor_selection_msg_id, replay_file);
 	findTimestampAndPublish(ekf2_timestamps.timestamp, _launch_detection_status_msg_id, replay_file);
 
-	publishUnmatchedImuSamples(ekf2_timestamps.timestamp, replay_file);
-
-	// sensor_combined: publish last because ekf2 is polling on this. If the sample is missing from the log, ekf2
-	// skips this cycle; publishing a later sample instead would shift the IMU stream against the other sensors.
-	return findTimestampAndPublish(ekf2_timestamps.timestamp, _sensor_combined_msg_id, replay_file);
+	// sensor_combined: publish last because ekf2 is polling on this
+	return publishMatchingSensorCombined(ekf2_timestamps.timestamp, replay_file);
 }
 
-void
-ReplayEkf2::publishUnmatchedImuSamples(uint64_t timestamp, std::ifstream &replay_file)
+bool
+ReplayEkf2::publishMatchingSensorCombined(uint64_t timestamp, std::ifstream &replay_file)
 {
 	if (_sensor_combined_msg_id == msg_id_invalid) {
-		return;
+		return false;
 	}
 
 	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) {
 		readTopicDataToBuffer(sub, replay_file);
 		publishSensorCombined(sub, _read_buffer.data());
+		memcpy(&_last_sensor_combined, _read_buffer.data(), sizeof(_last_sensor_combined));
+		++_sensor_combined_unmatched;
 		nextDataMessage(replay_file, sub, _sensor_combined_msg_id);
 	}
+
+	if (sub.orb_meta && sub.next_timestamp == timestamp) {
+		readTopicDataToBuffer(sub, replay_file);
+		publishSensorCombined(sub, _read_buffer.data());
+		memcpy(&_last_sensor_combined, _read_buffer.data(), sizeof(_last_sensor_combined));
+		nextDataMessage(replay_file, sub, _sensor_combined_msg_id);
+		return true;
+	}
+
+	++_sensor_combined_missing;
+
+	if (_last_sensor_combined.timestamp == 0 || timestamp <= _last_sensor_combined.timestamp) {
+		return false;
+	}
+
+	sensor_combined_s synthesized = _last_sensor_combined;
+	const uint32_t dt = static_cast<uint32_t>(timestamp - _last_sensor_combined.timestamp);
+	synthesized.timestamp = timestamp;
+	synthesized.gyro_integral_dt = dt;
+	synthesized.accelerometer_integral_dt = dt;
+	_last_sensor_combined = synthesized;
+	publishSensorCombined(sub, &synthesized);
+	return true;
 }
 
 bool
@@ -293,14 +312,7 @@ ReplayEkf2::findTimestampAndPublish(uint64_t timestamp, uint16_t msg_id, std::if
 		}
 
 		readTopicDataToBuffer(sub, replay_file);
-
-		if (msg_id == _sensor_combined_msg_id) {
-			publishSensorCombined(sub, _read_buffer.data());
-
-		} else {
-			publishTopic(sub, _read_buffer.data());
-		}
-
+		publishTopic(sub, _read_buffer.data());
 		topic_published = true;
 
 		nextDataMessage(replay_file, sub, msg_id);
@@ -449,6 +461,12 @@ ReplayEkf2::onExitMainLoop()
 	print_multi_sensor_statistics(_distance_sensor_msg_ids, "distance_sensor");
 	print_multi_sensor_statistics(_optical_flow_msg_ids, "vehicle_optical_flow");
 	print_sensor_statistics(_sensor_combined_msg_id, "sensor_combined");
+
+	if (_sensor_combined_unmatched > 0 || _sensor_combined_missing > 0) {
+		PX4_INFO("sensor_combined: %u samples without ekf2_timestamps entry (published), %u missing in the log (synthesized)",
+			 _sensor_combined_unmatched, _sensor_combined_missing);
+	}
+
 	print_sensor_statistics(_vehicle_air_data_msg_id, "vehicle_air_data");
 	print_sensor_statistics(_vehicle_magnetometer_msg_id, "vehicle_magnetometer");
 	print_sensor_statistics(_vehicle_visual_odometry_msg_id, "vehicle_visual_odometry");

src/modules/replay/ReplayEkf2.hpp

@@ -122,11 +122,12 @@ private:
 	bool findTimestampAndPublish(uint64_t timestamp, const std::vector<uint16_t> &msg_ids, 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)
+	 * publish the sensor_combined sample of the given ekf2 cycle. Logged samples without an ekf2_timestamps entry are
+	 * published in a cycle of their own (ekf2 processed them in flight, only their ekf2_timestamps message is missing),
+	 * a sample missing from the log is synthesized from the previous one with the integration time covering the gap.
+	 * @return true if a sample was published
 	 */
-	void publishUnmatchedImuSamples(uint64_t timestamp, std::ifstream &replay_file);
+	bool publishMatchingSensorCombined(uint64_t timestamp, std::ifstream &replay_file);
 
 	static constexpr uint16_t msg_id_invalid = 0xffff;
 
@@ -161,6 +162,10 @@ private:
 	bool _ekf2_sync_enabled{true};
 	unsigned _ekf2_update_timeouts{0};
 	unsigned _ekf2_consecutive_timeouts{0};
+
+	unsigned _sensor_combined_unmatched{0};
+	unsigned _sensor_combined_missing{0};
+	sensor_combined_s _last_sensor_combined{};
 };
 
 } //namespace px4