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

AP_Common: move get_bearing_cd to Location and rename to get_bearing_to

This commit is contained in:
Pierre Kancir 2019-04-05 15:02:42 +02:00 committed by Peter Barker
parent 5ac19bc5a3
commit 6faf1d2849
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

@ -270,3 +270,16 @@ bool Location::sanitize(const Location &defaultLoc)
// make sure we know what size the Location object is: // make sure we know what size the Location object is:
assert_storage_size<Location, 16> _assert_storage_size_Location; assert_storage_size<Location, 16> _assert_storage_size_Location;
// return bearing in centi-degrees from location to loc2
int32_t Location::get_bearing_to(const struct Location &loc2) const
{
const int32_t off_x = loc2.lng - lng;
const int32_t off_y = (loc2.lat - lat) / loc2.longitude_scale();
int32_t bearing = 9000 + atan2f(-off_y, off_x) * DEGX100;
if (bearing < 0) {
bearing += 36000;
}
return bearing;
}

3
libraries/AP_Common/Location.h
View File

@ -85,6 +85,9 @@ public:
void zero(void); void zero(void);
// return bearing in centi-degrees from location to loc2
int32_t get_bearing_to(const struct 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
*/ */