Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Common: Add same_alt_as function to Location

Browse Source
This commit is contained in:
Nick Exton 2023-02-15 15:49:40 +11:00 committed by Andrew Tridgell
parent 9e71c9953d
commit c445bb5f9e
2 changed files with 17 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_Common/Location.cpp
View File

@ -393,6 +393,20 @@ bool Location::same_latlon_as(const Location &loc2) const
return (lat == loc2.lat) && (lng == loc2.lng); return (lat == loc2.lat) && (lng == loc2.lng);
} }
bool Location::same_alt_as(const Location &loc2) const
{
// fast path if the altitude frame is the same
if (this->get_alt_frame() == loc2.get_alt_frame()) {
return this->alt == loc2.alt;
}
ftype alt_diff;
bool have_diff = this->get_alt_distance(loc2, alt_diff);
const ftype tolerance = FLT_EPSILON;
return have_diff && (fabsF(alt_diff) < tolerance);
}
// return true when lat and lng are within range // return true when lat and lng are within range
bool Location::check_latlng() const bool Location::check_latlng() const
{ {

3
libraries/AP_Common/Location.h
View File

@ -107,6 +107,9 @@ public:
// check if lat and lng match. Ignore altitude and options // check if lat and lng match. Ignore altitude and options
bool same_latlon_as(const Location &loc2) const; bool same_latlon_as(const Location &loc2) const;
// check if altitude matches.
bool same_alt_as(const Location &loc2) const;
/* /*
* convert invalid waypoint with useful data. return true if location changed * convert invalid waypoint with useful data. return true if location changed
*/ */