aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

APM: fixed a problem with wayoints completing early

Browse Source 
when a user forces a waypoint jump, we may need to reset prev_WP to
prevent us thinking the waypoint is complete before we have started
This commit is contained in:
Andrew Tridgell 2012-07-21 20:32:07 +10:00
parent 175b6d2606
commit 9e72d4c3f4
1 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduPlane/commands.pde
View File

@ -143,6 +143,16 @@ static void set_next_WP(struct Location *wp)
// ---------------------
next_WP = *wp;
// are we already past the waypoint? This happens when we jump
// waypoints, and it can cause us to skip a waypoint. If we are
// past the waypoint when we start on a leg, then use the current
// location as the previous waypoint, to prevent immediately
// considering the waypoint complete
if (location_passed_point(current_loc, prev_WP, next_WP)) {
gcs_send_text_P(SEVERITY_LOW, PSTR("Resetting prev_WP"));
prev_WP = current_loc;
}
// used to control FBW and limit the rate of climb
// -----------------------------------------------
target_altitude = current_loc.alt;