From e995a198bd71f7b5c03f1a6784be749525cc4428 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 5 Oct 2020 08:18:12 +1100 Subject: [PATCH] AP_GPS: always send GPS2_RAW if 2nd GPS configured this improves the display on the GCS when the GPS has not yet been found. This is particularly important after a reboot, as otherwise the GCS may display stale information from the previous boot --- libraries/AP_GPS/AP_GPS.cpp | 7 ++----- libraries/AP_GPS/AP_GPS.h | 3 --- 2 files changed, 2 insertions(+), 8 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 7b5b8c8024..06c0c92b8c 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -665,9 +665,6 @@ found_gps: timing[instance].last_message_time_ms = now; timing[instance].delta_time_ms = GPS_TIMEOUT_MS; new_gps->broadcast_gps_type(); - if (instance == 1) { - has_had_second_instance = true; - } } } @@ -1186,8 +1183,8 @@ void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan) #if GPS_MAX_RECEIVERS > 1 void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan) { - // always send the message once we've ever seen the GPS - if (!has_had_second_instance) { + // always send the message if 2nd GPS is configured + if (_type[1] == GPS_TYPE_NONE) { return; } diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 32956dc80f..7c84db331b 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -659,9 +659,6 @@ private: // used for flight testing with GPS yaw loss bool _force_disable_gps_yaw; - - // used to ensure we continue sending status messages if we ever detected the second GPS - bool has_had_second_instance; }; namespace AP {