From e7b3c00767531d2201152fa63d03cede6de2c8ef Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 15 May 2014 22:04:35 +0900 Subject: [PATCH] AC_WPNav: set_loiter_target uses set_xy_target Loiter is only a horizontal position controller so it should not set the z-axis position. Moved pos_control.set_speed and accel functions so order matches init_loiter_targets function order --- libraries/AC_WPNav/AC_WPNav.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index a69febda05..16416e7335 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -125,19 +125,17 @@ void AC_WPNav::set_loiter_target(const Vector3f& position, bool reset_I) // initialise position controller _pos_control.init_xy_controller(); + // initialise pos controller speed and acceleration + _pos_control.set_speed_xy(_loiter_speed_cms); + _loiter_accel_cms = _loiter_speed_cms/2.0f; + _pos_control.set_accel_xy(_loiter_accel_cms); + // set target position - _pos_control.set_pos_target(position); + _pos_control.set_xy_target(position.x, position.y); // initialise feed forward velocity to zero _pos_control.set_desired_velocity(0,0); - // initialise pos controller speed - _pos_control.set_speed_xy(_loiter_speed_cms); - - // initialise pos controller acceleration - _loiter_accel_cms = _loiter_speed_cms/2.0f; - _pos_control.set_accel_xy(_loiter_accel_cms); - // initialise desired accel and add fake wind _loiter_desired_accel.x = 0; _loiter_desired_accel.y = 0;