From 03679044ce48c99c999b92194ae74d29ba164435 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 7 May 2018 15:17:29 +0200 Subject: [PATCH] ekf2 replay: handle multi multiple GPS instances --- src/modules/replay/replay_main.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/modules/replay/replay_main.cpp b/src/modules/replay/replay_main.cpp index 14eb6435bc57..7b275cd3c433 100644 --- a/src/modules/replay/replay_main.cpp +++ b/src/modules/replay/replay_main.cpp @@ -946,7 +946,8 @@ bool ReplayEkf2::handleTopicUpdate(Subscription &sub, void *data, std::ifstream return true; - } else if (sub.orb_meta == ORB_ID(vehicle_status) || sub.orb_meta == ORB_ID(vehicle_land_detected)) { + } 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); } // else: do not publish @@ -965,7 +966,9 @@ void ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id) _distance_sensor_msg_id = msg_id; } else if (sub.orb_meta == ORB_ID(vehicle_gps_position)) { - _gps_msg_id = msg_id; + if (sub.multi_id == 0) { + _gps_msg_id = msg_id; + } } else if (sub.orb_meta == ORB_ID(optical_flow)) { _optical_flow_msg_id = msg_id; @@ -986,7 +989,8 @@ void ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id) // the main loop should only handle publication of the following topics, the sensor topics are // handled separately in publishEkf2Topics() 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_land_detected) && + (sub.orb_meta != ORB_ID(vehicle_gps_position) || sub.multi_id == 0); } bool ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifstream &replay_file)