From 4fca2ee852a31121ca64434791d04d33ab269d11 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 22 Feb 2024 08:00:06 +1100 Subject: [PATCH] AP_GPS: prevent announcing DroneCAN at 5Hz --- libraries/AP_GPS/AP_GPS.cpp | 3 +-- libraries/AP_GPS/AP_GPS_DroneCAN.cpp | 3 +++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 52f622458ef97..a496df51e68df 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -1000,8 +1000,7 @@ void AP_GPS::update_instance(uint8_t instance) state[instance].corrected_timestamp_updated = false; } - // we set delta_time_ms to the timeout value when clearing - // state; use it being zero to mark first message + // announce the GPS type once if (!state[instance].announced_detection) { state[instance].announced_detection = true; GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %d: detected %s", instance + 1, drivers[instance]->name()); diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp index 5da56758a517c..8515f5d9ee057 100644 --- a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp @@ -717,6 +717,9 @@ bool AP_GPS_DroneCAN::read(void) interim_state.vertical_accuracy = MIN(interim_state.vertical_accuracy, 1000.0); interim_state.speed_accuracy = MIN(interim_state.speed_accuracy, 1000.0); + // prevent announcing multiple times + interim_state.announced_detection = state.announced_detection; + state = interim_state; if (interim_state.last_corrected_gps_time_us) { // If we were able to get a valid last_corrected_gps_time_us