AP_GPS: Prefer u-blox F9P

libraries/AP_GPS/AP_GPS.cpp

@@ -794,26 +794,35 @@ void AP_GPS::update(void)
                     if (i == primary_instance) {
                         continue;
                     }
-                    if (state[i].status > state[primary_instance].status) {
-                        // we have a higher status lock, or primary is set to the blended GPS, change GPS
-
-                        if (state[primary_instance].status < GPS_OK_FIX_3D) {
-                            primary_instance = i;
-                            _last_instance_swap_ms = now;
-                            gcs().send_text(MAV_SEVERITY_CRITICAL, "GPS Switch: Switched to %u", primary_instance+1);
-                            // add alert when a switch happens
-                            continue;
-                        }
-                        // switch only if the currently used GPS has a 2D FIX or lower
-
-                        if (state[i].status > GPS_OK_FIX_3D) {
-                            primary_instance = i;
-                            _last_instance_swap_ms = now;
-                            gcs().send_text(MAV_SEVERITY_CRITICAL, "GPS Switch: Switched to %u", primary_instance+1);
-                            // add alert when a switch happens
-                            continue;
-                        }
-                        // er if the other GPS has a DGPS FIX (the best possible fix)
+
+                    GPS_Status status_i = state[i].status;
+                    GPS_Status status_primary = state[primary_instance].status;
+
+
+                    // Treat U-blox F9 as SBAS if it has more than or equal to 16 satellites, as it is dual band GPS and is more accurate than M8 with SBAS                    
+                    if (status_i == GPS_OK_FIX_3D && state[i].num_sats >= 16 && strcmp(drivers[i]->name(), "u-blox") == 0 && drivers[i]->hardware_generation() == AP_GPS_UBLOX::UBLOX_F9) {
+                        status_i = GPS_OK_FIX_3D_DGPS;
+                    }
+
+                    // Treat U-blox F9 as SBAS if it has more than or equal to 16 satellites, as it is dual band GPS and is more accurate than M8 with SBAS
+                    if (status_primary == GPS_OK_FIX_3D && state[primary_instance].num_sats >= 16 && strcmp(drivers[primary_instance]->name(), "u-blox") == 0 && drivers[primary_instance]->hardware_generation() == AP_GPS_UBLOX::UBLOX_F9) {
+                        status_primary = GPS_OK_FIX_3D_DGPS;
+                    }
+
+                    bool should_switch = false;
+
+                    if (status_i > status_primary) {
+                        should_switch = true;
+                    }
+
+                    else if (status_i == status_primary && !hal.util->get_soft_armed() && state[i].num_sats >= state[primary_instance].num_sats+3) {
+                        should_switch = true;                        
+                    }
+
+                    if (should_switch) {
+                        primary_instance = i;
+                        _last_instance_swap_ms = now;
+                        gcs().send_text(MAV_SEVERITY_CRITICAL, "GPS Switch: Switched to %u", primary_instance+1);
                     }
                 }
             }

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -128,6 +128,17 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
 
     const char *name() const override { return "u-blox"; }
 
+    enum ubx_hardware_version {
+        ANTARIS = 0,
+        UBLOX_5,
+        UBLOX_6,
+        UBLOX_7,
+        UBLOX_M8,
+        UBLOX_F9 = 0x80, // comes from MON_VER hwVersion string
+        UBLOX_UNKNOWN_HARDWARE_GENERATION = 0xff // not in the ublox spec used for
+                                                 // flagging state in the driver
+    };
+
 private:
     // u-blox UBX protocol essentials
     struct PACKED ubx_header {
@@ -564,16 +575,6 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
         NAV_STATUS_FIX_VALID = 1,
         NAV_STATUS_DGPS_USED = 2
     };
-    enum ubx_hardware_version {
-        ANTARIS = 0,
-        UBLOX_5,
-        UBLOX_6,
-        UBLOX_7,
-        UBLOX_M8,
-        UBLOX_F9 = 0x80, // comes from MON_VER hwVersion string
-        UBLOX_UNKNOWN_HARDWARE_GENERATION = 0xff // not in the ublox spec used for
-                                                 // flagging state in the driver
-    };
 
     enum config_step {
         STEP_PVT = 0,
@@ -673,7 +674,7 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
     }
 
     // returns hardware generation of the GPS
-    uint32_t hardware_generation() { 
+    uint32_t hardware_generation() const override { 
         return _hardware_generation;
     }
 };

libraries/AP_GPS/GPS_Backend.h

@@ -66,7 +66,7 @@ class AP_GPS_Backend
     virtual bool prepare_for_arming(void) { return true; }
 
     // returns hardware generation variable
-    virtual uint32_t hardware_generation() { return 0; }
+    virtual uint32_t hardware_generation() const { return 0; }
 
 protected:
     AP_HAL::UARTDriver *port;           ///< UART we are attached to