From 7706102d1ee4514b4302306b0e7b27855caa0e52 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 16 Feb 2015 19:44:04 +0900
Subject: [PATCH] AC_WPNav: add shift_loiter_target method

Shift the loiter target and freeze the feedforward if necessary
---
 libraries/AC_WPNav/AC_WPNav.cpp | 15 +++++++++++++++
 libraries/AC_WPNav/AC_WPNav.h   |  7 +++++++
 2 files changed, 22 insertions(+)

diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
index a4c21d509c..93bc9e200f 100644
--- a/libraries/AC_WPNav/AC_WPNav.cpp
+++ b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -166,6 +166,21 @@ void AC_WPNav::init_loiter_target(const Vector3f& position, bool reset_I)
     _pilot_accel_rgt_cms = 0;
 }
 
+/// shift_loiter_target - shifts the loiter target by the given pos_adjustment
+///     used by precision landing to adjust horizontal position target
+void AC_WPNav::shift_loiter_target(const Vector3f &pos_adjustment)
+{
+    Vector3f new_target = _pos_control.get_pos_target() + pos_adjustment;
+
+    // move pos controller target
+    _pos_control.set_xy_target(new_target.x, new_target.y);
+
+    // disable feed forward
+    if (fabsf(pos_adjustment.x) > 0.0f || fabsf(pos_adjustment.y) > 0.0f) {
+        _pos_control.freeze_ff_xy();
+    }
+}
+
 /// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity
 void AC_WPNav::init_loiter_target()
 {
diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h
index 0d2e225921..c49f23afea 100644
--- a/libraries/AC_WPNav/AC_WPNav.h
+++ b/libraries/AC_WPNav/AC_WPNav.h
@@ -64,6 +64,10 @@ public:
     /// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity
     void init_loiter_target();
 
+    /// shift_loiter_target - shifts the loiter target by the given pos_adjustment
+    ///     used by precision landing to adjust horizontal position target
+    void shift_loiter_target(const Vector3f &pos_adjustment);
+
     /// loiter_soften_for_landing - reduce response for landing
     void loiter_soften_for_landing();
 
@@ -85,6 +89,9 @@ public:
     /// get_loiter_bearing_to_target - get bearing to loiter target in centi-degrees
     int32_t get_loiter_bearing_to_target() const;
 
+    /// get_loiter_target - returns loiter target position
+    const Vector3f& get_loiter_target() const { return _pos_control.get_pos_target(); }
+
     /// update_loiter - run the loiter controller - should be called at 10hz
     void update_loiter(float ekfGndSpdLimit, float ekfNavVelGainScaler);