AC_WPNav: add reset_I to set_loiter_target

libraries/AC_WPNav/AC_WPNav.cpp

@@ -97,10 +97,13 @@ 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)
+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;
+
     // set target position
-    _pos_control.set_pos_target(_inav->get_position());
+    _pos_control.set_pos_target(position);
 
     // initialise feed forward velocity to zero
     _pos_control.set_desired_velocity(0,0);

libraries/AC_WPNav/AC_WPNav.h

@@ -53,10 +53,10 @@ public:
     ///
 
     /// set_loiter_target in cm from home
-    void set_loiter_target(const Vector3f& position);
+    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
-    void init_loiter_target(bool reset_I=true);
+    void init_loiter_target();
 
     /// set_loiter_velocity - allows main code to pass the maximum velocity for loiter
     void set_loiter_velocity(float velocity_cms);