From 68b4c38180d93fb71649e5216a0e0af3b3d138d7 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 1 Feb 2022 20:34:22 +1100
Subject: [PATCH] AP_GPS: prevent switching to a dead GPS

when we switch away from the blended GPS instance we need to ensure we
don't switch to a GPS that is timing out, and may be the instance that
is triggering the disable of blending
---
 libraries/AP_GPS/AP_GPS.cpp | 29 +++++++++++++++++++++++++----
 1 file changed, 25 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
index c93206719c..46a205cd89 100644
--- a/libraries/AP_GPS/AP_GPS.cpp
+++ b/libraries/AP_GPS/AP_GPS.cpp
@@ -1113,13 +1113,34 @@ void AP_GPS::update_primary(void)
     if (primary_instance == GPS_BLENDED_INSTANCE) {
         primary_instance = 0;
         for (uint8_t i=1; i<GPS_MAX_RECEIVERS; i++) {
-            // choose GPS with highest state or higher number of satellites
-            if ((state[i].status > state[primary_instance].status) ||
-                ((state[i].status == state[primary_instance].status) && (state[i].num_sats > state[primary_instance].num_sats))) {
+            // choose GPS with highest state or higher number of
+            // satellites. Reject a GPS with an old update time, as it
+            // may be the old timestamp that triggered the loss of
+            // blending
+            const uint32_t delay_threshold = 400;
+            const bool higher_status = state[i].status > state[primary_instance].status;
+            const bool old_data_primary = (now - state[primary_instance].last_gps_time_ms) > delay_threshold;
+            const bool old_data = (now - state[i].last_gps_time_ms) > delay_threshold;
+            const bool equal_status = state[i].status == state[primary_instance].status;
+            const bool more_sats = state[i].num_sats > state[primary_instance].num_sats;
+            if (old_data && !old_data_primary) {
+                // don't switch to a GPS that has not updated in 400ms
+                continue;
+            }
+            if (state[i].status < GPS_OK_FIX_3D) {
+                // don't use a GPS without 3D fix
+                continue;
+            }
+            // select the new GPS if the primary has old data, or new
+            // GPS either has higher status, or has the same status
+            // and more satellites
+            if ((old_data_primary && !old_data) ||
+                higher_status ||
+                (equal_status && more_sats)) {
                 primary_instance = i;
-                _last_instance_swap_ms = now;
             }
         }
+        _last_instance_swap_ms = now;
         return;
     }
 #endif  // defined(GPS_BLENDED_INSTANCE)