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

APM: changed test for having completed a waypoint

Browse Source 
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; hold_course = -1;
update_crosstrack(); update_crosstrack();
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) { 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; return true;
} }
// add in a more complex case
// Doug to do // have we circled around the waypoint?
if(loiter_sum > 300){ if (loiter_sum > 300){
gcs_send_text_P(SEVERITY_MEDIUM,PSTR("Missed WP")); gcs_send_text_P(SEVERITY_MEDIUM,PSTR("Missed WP"));
return true; 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; return false;
} }