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

APM: changed test for having completed a waypoint 

the new test is that we have passed a "finish line" perpendicular to
the track between the last waypoint and the current waypoint.

The previous tests are also still used, so if we circle a waypoint or
get within the waypoint radius we also consider it completed
This commit is contained in:
Andrew Tridgell 2012-07-04 09:21:34 +10:00
parent d9351ff681
commit 5623ef99a0
1 changed files with 15 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
ArduPlane/commands_logic.pde
View File

@ -357,15 +357,26 @@ static bool verify_nav_wp()
hold_course = -1;
update_crosstrack();
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
gcs_send_text_fmt(PSTR("Reached Waypoint #%i"),nav_command_index);
gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
(unsigned)nav_command_index,
(unsigned)get_distance(&current_loc, &next_WP));
return true;
}
// add in a more complex case
// Doug to do
if(loiter_sum > 300){
// have we circled around the waypoint?
if (loiter_sum > 300){
gcs_send_text_P(SEVERITY_MEDIUM,PSTR("Missed WP"));
return true;
}
// have we flown past the waypoint?
if (location_passed_point(current_loc, prev_WP, next_WP)) {
gcs_send_text_fmt(PSTR("Passed Waypoint #%i dist %um"),
(unsigned)nav_command_index,
(unsigned)get_distance(&current_loc, &next_WP));
return true;
}
return false;
}