AP_GPS: priortise F9P with 3D lock over M8

Author: tridge

libraries/AP_GPS/AP_GPS.cpp

@@ -945,6 +945,25 @@ void AP_GPS::update_primary(void)
         if (i == primary_instance) {
             continue;
         }
+
+        // if primary is not a F9 and altternative is a F9 with 3D fix then use the F9
+        if (state[i].status >= GPS_OK_FIX_3D &&
+            strcmp(drivers[i]->name(), "u-blox") == 0 && drivers[i]->hardware_generation() == AP_GPS_UBLOX::UBLOX_F9 &&
+            strcmp(drivers[primary_instance]->name(), "u-blox") == 0 && drivers[primary_instance]->hardware_generation() != AP_GPS_UBLOX::UBLOX_F9) {
+            primary_instance = i;
+            _last_instance_swap_ms = now;
+            continue;
+        }
+
+        // if primary F9 has 3D lock, and 2nd GPS is a M8, don't switch
+        if (state[primary_instance].status >= GPS_OK_FIX_3D &&
+            strcmp(drivers[primary_instance]->name(), "u-blox") == 0 && drivers[primary_instance]->hardware_generation() == AP_GPS_UBLOX::UBLOX_F9 &&
+            strcmp(drivers[i]->name(), "u-blox") == 0 && drivers[i]->hardware_generation() != AP_GPS_UBLOX::UBLOX_F9) {
+            // don't switch away from a F9 with 3D lock to a M8
+            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
             primary_instance = i;

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -144,6 +144,17 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
 
     // ublox specific healthy checks
     bool is_healthy(void) const override;
+
+    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
@@ -620,16 +631,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,