Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 15:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: Add a 3D location difference, returning NED

Browse Source
This commit is contained in:
Michael du Breuil 2017-01-30 12:05:05 -07:00 committed by Andrew Tridgell
parent c716e76bfc
commit 97c57764c4
2 changed files with 17 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_Math/location.cpp
View File

@ -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
*/

6
libraries/AP_Math/location.h
View File

@ -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
*/