From 97c57764c46d93d4e3c3798ca731c628454d1641 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Mon, 30 Jan 2017 12:05:05 -0700 Subject: [PATCH] AP_Math: Add a 3D location difference, returning NED --- libraries/AP_Math/location.cpp | 11 +++++++++++ libraries/AP_Math/location.h | 6 ++++++ 2 files changed, 17 insertions(+) diff --git a/libraries/AP_Math/location.cpp b/libraries/AP_Math/location.cpp index bb8072bce6..eebaa877f4 100644 --- a/libraries/AP_Math/location.cpp +++ b/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 */ diff --git a/libraries/AP_Math/location.h b/libraries/AP_Math/location.h index bf32e5484f..ef9bf73edc 100644 --- a/libraries/AP_Math/location.h +++ b/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 */