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

AP_Math: cope with co-located waypoints in location_passed_point()

This commit is contained in:
Andrew Tridgell 2012-07-04 14:24:04 +10:00
parent 7be29c8b04
commit 6cf2e2fa13
1 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Math/location.cpp
View File

@ -79,7 +79,15 @@ bool location_passed_point(struct Location &location,
Vector2f pt1(point1.lat, point1.lng);
Vector2f pt2(point2.lat, point2.lng);
float angle = (loc1 - pt2).angle(pt1 - pt2);
if (angle == 0) {
if (isinf(angle)) {
// two of the points are co-located.
// If location is equal to point2 then say we have passed the
// waypoint, otherwise say we haven't
if (get_distance(&location, &point2) == 0) {
return true;
}
return false;
} else if (angle == 0) {
// if we are exactly on the line between point1 and
// point2 then we are past the waypoint if the
// distance from location to point1 is greater then