From 3544549cf497f35fa55dfede2b744437c00fb9ac Mon Sep 17 00:00:00 2001
From: Tom Pittenger <magicrub@gmail.com>
Date: Thu, 21 May 2015 14:07:59 -0700
Subject: [PATCH] AP_GPS: Allow switching primary GPS instance with 1 sat
 difference

Rapid switching between GPS receivers can cause real problems.
Switch if:
1) secondary GPS has 1 more satellite for at least 20 seconds
OR
2) secondary GPS has 2 more satellites for at least 5 seconds

Fixes https://github.com/diydrones/ardupilot/pull/2320
---
 libraries/AP_GPS/AP_GPS.cpp | 17 ++++++++++++++---
 libraries/AP_GPS/AP_GPS.h   |  1 +
 2 files changed, 15 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
index 7c3761ab5d..311e742460 100644
--- a/libraries/AP_GPS/AP_GPS.cpp
+++ b/libraries/AP_GPS/AP_GPS.cpp
@@ -120,6 +120,7 @@ void AP_GPS::init(DataFlash_Class *dataflash, const AP_SerialManager& serial_man
 
 #if GPS_MAX_INSTANCES > 1
     _port[1] = serial_manager.find_serial(AP_SerialManager::SerialProtocol_GPS, 1);
+    _last_instance_swap_ms = 0;
 #endif
 }
 
@@ -380,14 +381,24 @@ AP_GPS::update(void)
                 primary_instance = i;
                 continue;
             }
-            if (state[i].status == state[primary_instance].status &&
-                state[i].num_sats >= state[primary_instance].num_sats + 2) {
-                // this GPS has at least 2 more satellites than the
+
+            bool another_gps_has_1_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 1);
+
+            if (state[i].status == state[primary_instance].status && another_gps_has_1_or_more_sats) {
+
+                uint32_t now = hal.scheduler->millis();
+                bool another_gps_has_2_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 2);
+
+                if ( (another_gps_has_1_or_more_sats && (now - _last_instance_swap_ms) >= 20000) ||
+                     (another_gps_has_2_or_more_sats && (now - _last_instance_swap_ms) >= 5000 ) ) {
+                // this GPS has more satellites than the
                 // current primary, switch primary. Once we switch we will
                 // then tend to stick to the new GPS as primary. We don't
                 // want to switch too often as it will look like a
                 // position shift to the controllers.
                 primary_instance = i;
+                _last_instance_swap_ms = now;
+                }
             }
         } else {
             primary_instance = 0;
diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h
index a71fcae24e..e01ba56ba0 100644
--- a/libraries/AP_GPS/AP_GPS.h
+++ b/libraries/AP_GPS/AP_GPS.h
@@ -331,6 +331,7 @@ public:
     AP_Int8 _min_dgps;
     AP_Int16 _sbp_logmask;
     AP_Int8 _inject_to;
+    uint32_t _last_instance_swap_ms;
 #endif
     AP_Int8 _sbas_mode;
     AP_Int8 _min_elevation;