From daa44701e9cc2a799cd61bd9c5f0375da9346aa8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Aug 2023 13:56:01 +1000 Subject: [PATCH] AP_GPS: fixed RTK injection when first module is a BASE this is a partial backport of #24132 which fixes RTK injection when the 1st GPS module is a DroneCAN RTK rover. Without this change RTCM injection for RTK fix on the base will only work if it happens to come up as the first module --- libraries/AP_GPS/AP_GPS_UAVCAN.cpp | 5 ++++- libraries/AP_GPS/AP_GPS_UAVCAN.h | 1 + 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp index c108f64d96..014f6546e5 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp @@ -820,9 +820,12 @@ void AP_GPS_UAVCAN::inject_data(const uint8_t *data, uint16_t len) // using a different uavcan instance than the first GPS, as we // send the data as broadcast on all UAVCAN devive ports and we // don't want to send duplicates + const uint32_t now_ms = AP_HAL::millis(); if (_detected_module == 0 || - _detected_modules[_detected_module].ap_uavcan != _detected_modules[0].ap_uavcan) { + _detected_modules[_detected_module].ap_uavcan != _detected_modules[0].ap_uavcan || + now_ms - _detected_modules[0].last_inject_ms > 2000) { _detected_modules[_detected_module].ap_uavcan->send_RTCMStream(data, len); + _detected_modules[_detected_module].last_inject_ms = now_ms; } } diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.h b/libraries/AP_GPS/AP_GPS_UAVCAN.h index 9ac0894bb6..5daeca1c48 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.h +++ b/libraries/AP_GPS/AP_GPS_UAVCAN.h @@ -127,6 +127,7 @@ private: AP_UAVCAN* ap_uavcan; uint8_t node_id; uint8_t instance; + uint32_t last_inject_ms; AP_GPS_UAVCAN* driver; } _detected_modules[GPS_MAX_RECEIVERS];