diff --git a/src/modules/replay/ReplayEkf2.cpp b/src/modules/replay/ReplayEkf2.cpp index 1add36489d82..89f4874ce545 100644 --- a/src/modules/replay/ReplayEkf2.cpp +++ b/src/modules/replay/ReplayEkf2.cpp @@ -108,6 +108,15 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id) } else if (sub.orb_meta == ORB_ID(aux_global_position)) { _aux_global_position_msg_id = msg_id; + + } else if (sub.orb_meta == ORB_ID(vehicle_local_position_groundtruth)) { + _vehicle_local_position_groundtruth_msg_id = msg_id; + + } else if (sub.orb_meta == ORB_ID(vehicle_attitude_groundtruth)) { + _vehicle_attitude_groundtruth_msg_id = msg_id; + + } else if (sub.orb_meta == ORB_ID(vehicle_global_position_groundtruth)) { + _vehicle_global_position_groundtruth_msg_id = msg_id; } // the main loop should only handle publication of the following topics, the sensor topics are @@ -135,6 +144,9 @@ ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifs handle_sensor_publication(ekf2_timestamps.vehicle_magnetometer_timestamp_rel, _vehicle_magnetometer_msg_id); handle_sensor_publication(ekf2_timestamps.visual_odometry_timestamp_rel, _vehicle_visual_odometry_msg_id); handle_sensor_publication(0, _aux_global_position_msg_id); + handle_sensor_publication(0, _vehicle_local_position_groundtruth_msg_id); + handle_sensor_publication(0, _vehicle_global_position_groundtruth_msg_id); + handle_sensor_publication(0, _vehicle_attitude_groundtruth_msg_id); // sensor_combined: publish last because ekf2 is polling on this if (!findTimestampAndPublish(ekf2_timestamps.timestamp / 100, _sensor_combined_msg_id, replay_file)) { diff --git a/src/modules/replay/ReplayEkf2.hpp b/src/modules/replay/ReplayEkf2.hpp index bb7f8d135939..13cb3d8be5d5 100644 --- a/src/modules/replay/ReplayEkf2.hpp +++ b/src/modules/replay/ReplayEkf2.hpp @@ -89,6 +89,9 @@ class ReplayEkf2 : public Replay uint16_t _vehicle_magnetometer_msg_id = msg_id_invalid; uint16_t _vehicle_visual_odometry_msg_id = msg_id_invalid; uint16_t _aux_global_position_msg_id = msg_id_invalid; + 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; }; } //namespace px4