ekf2-replay: best effort replay when no ekf2_timestamps

Run at sensor_combined speed and publish all other sensors occuring
between 2 sonsor_combined samples.
This allows a crude replay in case ekf2 replay was not enabled during
the flight.

src/modules/replay/Replay.cpp

@@ -730,6 +730,7 @@ Replay::nextDataMessage(std::ifstream &file, Subscription &subscription, int msg
 						subscription.next_read_pos = cur_pos;
 						file.seekg(subscription.timestamp_offset, ios::cur);
 						file.read((char *)&subscription.next_timestamp, sizeof(subscription.next_timestamp));
+						subscription.published = false;
 						done = true;
 
 					} else { //sanity check failed!
@@ -1125,6 +1126,7 @@ Replay::publishTopic(Subscription &sub, void *data)
 
 	if (published) {
 		++sub.publication_counter;
+		sub.published = true;
 	}
 
 	return published;

src/modules/replay/Replay.hpp

@@ -44,7 +44,6 @@
 
 #include <px4_platform_common/module.h>
 #include <uORB/topics/uORBTopics.hpp>
-#include <uORB/topics/ekf2_timestamps.h>
 
 namespace px4
 {
@@ -141,8 +140,10 @@ protected:
 		CompatBase *compat = nullptr;
 
 		// statistics
-		int error_counter = 0;
+		int approx_timestamp_counter = 0;
 		int publication_counter = 0;
+
+		bool published = false;
 	};
 
 	/**

src/modules/replay/ReplayEkf2.cpp

@@ -75,6 +75,20 @@ ReplayEkf2::handleTopicUpdate(Subscription &sub, void *data, std::ifstream &repl
 
 		return true;
 
+	} else if (sub.orb_meta == ORB_ID(sensor_combined) && !_ekf2_timestamps_exists) {
+		// No ekf2_timestamps topic, publish with approximate timestamps
+		sensor_combined_s sensor_combined;
+		memcpy(&sensor_combined, data, sub.orb_meta->o_size);
+
+		if (!publishEkf2Topics(sensor_combined, replay_file)) {
+			return false;
+		}
+
+		// Wait for modules to process the data
+		px4_lockstep_wait_for_components();
+
+		return true;
+
 	} else if (sub.orb_meta == ORB_ID(vehicle_status) || sub.orb_meta == ORB_ID(vehicle_land_detected)
 		   || sub.orb_meta == ORB_ID(vehicle_gps_position)) {
 		return publishTopic(sub, data);
@@ -121,13 +135,34 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id)
 
 	} else if (sub.orb_meta == ORB_ID(vehicle_global_position_groundtruth)) {
 		_vehicle_global_position_groundtruth_msg_id = msg_id;
+
+	} else if (sub.orb_meta == ORB_ID(ekf2_timestamps)) {
+		_ekf2_timestamps_exists = true;
 	}
 
 	// the main loop should only handle publication of the following topics, the sensor topics are
 	// handled separately in publishEkf2Topics()
 	// Note: the GPS is not treated here since not missing data is more important than the accuracy of the timestamp
 	sub.ignored = sub.orb_meta != ORB_ID(ekf2_timestamps) && sub.orb_meta != ORB_ID(vehicle_status)
-		      && sub.orb_meta != ORB_ID(vehicle_land_detected) && sub.orb_meta != ORB_ID(vehicle_gps_position);
+		      && sub.orb_meta != ORB_ID(vehicle_land_detected) && sub.orb_meta != ORB_ID(vehicle_gps_position)
+		      && sub.orb_meta != ORB_ID(sensor_combined);
+}
+
+bool
+ReplayEkf2::publishEkf2Topics(sensor_combined_s &sensor_combined, std::ifstream &replay_file)
+{
+	findTimestampAndPublish(sensor_combined.timestamp / 100, _airspeed_msg_id, replay_file);
+	findTimestampAndPublish(sensor_combined.timestamp / 100, _distance_sensor_msg_id, replay_file);
+	findTimestampAndPublish(sensor_combined.timestamp / 100, _optical_flow_msg_id, replay_file);
+	findTimestampAndPublish(sensor_combined.timestamp / 100, _vehicle_air_data_msg_id, replay_file);
+	findTimestampAndPublish(sensor_combined.timestamp / 100, _vehicle_magnetometer_msg_id, replay_file);
+	findTimestampAndPublish(sensor_combined.timestamp / 100, _vehicle_visual_odometry_msg_id, replay_file);
+	findTimestampAndPublish(sensor_combined.timestamp / 100, _aux_global_position_msg_id, replay_file);
+
+	// sensor_combined: publish last because ekf2 is polling on this
+	publishTopic(*_subscriptions[_sensor_combined_msg_id], &sensor_combined);
+
+	return true;
 }
 
 bool
@@ -182,25 +217,26 @@ ReplayEkf2::findTimestampAndPublish(uint64_t timestamp, uint16_t msg_id, std::if
 
 	Subscription &sub = *_subscriptions[msg_id];
 
-	while (sub.next_timestamp / 100 < timestamp && sub.orb_meta) {
+	bool topic_published = false;
+
+	while (sub.next_timestamp / 100 <= timestamp && sub.orb_meta) {
+		if (!sub.published) {
+			if (sub.next_timestamp / 100 != timestamp) {
+				// Not the exact sample, publish but notify error
+				PX4_DEBUG("No timestamp match found for topic %s (%" PRIu64 ", %" PRIu64 ")\n", sub.orb_meta->o_name, sub.next_timestamp / 100,
+					  timestamp);
+				++sub.approx_timestamp_counter;
+			}
+
+			readTopicDataToBuffer(sub, replay_file);
+			publishTopic(sub, _read_buffer.data());
+			topic_published = true;
+		}
+
 		nextDataMessage(replay_file, sub, msg_id);
 	}
 
-	if (!sub.orb_meta) { // no messages anymore
-		return false;
-	}
-
-	if (sub.next_timestamp / 100 != timestamp) {
-		// this can happen in beginning of the log or on a dropout
-		PX4_DEBUG("No timestamp match found for topic %s (%i, %i)", sub.orb_meta->o_name, (int)sub.next_timestamp / 100,
-			  timestamp);
-		++sub.error_counter;
-		return false;
-	}
-
-	readTopicDataToBuffer(sub, replay_file);
-	publishTopic(sub, _read_buffer.data());
-	return true;
+	return topic_published;
 }
 
 void
@@ -220,14 +256,19 @@ ReplayEkf2::onExitMainLoop()
 		if (msg_id != msg_id_invalid) {
 			Subscription &sub = *_subscriptions[msg_id];
 
-			if (sub.publication_counter > 0 || sub.error_counter > 0) {
-				PX4_INFO("%s: %i, %i", name, sub.publication_counter, sub.error_counter);
+			if (sub.publication_counter > 0 || sub.approx_timestamp_counter > 0) {
+				PX4_INFO("%s: %i (%i)", name, sub.publication_counter, sub.approx_timestamp_counter);
 			}
 		}
 	};
 
 	PX4_INFO("");
-	PX4_INFO("Topic, Num Published, Num Error (no timestamp match found):");
+
+	if (!_ekf2_timestamps_exists) {
+		PX4_INFO("/!\\ Approximate replay (ekf2_timestamps not found)\n");
+	}
+
+	PX4_INFO("Topic, Num Published (Num approximate timestamp found):");
 
 	print_sensor_statistics(_airspeed_msg_id, "airspeed");
 	print_sensor_statistics(_airspeed_validated_msg_id, "airspeed_validated");

src/modules/replay/ReplayEkf2.hpp

@@ -35,6 +35,9 @@
 
 #include "Replay.hpp"
 
+#include <uORB/topics/ekf2_timestamps.h>
+#include <uORB/topics/sensor_combined.h>
+
 namespace px4
 {
 
@@ -70,6 +73,8 @@ private:
 
 	bool publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifstream &replay_file);
 
+	bool publishEkf2Topics(sensor_combined_s &sensors_combined, std::ifstream &replay_file);
+
 	/**
 	 * find the next message for a subscription that matches a given timestamp and publish it
 	 * @param timestamp in 0.1 ms
@@ -93,6 +98,8 @@ private:
 	uint16_t _vehicle_local_position_groundtruth_msg_id = msg_id_invalid;
 	uint16_t _vehicle_global_position_groundtruth_msg_id = msg_id_invalid;
 	uint16_t _vehicle_attitude_groundtruth_msg_id = msg_id_invalid;
+
+	bool _ekf2_timestamps_exists{false};
 };
 
 } //namespace px4