diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
index 70d29a4ba4..83322cb10b 100644
--- a/libraries/AP_GPS/AP_GPS.cpp
+++ b/libraries/AP_GPS/AP_GPS.cpp
@@ -101,8 +101,8 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
 #if GPS_MAX_RECEIVERS > 1
     // @Param: AUTO_SWITCH
     // @DisplayName: Automatic Switchover Setting
-    // @Description: Automatic switchover to GPS reporting best lock, 1:UseBest selects the GPS with highest status, if both are equal the GPS with highest satellite count is used
-    // @Values: 0:Use primary, 1:UseBest, 2:Blend
+    // @Description: Automatic switchover to GPS reporting best lock, 1:UseBest selects the GPS with highest status, if both are equal the GPS with highest satellite count is used 4:Use primary if 3D fix or better, will revert over 'UseBest' behaviour if 3D fix is lost on primary
+    // @Values: 0:Use primary, 1:UseBest, 2:Blend, 4:Use primary if 3D fix or better
     // @User: Advanced
     AP_GROUPINFO("AUTO_SWITCH", 3, AP_GPS, _auto_switch, (int8_t)GPSAutoSwitch::USE_BEST),
 #endif
@@ -988,6 +988,17 @@ void AP_GPS::update_primary(void)
         return;
     }
 
+
+    // Use primary if 3D fix or better
+    if (((GPSAutoSwitch)_auto_switch.get() == GPSAutoSwitch::USE_PRIMARY_IF_3D_FIX) && (state[primary_param].status >= GPS_OK_FIX_3D)) {
+        // Primary GPS has a least a 3D fix, switch to it if necessary
+        if (primary_instance != primary_param) {
+            primary_instance = primary_param;
+            _last_instance_swap_ms = now;
+        }
+        return;
+    }
+
     // handle switch between real GPSs
     for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
         if (i == primary_instance) {
diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h
index bbb2e5fc9a..00ec53e017 100644
--- a/libraries/AP_GPS/AP_GPS.h
+++ b/libraries/AP_GPS/AP_GPS.h
@@ -522,6 +522,7 @@ protected:
     AP_Int8 _blend_mask;
     AP_Float _blend_tc;
     AP_Int16 _driver_options;
+    AP_Int8 _primary;
 
 #if GPS_MOVING_BASELINE
     MovingBase mb_params[GPS_MAX_RECEIVERS];
@@ -660,6 +661,7 @@ private:
         USE_BEST    = 1,
         BLEND       = 2,
         //USE_SECOND  = 3, deprecated for new primary param
+        USE_PRIMARY_IF_3D_FIX = 4,
     };
 
     // used for flight testing with GPS loss