From fb668994140108cd5deaf4660a04edba604c6237 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 4 Jul 2012 09:21:34 +1000 Subject: [PATCH] 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 --- ArduPlane/commands_logic.pde | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index fcde508e03..25305dda75 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -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(¤t_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(¤t_loc, &next_WP)); + return true; + } + return false; }