Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Common: add `get_distance_NED_alt_frame` method

This commit is contained in:
Iampete1 2024-03-03 01:01:25 +00:00 committed by Randy Mackay
parent 1cdff47246
commit 1b1ce9530c
2 changed files with 16 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_Common/Location.cpp
View File

@ -294,6 +294,19 @@ Vector3d Location::get_distance_NED_double(const Location &loc2) const
(alt - loc2.alt) * 0.01);
}
// return the distance in meters in North/East/Down plane as a N/E/D vector to loc2 considering alt frame, if altitude cannot be resolved down distance is 0
Vector3f Location::get_distance_NED_alt_frame(const Location &loc2) const
{
int32_t alt1, alt2 = 0;
if (!get_alt_cm(AltFrame::ABSOLUTE, alt1) || !loc2.get_alt_cm(AltFrame::ABSOLUTE, alt2)) {
// one or both of the altitudes are invalid, don't do alt distance calc
alt1 = 0, alt2 = 0;
}
return Vector3f((loc2.lat - lat) * LOCATION_SCALING_FACTOR,
diff_longitude(loc2.lng,lng) * LOCATION_SCALING_FACTOR * longitude_scale((loc2.lat+lat)/2),
(alt1 - alt2) * 0.01);
}
Vector2d Location::get_distance_NE_double(const Location &loc2) const
{
return Vector2d((loc2.lat - lat) * double(LOCATION_SCALING_FACTOR),

3
libraries/AP_Common/Location.h
View File

@ -73,6 +73,9 @@ public:
Vector3f get_distance_NED(const Location &loc2) const;
Vector3d get_distance_NED_double(const Location &loc2) const;
// return the distance in meters in North/East/Down plane as a N/E/D vector to loc2 considering alt frame, if altitude cannot be resolved down distance is 0
Vector3f get_distance_NED_alt_frame(const Location &loc2) const;
// return the distance in meters in North/East plane as a N/E vector to loc2
Vector2f get_distance_NE(const Location &loc2) const;
Vector2d get_distance_NE_double(const Location &loc2) const;