AP_GPS: add primary param

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
-    // @Values: 0:Disabled,1:UseBest,2:Blend,3:UseSecond
+    // @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
     // @User: Advanced
     AP_GROUPINFO("AUTO_SWITCH", 3, AP_GPS, _auto_switch, (int8_t)GPSAutoSwitch::USE_BEST),
 #endif
@@ -336,6 +336,16 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
 
 #endif // GPS_MOVING_BASELINE
 
+#if GPS_MAX_RECEIVERS > 1
+    // @Param: PRIMARY
+    // @DisplayName: Primary GPS
+    // @Description: This GPS will be used when GPS_AUTO_SWITCH is 0 and used preferentially with GPS_AUTO_SWITCH = 4.
+    // @Increment: 1
+    // @Values: 0:FirstGPS, 1:SecondGPS
+    // @User: Advanced
+    AP_GROUPINFO("PRIMARY", 27, AP_GPS, _primary, 0),
+#endif
+
     AP_GROUPEND
 };
 
@@ -372,7 +382,11 @@ bool AP_GPS::needs_uart(GPS_Type type) const
 /// Startup initialisation.
 void AP_GPS::init(const AP_SerialManager& serial_manager)
 {
-    primary_instance = 0;
+    // Set new primary param based on old auto_switch use second option
+    if ((_auto_switch.get() == 3) && !_primary.configured()) {
+        _primary.set_and_save(1);
+        _auto_switch.set_and_save(0);
+    }
 
     // search for serial ports with gps protocol
     uint8_t uart_idx = 0;
@@ -903,15 +917,19 @@ void AP_GPS::update_primary(void)
         return;
     }
 
-    if ((GPSAutoSwitch)_auto_switch.get() == GPSAutoSwitch::NONE) {
-        // AUTO_SWITCH is 0 so no switching of GPSs, always use first instance
-        primary_instance = 0;
-        return;
+    // check the primary param is set to possible GPS
+    int8_t primary_param = _primary.get();
+    if ((primary_param < 0) || (primary_param>=GPS_MAX_RECEIVERS)) {
+        primary_param = 0;
+    }
+    // if primary is not enabled try first instance
+    if (get_type(primary_param) == GPS_TYPE_NONE) {
+        primary_param = 0;
     }
 
-    if ((GPSAutoSwitch)_auto_switch.get() == GPSAutoSwitch::USE_SECOND) {
-        // always select the second GPS instance
-        primary_instance = 1;
+    if ((GPSAutoSwitch)_auto_switch.get() == GPSAutoSwitch::NONE) {
+        // No switching of GPSs, always use the primary instance
+        primary_instance = primary_param;
         return;
     }
 
@@ -1782,6 +1800,10 @@ bool AP_GPS::is_healthy(uint8_t instance) const
         return false;
     }
 
+    if (get_type(_primary.get()) == GPS_TYPE_NONE) {
+        return false;
+    }
+
     /*
       allow two lost frames before declaring the GPS unhealthy, but
       require the average frame rate to be close to 5Hz. We allow for

libraries/AP_GPS/AP_GPS.h

@@ -659,7 +659,7 @@ private:
         NONE        = 0,
         USE_BEST    = 1,
         BLEND       = 2,
-        USE_SECOND  = 3,
+        //USE_SECOND  = 3, deprecated for new primary param
     };
 
     // used for flight testing with GPS loss