From fc427967ae026d44684e941be4757bf5eb292909 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 25 Jan 2014 17:23:27 +0900 Subject: [PATCH] AC_WPNav: set wp origin to horiz and vert stopping point Also bug fix to set pos_control's down speed as a negative number --- libraries/AC_WPNav/AC_WPNav.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 4f8beb774a..d98d7388ed 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -247,6 +247,7 @@ void AC_WPNav::set_wp_destination(const Vector3f& destination) }else{ // otherwise calculate origin from the current position and velocity _pos_control.get_stopping_point_xy(_origin); + _pos_control.get_stopping_point_z(_origin); } // set origin and destination @@ -276,7 +277,7 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto // initialise position controller speed and acceleration _pos_control.set_speed_xy(_wp_speed_cms); _pos_control.set_accel_xy(_wp_accel_cms); - _pos_control.set_speed_z(_wp_speed_down_cms, _wp_speed_up_cms); + _pos_control.set_speed_z(-_wp_speed_down_cms, _wp_speed_up_cms); _pos_control.calc_leash_length_xy(); _pos_control.calc_leash_length_z();