From 407b70b82e68cf75ce501ff6f8051a9e7f45072d Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sun, 19 Sep 2021 00:13:07 +0100
Subject: [PATCH] AP_Common: location: add get alt distance method

---
 libraries/AP_Common/Location.cpp | 12 +++++++++++-
 libraries/AP_Common/Location.h   |  4 ++++
 2 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp
index a13a251a5f..92a86ebc23 100644
--- a/libraries/AP_Common/Location.cpp
+++ b/libraries/AP_Common/Location.cpp
@@ -244,6 +244,16 @@ ftype Location::get_distance(const struct Location &loc2) const
     return norm(dlat, dlng) * LOCATION_SCALING_FACTOR;
 }
 
+// return the altitude difference in meters taking into account alt frame.
+bool Location::get_alt_distance(const struct Location &loc2, ftype &distance) const
+{
+    int32_t alt1, alt2;
+    if (!get_alt_cm(AltFrame::ABSOLUTE, alt1) || !loc2.get_alt_cm(AltFrame::ABSOLUTE, alt2)) {
+        return false;
+    }
+    distance = (alt1 - alt2) * 0.01;
+    return true;
+}
 
 /*
   return the distance in meters in North/East plane as a N/E vector
@@ -255,7 +265,7 @@ Vector2f Location::get_distance_NE(const Location &loc2) const
                     diff_longitude(loc2.lng,lng) * LOCATION_SCALING_FACTOR * longitude_scale((loc2.lat+lat)/2));
 }
 
-// return the distance in meters in North/East/Down plane as a N/E/D vector to loc2
+// return the distance in meters in North/East/Down plane as a N/E/D vector to loc2, NOT CONSIDERING ALT FRAME!
 Vector3f Location::get_distance_NED(const Location &loc2) const
 {
     return Vector3f((loc2.lat - lat) * LOCATION_SCALING_FACTOR,
diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h
index 8f5a34c3ac..9a1d8a7213 100644
--- a/libraries/AP_Common/Location.h
+++ b/libraries/AP_Common/Location.h
@@ -59,7 +59,11 @@ public:
     // return distance in meters between two locations
     ftype get_distance(const struct Location &loc2) const;
 
+    // return the altitude difference in meters taking into account alt frame.
+    bool get_alt_distance(const struct Location &loc2, ftype &distance) const WARN_IF_UNUSED;
+
     // return the distance in meters in North/East/Down plane as a N/E/D vector to loc2
+    // NOT CONSIDERING ALT FRAME!
     Vector3f get_distance_NED(const Location &loc2) const;
     Vector3d get_distance_NED_double(const Location &loc2) const;