From be808343fa7557b2113235feac3b1a7ce294cbfb Mon Sep 17 00:00:00 2001
From: Mykhailo Tomashivskyi <m.tomashivskyi@steel.mil.ua>
Date: Thu, 26 Sep 2024 09:23:25 +0300
Subject: [PATCH] AP_GPS: Ignore Z-axis difference while dual GPS difference
 check

---
 libraries/AP_GPS/AP_GPS.cpp      | 9 +++++++--
 libraries/AP_GPS/AP_GPS.h        | 1 +
 libraries/AP_GPS/AP_GPS_MAV.cpp  | 1 +
 libraries/AP_GPS/GPS_Backend.cpp | 1 +
 4 files changed, 10 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
index 070276dd45..bfb4a86b8b 100644
--- a/libraries/AP_GPS/AP_GPS.cpp
+++ b/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);
 }
diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h
index a3c814ffc2..25ebf90304 100644
--- a/libraries/AP_GPS/AP_GPS.h
+++ b/libraries/AP_GPS/AP_GPS.h
@@ -216,6 +216,7 @@ public:
         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
diff --git a/libraries/AP_GPS/AP_GPS_MAV.cpp b/libraries/AP_GPS/AP_GPS_MAV.cpp
index b9100f5f17..e94c912dde 100644
--- a/libraries/AP_GPS/AP_GPS_MAV.cpp
+++ b/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) {
diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp
index eb55bd76e4..bb5606eb05 100644
--- a/libraries/AP_GPS/GPS_Backend.cpp
+++ b/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;
 }
 
 /**