From c7c3dd561aedaa7db236dda1108fef14fe30c0cb Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 13 Jul 2017 20:45:24 +0900 Subject: [PATCH] AC_WPNav: brake, wp and spline clear desired velocity_xy --- libraries/AC_WPNav/AC_WPNav.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 65a1aa4d76..6e86c9fde2 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -347,6 +347,7 @@ void AC_WPNav::init_brake_target(float accel_cmss) Vector3f stopping_point; // initialise position controller + _pos_control.set_desired_accel_xy(0.0f,0.0f); _pos_control.init_xy_controller(); // initialise pos controller speed and acceleration @@ -391,9 +392,13 @@ void AC_WPNav::wp_and_spline_init() } // initialise position controller + _pos_control.set_desired_accel_xy(0.0f,0.0f); _pos_control.init_xy_controller(); _pos_control.clear_desired_velocity_ff_z(); + // initialise feed forward velocity to zero + _pos_control.set_desired_velocity_xy(0.0f, 0.0f); + // initialise position controller speed and acceleration _pos_control.set_speed_xy(_wp_speed_cms); _pos_control.set_accel_xy(_wp_accel_cms); @@ -547,7 +552,6 @@ void AC_WPNav::shift_wp_origin_to_current_pos() // move pos controller target and disable feed forward _pos_control.set_pos_target(curr_pos); - _pos_control.freeze_ff_xy(); _pos_control.freeze_ff_z(); } @@ -763,7 +767,6 @@ bool AC_WPNav::update_wpnav() // TODO: why always consider Z axis discontinuous? if (_flags.new_wp_destination) { _flags.new_wp_destination = false; - _pos_control.freeze_ff_xy(); } _pos_control.freeze_ff_z(); @@ -1055,7 +1058,6 @@ bool AC_WPNav::update_spline() // TODO: why always consider Z axis discontinuous? if (_flags.new_wp_destination) { _flags.new_wp_destination = false; - _pos_control.freeze_ff_xy(); } _pos_control.freeze_ff_z();