replay: add backward compatibility for updated sensor_combined topic

src/modules/replay/replay.hpp

@@ -95,6 +95,36 @@ public:
 
 protected:
 
+	/**
+	 * @class Compatibility base class to convert topics to an updated format
+	 */
+	class CompatBase
+	{
+	public:
+		virtual ~CompatBase() = default;
+
+		/**
+		 * apply compatibility to a topic
+		 * @param data input topic (can be modified in place)
+		 * @return new topic data
+		 */
+		virtual void *apply(void *data) = 0;
+	};
+
+	class CompatSensorCombinedDtType : public CompatBase
+	{
+	public:
+		CompatSensorCombinedDtType(int gyro_integral_dt_offset_log, int gyro_integral_dt_offset_intern,
+					   int accelerometer_integral_dt_offset_log, int accelerometer_integral_dt_offset_intern);
+
+		void *apply(void *data) override;
+	private:
+		int _gyro_integral_dt_offset_log;
+		int _gyro_integral_dt_offset_intern;
+		int _accelerometer_integral_dt_offset_log;
+		int _accelerometer_integral_dt_offset_intern;
+	};
+
 	struct Subscription {
 
 		const orb_metadata *orb_meta = nullptr; ///< if nullptr, this subscription is invalid
@@ -107,6 +137,8 @@ protected:
 		std::streampos next_read_pos;
 		uint64_t next_timestamp; ///< timestamp of the file
 
+		CompatBase *compat = nullptr;
+
 		// statistics
 		int error_counter = 0;
 		int publication_counter = 0;

src/modules/replay/replay_main.cpp

@@ -86,6 +86,31 @@ class Replay;
 
 char *Replay::_replay_file = nullptr;
 
+Replay::CompatSensorCombinedDtType::CompatSensorCombinedDtType(int gyro_integral_dt_offset_log,
+		int gyro_integral_dt_offset_intern,
+		int accelerometer_integral_dt_offset_log, int accelerometer_integral_dt_offset_intern)
+	: _gyro_integral_dt_offset_log(gyro_integral_dt_offset_log),
+	  _gyro_integral_dt_offset_intern(gyro_integral_dt_offset_intern),
+	  _accelerometer_integral_dt_offset_log(accelerometer_integral_dt_offset_log),
+	  _accelerometer_integral_dt_offset_intern(accelerometer_integral_dt_offset_intern)
+{
+}
+
+void *Replay::CompatSensorCombinedDtType::apply(void *data)
+{
+	// the types have the same size so we can do the conversion in-place
+	uint8_t *ptr = (uint8_t *)data;
+	float gyro_integral_dt;
+	float accel_integral_dt;
+	memcpy(&gyro_integral_dt, ptr + _gyro_integral_dt_offset_log, sizeof(float));
+	memcpy(&accel_integral_dt, ptr + _accelerometer_integral_dt_offset_log, sizeof(float));
+	uint32_t igyro_integral_dt = (uint32_t)(gyro_integral_dt * 1e6f);
+	uint32_t iaccel_integral_dt = (uint32_t)(accel_integral_dt * 1e6f);
+	memcpy(ptr + _gyro_integral_dt_offset_intern, &igyro_integral_dt, sizeof(float));
+	memcpy(ptr + _accelerometer_integral_dt_offset_intern, &iaccel_integral_dt, sizeof(float));
+	return data;
+}
+
 void Replay::setupReplayFile(const char *file_name)
 {
 	if (_replay_file) {
@@ -316,20 +341,53 @@ bool Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size)
 		return true;
 	}
 
+	CompatBase *compat = nullptr;
+
 	//check the format: the field definitions must match
 	//FIXME: this should check recursively, all used nested types
 	string file_format = _file_formats[topic_name];
 
 	if (file_format != orb_meta->o_fields) {
-		PX4_WARN("Formats for %s don't match. Will ignore it.", topic_name.c_str());
-		PX4_WARN(" Internal format: %s", orb_meta->o_fields);
-		PX4_WARN(" File format    : %s", file_format.c_str());
-		return true; // not a fatal error
+		// check if we have a compatibility conversion available
+		if (topic_name == "sensor_combined") {
+			if (string(orb_meta->o_fields) == "uint64_t timestamp;float[3] gyro_rad;uint32_t gyro_integral_dt;"
+			    "int32_t accelerometer_timestamp_relative;float[3] accelerometer_m_s2;"
+			    "uint32_t accelerometer_integral_dt;int32_t magnetometer_timestamp_relative;"
+			    "float[3] magnetometer_ga;int32_t baro_timestamp_relative;float baro_alt_meter;"
+			    "float baro_temp_celcius;" &&
+			    file_format == "uint64_t timestamp;float[3] gyro_rad;float gyro_integral_dt;"
+			    "int32_t accelerometer_timestamp_relative;float[3] accelerometer_m_s2;"
+			    "float accelerometer_integral_dt;int32_t magnetometer_timestamp_relative;"
+			    "float[3] magnetometer_ga;int32_t baro_timestamp_relative;float baro_alt_meter;"
+			    "float baro_temp_celcius;") {
+				int gyro_integral_dt_offset_log;
+				int gyro_integral_dt_offset_intern;
+				int accelerometer_integral_dt_offset_log;
+				int accelerometer_integral_dt_offset_intern;
+				int unused;
+
+				if (findFieldOffset(file_format, "gyro_integral_dt", gyro_integral_dt_offset_log, unused) &&
+				    findFieldOffset(orb_meta->o_fields, "gyro_integral_dt", gyro_integral_dt_offset_intern, unused) &&
+				    findFieldOffset(file_format, "accelerometer_integral_dt", accelerometer_integral_dt_offset_log, unused) &&
+				    findFieldOffset(orb_meta->o_fields, "accelerometer_integral_dt", accelerometer_integral_dt_offset_intern, unused)) {
+					compat = new CompatSensorCombinedDtType(gyro_integral_dt_offset_log, gyro_integral_dt_offset_intern,
+										accelerometer_integral_dt_offset_log, accelerometer_integral_dt_offset_intern);
+				}
+			}
+		}
+
+		if (!compat) {
+			PX4_WARN("Formats for %s don't match. Will ignore it.", topic_name.c_str());
+			PX4_WARN(" Internal format: %s", orb_meta->o_fields);
+			PX4_WARN(" File format    : %s", file_format.c_str());
+			return true; // not a fatal error
+		}
 	}
 
 	Subscription subscription;
 	subscription.orb_meta = orb_meta;
 	subscription.multi_id = multi_id;
+	subscription.compat = compat;
 
 
 	//find the timestamp offset
@@ -755,6 +813,11 @@ void Replay::run()
 	}
 
 	for (auto &subscription : _subscriptions) {
+		if (subscription.compat) {
+			delete subscription.compat;
+			subscription.compat = nullptr;
+		}
+
 		if (subscription.orb_advert) {
 			orb_unadvertise(subscription.orb_advert);
 			subscription.orb_advert = nullptr;
@@ -805,6 +868,10 @@ bool Replay::publishTopic(Subscription &sub, void *data)
 {
 	bool published = false;
 
+	if (sub.compat) {
+		data = sub.compat->apply(data);
+	}
+
 	if (sub.orb_advert) {
 		orb_publish(sub.orb_meta, sub.orb_advert, data);
 		published = true;