diff --git a/src/modules/replay/replay.hpp b/src/modules/replay/replay.hpp index 7aba892807..1cbcb3c99f 100644 --- a/src/modules/replay/replay.hpp +++ b/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; diff --git a/src/modules/replay/replay_main.cpp b/src/modules/replay/replay_main.cpp index 8c0de8efd5..418e04040c 100644 --- a/src/modules/replay/replay_main.cpp +++ b/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;