From eb89b5bbb616d9a2e2fe07f43d73e8afaac33c00 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 18 Mar 2016 13:43:15 +1100 Subject: [PATCH] AP_L1_Control: prevent possibly fly-away when passed waypoint in L1 if the top level controller doesn't consider a waypoint complete when we are passed next_WP then it would keep flying away from the line segment. This doesn't happen with the current master code, but we want to ensure it is handled --- libraries/AP_L1_Control/AP_L1_Control.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index a5813bd882..30a10cc004 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -177,7 +177,8 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct // Calculate the NE position of WP B relative to WP A Vector2f AB = location_diff(prev_WP, next_WP); - + float AB_length = AB.length(); + // Check for AB zero length and track directly to the destination // if too small if (AB.length() < 1.0e-6f) { @@ -207,7 +208,15 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct ltrackVel = _groundspeed_vector * (-A_air_unit); // Velocity along line Nu = atan2f(xtrackVel,ltrackVel); _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point - + } else if (alongTrackDist > AB_length + groundSpeed*3) { + // we have passed point B by 3 seconds. Head towards B + // Calc Nu to fly To WP B + Vector2f B_air = location_diff(next_WP, _current_loc); + Vector2f B_air_unit = (B_air).normalized(); // Unit vector from WP B to aircraft + xtrackVel = _groundspeed_vector % (-B_air_unit); // Velocity across line + ltrackVel = _groundspeed_vector * (-B_air_unit); // Velocity along line + Nu = atan2f(xtrackVel,ltrackVel); + _nav_bearing = atan2f(-B_air_unit.y , -B_air_unit.x); // bearing (radians) from AC to L1 point } else { //Calc Nu to fly along AB line //Calculate Nu2 angle (angle of velocity vector relative to line connecting waypoints)