Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 23:43:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: use curr pos target as spline origin

Browse Source 
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);