From f2471861a38cc43bdd23e9642fffb86fddcf4c87 Mon Sep 17 00:00:00 2001 From: Alexander Lerach Date: Thu, 13 Feb 2025 10:13:26 +0100 Subject: [PATCH] Fix GPS RTCM instance selection --- src/drivers/gps/gps.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index aeeba755e3..ba6f24070f 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -547,8 +547,13 @@ void GPS::handleInjectDataTopic() for (int instance = 0; instance < _orb_inject_data_sub.size(); instance++) { const bool exists = _orb_inject_data_sub[instance].advertised(); - if (exists) { - if (_orb_inject_data_sub[instance].copy(&msg)) { + if (exists && _orb_inject_data_sub[instance].copy(&msg)) { + /* Don't select the own RTCM instance. In case it has a lower + * instance number, it will be selected and will be rejected + * later in the code, resulting in no RTCM injection at all. + */ + if (msg.device_id != get_device_id()) { + // Only use the message if it is up to date if ((hrt_absolute_time() - msg.timestamp) < 5_s) { // Remember that we already did a copy on this instance. already_copied = true;