AC_WPNav: reset_I flag moved to position controller

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);
     }
 }
 

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)