From 9ecbd0e30f7c0d349f67fe6ba51ee79671f81a96 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Wed, 8 Apr 2015 15:21:02 -0700 Subject: [PATCH] AP_L1_Control: fixed uTurn-wobble When performing a u-turn in AUTO (waypoints are 180deg turn from each other) sometimes the aircraft can't decide to turn left or right and wobbled back and forth a couple times. There was existing code to solve this but it was not executing all the time like when in LOITER mode. Frankly, I don't understand the criteria that was required to make it execute and i suspect there is still a gremlin in that logic but just executing the check all the time makes prevents the wobble behavior. --- libraries/AP_L1_Control/AP_L1_Control.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index d770e09f26..b2388c9ee4 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -193,9 +193,6 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct xtrackVel = _groundspeed_vector % (-A_air_unit); // Velocity across line ltrackVel = _groundspeed_vector * (-A_air_unit); // Velocity along line Nu = atan2f(xtrackVel,ltrackVel); - - _prevent_indecision(Nu); - _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point } else { //Calc Nu to fly along AB line @@ -214,6 +211,7 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct _nav_bearing = atan2f(AB.y, AB.x) + Nu1; // bearing (radians) from AC to L1 point } + _prevent_indecision(Nu); _last_Nu = Nu; //Limit Nu to +-pi