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

ArduPilot · Sep 28, 2024 · 1f2fc72 · 1f2fc72
1 parent 8895723
commit 1f2fc72
Show file tree

Hide file tree

Showing 4 changed files with 9 additions and 1 deletion.
diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
@@ -1490,7 +1490,12 @@ bool AP_GPS::all_consistent(float &distance) const
     }
 
     // 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
@@ -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

diff --git 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
@@ -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;
 }
 
 /**