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

AP_Math: add location sanity checker/fixer util

Browse Source
This commit is contained in:
Tom Pittenger 2016-03-02 08:43:18 -08:00
parent be3941efdf
commit 278fb2e60d
2 changed files with 27 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_Math/location.cpp
View File

@ -240,6 +240,28 @@ bool locations_are_same(const struct Location &loc1, const struct Location &loc2
return (loc1.lat == loc2.lat) && (loc1.lng == loc2.lng);
}
/*
* convert invalid waypoint with useful data. return true if location changed
*/
bool location_sanitize(const struct Location &defaultLoc, struct Location &loc)
{
bool has_changed = false;
// convert lat/lng=0 to mean current point
if (loc.lat == 0 && loc.lng == 0) {
loc.lat = defaultLoc.lat;
loc.lng = defaultLoc.lng;
has_changed = true;
}
// convert relative alt=0 to mean current alt
if (loc.alt == 0 && loc.flags.relative_alt) {
loc.flags.relative_alt = false;
loc.alt = defaultLoc.alt;
has_changed = true;
}
return has_changed;
}
/*
print a int32_t lat/long in decimal degrees
*/

5
libraries/AP_Math/location.h
View File

@ -59,6 +59,11 @@ Vector2f location_diff(const struct Location &loc1, const struct Location &lo
*/
bool locations_are_same(const struct Location &loc1, const struct Location &loc2);
/*
* convert invalid waypoint with useful data. return true if location changed
*/
bool location_sanitize(const struct Location &defaultLoc, struct Location &loc);
/*
print a int32_t lat/long in decimal degrees
*/