diff --git a/src/modules/replay/Replay.cpp b/src/modules/replay/Replay.cpp index 8329477649..1f5c835b21 100644 --- a/src/modules/replay/Replay.cpp +++ b/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; diff --git a/src/modules/replay/Replay.hpp b/src/modules/replay/Replay.hpp index eebf82fb47..6d85b44040 100644 --- a/src/modules/replay/Replay.hpp +++ b/src/modules/replay/Replay.hpp @@ -44,7 +44,6 @@ #include #include -#include 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; }; /** diff --git a/src/modules/replay/ReplayEkf2.cpp b/src/modules/replay/ReplayEkf2.cpp index e03072b59e..6f379d5e72 100644 --- a/src/modules/replay/ReplayEkf2.cpp +++ b/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"); diff --git a/src/modules/replay/ReplayEkf2.hpp b/src/modules/replay/ReplayEkf2.hpp index 519b4242d2..357f188e6d 100644 --- a/src/modules/replay/ReplayEkf2.hpp +++ b/src/modules/replay/ReplayEkf2.hpp @@ -35,6 +35,9 @@ #include "Replay.hpp" +#include +#include + 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