From 7c02a02bd8c6031bbfabbf94618594544aea9ecb Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 23 Apr 2014 14:40:55 +0900 Subject: [PATCH] AC_WPNav: reset_I flag moved to position controller --- libraries/AC_WPNav/AC_WPNav.cpp | 13 ++++++------- libraries/AC_WPNav/AC_WPNav.h | 2 +- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 7b55ca91f4..32c97d734b 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -99,8 +99,10 @@ AC_WPNav::AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, AC_PosContro /// set_loiter_target in cm from home void AC_WPNav::set_loiter_target(const Vector3f& position, bool reset_I) { - // flag in case of loiter x_y I_term should not be reset => reset_I=false. default is true - _reset_I = reset_I; + // if reset_I is false we warn position controller not to reset I terms + if (!reset_I) { + _pos_control.keep_xy_I_terms(); + } // set target position _pos_control.set_pos_target(position); @@ -121,11 +123,8 @@ void AC_WPNav::set_loiter_target(const Vector3f& position, bool reset_I) } /// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity -void AC_WPNav::init_loiter_target(bool reset_I) +void AC_WPNav::init_loiter_target() { - // flag in case of loiter x_y I_term should not be reset => reset_I=false. default is true - _reset_I = reset_I; - Vector3f curr_vel = _inav->get_velocity(); // set target position @@ -256,7 +255,7 @@ void AC_WPNav::update_loiter() _pos_control.trigger_xy(); }else{ // run horizontal position controller - _pos_control.update_xy_controller(true, _reset_I); + _pos_control.update_xy_controller(true); } } diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 9e6b77acb2..885895309b 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -53,6 +53,7 @@ public: /// /// set_loiter_target in cm from home + /// caller can set reset_I to false to preserve I term since previous time loiter controller ran. Should only be false when caller is sure that not too much time has passed to invalidate the I terms void set_loiter_target(const Vector3f& position, bool reset_I=true); /// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity @@ -235,7 +236,6 @@ protected: AP_Float _wp_accel_cms; // acceleration in cm/s/s during missions // loiter controller internal variables - bool _reset_I; // if true, reset x_y_I_terms of loiter controller uint32_t _loiter_last_update; // time of last update_loiter call uint8_t _loiter_step; // used to decide which portion of loiter controller to run during this iteration int16_t _pilot_accel_fwd_cms; // pilot's desired acceleration forward (body-frame)