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;