Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Andrew Tridgell 2016-03-18 13:43:15 +11:00
parent 7d92202db9
commit eb89b5bbb6
1 changed files with 11 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_L1_Control/AP_L1_Control.cpp
View File

@ -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)