From ab4b545bb5f3d579c1ecc019a2a6d4839f53edf0 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 19 Sep 2014 15:39:08 +0900
Subject: [PATCH] AC_WPNav: add loiter_soften_for_landing method

This resets the position target to the current location.
---
 libraries/AC_WPNav/AC_WPNav.cpp | 10 ++++++++++
 libraries/AC_WPNav/AC_WPNav.h   |  3 +++
 2 files changed, 13 insertions(+)

diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
index 16f46d23db..1fa531be4a 100644
--- a/libraries/AC_WPNav/AC_WPNav.cpp
+++ b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -176,6 +176,16 @@ void AC_WPNav::init_loiter_target()
     _pilot_accel_rgt_cms = 0;
 }
 
+/// loiter_soften_for_landing - reduce response for landing
+void AC_WPNav::loiter_soften_for_landing()
+{
+    const Vector3f& curr_pos = _inav.get_position();
+
+    // set target position to current position
+    _pos_control.set_xy_target(curr_pos.x, curr_pos.y);
+    _pos_control.freeze_ff_xy();
+}
+
 /// set_loiter_velocity - allows main code to pass the maximum velocity for loiter
 void AC_WPNav::set_loiter_velocity(float velocity_cms)
 {
diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h
index 0342889f42..985f1206fa 100644
--- a/libraries/AC_WPNav/AC_WPNav.h
+++ b/libraries/AC_WPNav/AC_WPNav.h
@@ -69,6 +69,9 @@ public:
     /// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity
     void init_loiter_target();
 
+    /// loiter_soften_for_landing - reduce response for landing
+    void loiter_soften_for_landing();
+
     /// set_loiter_velocity - allows main code to pass the maximum velocity for loiter
     void set_loiter_velocity(float velocity_cms);