AP_GPS: Ignore Z-axis difference while dual GPS difference check

libraries/AP_GPS/AP_GPS.cpp

@@ -1484,13 +1484,18 @@ bool AP_GPS::all_consistent(float &distance) const
 {
     // return true immediately if only one valid receiver
     if (num_instances <= 1 ||
-        drivers[0] == nullptr || params[0].type == GPS_TYPE_NONE) {
+        drivers[0] == nullptr || params[0].type == GPS_TYPE_NONE || GPS_MAX_INSTANCES <= 1) {
         distance = 0;
         return true;
     }
 
     // calculate distance
-    distance = state[0].location.get_distance_NED(state[1].location).length();
+    if (state[0].have_altitude && state[1].have_altitude) {
+        distance = state[0].location.get_distance_NED(state[1].location).length();
+    } else {
+        distance = state[0].location.get_distance_NE(state[1].location).length();
+    }
+
     // success if distance is within 50m
     return (distance < 50);
 }

libraries/AP_GPS/AP_GPS.h

@@ -216,6 +216,7 @@ class AP_GPS
         bool have_vertical_accuracy;      ///< does GPS give vertical position accuracy? Set to true only once available.
         bool have_gps_yaw;                ///< does GPS give yaw? Set to true only once available.
         bool have_gps_yaw_accuracy;       ///< does the GPS give a heading accuracy estimate? Set to true only once available
+        bool have_altitude;               ///< does GPS give altitude? Set to false only if GPS instance does not provide altitude.
         float undulation;                   //<height that WGS84 is above AMSL at the current location
         bool have_undulation;               ///<do we have a value for the undulation
         uint32_t last_gps_time_ms;          ///< the system time we got the last GPS timestamp, milliseconds

libraries/AP_GPS/AP_GPS_MAV.cpp

@@ -72,6 +72,7 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t &msg)
             if (have_alt) {
                 loc.alt = packet.alt * 100; // convert to centimeters
             }
+            state.have_altitude = have_alt;
             state.location = loc;
 
             if (have_hdop) {

libraries/AP_GPS/GPS_Backend.cpp

@@ -50,6 +50,7 @@ AP_GPS_Backend::AP_GPS_Backend(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GP
     state.have_speed_accuracy = false;
     state.have_horizontal_accuracy = false;
     state.have_vertical_accuracy = false;
+    state.have_altitude = true;
 }
 
 /**