AP_Math: Add a 3D location difference, returning NED

libraries/AP_Math/location.cpp

@@ -142,6 +142,17 @@ Vector2f location_diff(const struct Location &loc1, const struct Location &loc2)
                     (loc2.lng - loc1.lng) * LOCATION_SCALING_FACTOR * longitude_scale(loc1));
 }
 
+/*
+  return the distance in meters in North/East/Down plane as a N/E/D vector
+  from loc1 to loc2
+ */
+Vector3f location_3d_diff_NED(const struct Location &loc1, const struct Location &loc2)
+{
+    return Vector3f((loc2.lat - loc1.lat) * LOCATION_SCALING_FACTOR,
+                    (loc2.lng - loc1.lng) * LOCATION_SCALING_FACTOR * longitude_scale(loc1),
+                    (loc1.alt - loc2.alt) * 0.01f);
+}
+
 /*
   return true if lat and lng match. Ignores altitude and options
  */

libraries/AP_Math/location.h

@@ -59,6 +59,12 @@ void        location_offset(struct Location &loc, float ofs_north, float ofs_eas
  */
 Vector2f    location_diff(const struct Location &loc1, const struct Location &loc2);
 
+/*
+  return the distance in meters in North/East/Down plane as a N/E/D vector
+  from loc1 to loc2
+ */
+Vector3f    location_3d_diff_NED(const struct Location &loc1, const struct Location &loc2);
+
 /*
  * check if lat and lng match. Ignore altitude and options
  */