From 4d5b73b968892e270a495f4d61d771b6a08303a8 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 23 Apr 2014 13:44:59 +0900
Subject: [PATCH] AC_WPNav: add reset_I to set_loiter_target

---
 libraries/AC_WPNav/AC_WPNav.cpp | 7 +++++--
 libraries/AC_WPNav/AC_WPNav.h   | 4 ++--
 2 files changed, 7 insertions(+), 4 deletions(-)

diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
index 21c09ad03b..7b55ca91f4 100644
--- a/libraries/AC_WPNav/AC_WPNav.cpp
+++ b/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);
diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h
index 7fc2b19d26..9e6b77acb2 100644
--- a/libraries/AC_WPNav/AC_WPNav.h
+++ b/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);