AC_WPNav: add reset_I to init_loiter_target

libraries/AC_WPNav/AC_WPNav.cpp

@@ -118,8 +118,11 @@ void AC_WPNav::set_loiter_target(const Vector3f& position)
 }
 
 /// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity
-void AC_WPNav::init_loiter_target()
+void AC_WPNav::init_loiter_target(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;
+
 	Vector3f curr_vel = _inav->get_velocity();
 
 	// set target position
@@ -250,7 +253,7 @@ void AC_WPNav::update_loiter()
         _pos_control.trigger_xy();
     }else{
         // run horizontal position controller
-        _pos_control.update_xy_controller(true);
+        _pos_control.update_xy_controller(true, _reset_I);
     }
 }
 

libraries/AC_WPNav/AC_WPNav.h

@@ -56,7 +56,7 @@ public:
     void set_loiter_target(const Vector3f& position);
 
     /// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity
-    void init_loiter_target();
+    void init_loiter_target(bool reset_I=true);
 
     /// set_loiter_velocity - allows main code to pass the maximum velocity for loiter
     void set_loiter_velocity(float velocity_cms);
@@ -235,6 +235,7 @@ 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)