Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_L1_Control: fixed uTurn-wobble

Browse Source 
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.
This commit is contained in:
Tom Pittenger 2015-04-08 15:21:02 -07:00
parent 6e8008ab69
commit 9ecbd0e30f
1 changed files with 1 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_L1_Control/AP_L1_Control.cpp
View File

@ -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 xtrackVel = _groundspeed_vector % (-A_air_unit); // Velocity across line
ltrackVel = _groundspeed_vector * (-A_air_unit); // Velocity along line ltrackVel = _groundspeed_vector * (-A_air_unit); // Velocity along line
Nu = atan2f(xtrackVel,ltrackVel); 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 _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 } 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 _nav_bearing = atan2f(AB.y, AB.x) + Nu1; // bearing (radians) from AC to L1 point
} }
_prevent_indecision(Nu);
_last_Nu = Nu; _last_Nu = Nu;
//Limit Nu to +-pi //Limit Nu to +-pi