From 76483a51f8f312667982cc24826361eca0e0fef2 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 896296bc9b..36877325d0 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp @@ -947,9 +947,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 fa4b7eaade..0ff1b8d0c6 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.h +++ b/libraries/AP_GPS/AP_GPS_UAVCAN.h @@ -124,6 +124,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];