aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_WPNav: set_wp_destination to use current target

Browse Source 
Previously we used the projected stopping point if the vehicle had not
reached the destination but this could lead to large jumps in target
position
This commit is contained in:
Randy Mackay 2014-04-08 23:01:46 +09:00
parent d30c4ffe80
commit 60f522a094
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -248,11 +248,11 @@ void AC_WPNav::set_wp_destination(const Vector3f& destination)
{
Vector3f origin;
// if waypoint controller is active and copter has reached the previous waypoint use it for the origin
if( _flags.reached_destination && ((hal.scheduler->millis() - _wp_last_update) < 1000) ) {
origin = _destination;
}else{
// otherwise calculate origin from the current position and velocity
// if waypoint controller is active use the existing position target as the origin
if ((hal.scheduler->millis() - _wp_last_update) < 1000) {
origin = _pos_control.get_pos_target();
} else {
// if waypoint controller is not active, set origin to reasonable stopping point (using curr pos and velocity)
_pos_control.get_stopping_point_xy(origin);
_pos_control.get_stopping_point_z(origin);
}