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

AC_WPNav: use curr pos target as spline origin 

We only use the current target position as origin if the waypoint
controller is active (i..e has been used in the past 1 second).  This is
consistent with how we initialise straight line waypoints
This commit is contained in:
Randy Mackay 2014-06-09 15:42:17 +09:00
parent 130eb07d48
commit ce85d1f6b2
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -635,9 +635,9 @@ void AC_WPNav::set_spline_destination(const Vector3f& destination, bool stopped_
{
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;
// if waypoint controller is active and copter has reached the previous waypoint use current pos target as the origin
if ((hal.scheduler->millis() - _wp_last_update) < 1000) {
origin = _pos_control.get_pos_target();
}else{
// otherwise calculate origin from the current position and velocity
_pos_control.get_stopping_point_xy(origin);