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

AP_Common: location: add get alt distance method

Browse Source
This commit is contained in:
Iampete1 2021-09-19 00:13:07 +01:00 committed by Andrew Tridgell
parent 0549e08c5c
commit 407b70b82e
2 changed files with 15 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Common/Location.cpp
View File

@ -244,6 +244,16 @@ ftype Location::get_distance(const struct Location &loc2) const
return norm(dlat, dlng) * LOCATION_SCALING_FACTOR; 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 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)); 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 Vector3f Location::get_distance_NED(const Location &loc2) const
{ {
return Vector3f((loc2.lat - lat) * LOCATION_SCALING_FACTOR, return Vector3f((loc2.lat - lat) * LOCATION_SCALING_FACTOR,

4
libraries/AP_Common/Location.h
View File

@ -59,7 +59,11 @@ public:
// return distance in meters between two locations // return distance in meters between two locations
ftype get_distance(const struct Location &loc2) const; 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 // 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; Vector3f get_distance_NED(const Location &loc2) const;
Vector3d get_distance_NED_double(const Location &loc2) const; Vector3d get_distance_NED_double(const Location &loc2) const;