From c66af9b7880656c6b0f2598fd04fd84ce587d7c2 Mon Sep 17 00:00:00 2001
From: Robert Lefebvre <robert.lefebvre@gmail.com>
Date: Wed, 23 Sep 2015 19:27:26 -0400
Subject: [PATCH] AC_HELI_PID: Add Leak-Min param and functionality.

---
 libraries/AC_PID/AC_HELI_PID.cpp | 19 +++++++++++++++++--
 libraries/AC_PID/AC_HELI_PID.h   |  3 +++
 2 files changed, 20 insertions(+), 2 deletions(-)

diff --git a/libraries/AC_PID/AC_HELI_PID.cpp b/libraries/AC_PID/AC_HELI_PID.cpp
index 0ea31c1151..dbe84fdb2c 100644
--- a/libraries/AC_PID/AC_HELI_PID.cpp
+++ b/libraries/AC_PID/AC_HELI_PID.cpp
@@ -37,10 +37,18 @@ const AP_Param::GroupInfo AC_HELI_PID::var_info[] = {
     // @Description:
     AP_GROUPINFO("FILT_HZ", 6, AC_HELI_PID, _filt_hz, AC_PID_FILT_HZ_DEFAULT),
 
+    // @Param: I_L_MIN
+    // @DisplayName: I-term Leak Minimum
+    // @Description: Point below which I-term will not leak down
+    // @Range: 0 4500
+    // @User: Advanced
+    AP_GROUPINFO("I_L_MIN", 7, AC_HELI_PID, _leak_min, AC_PID_LEAK_MIN),
+
     // @Param: AFF
     // @DisplayName: Acceleration FF FeedForward Gain
     // @Description: Acceleration FF Gain which produces an output value that is proportional to the change in demanded input
-    AP_GROUPINFO("AFF",    7, AC_HELI_PID, _aff, 0),
+    AP_GROUPINFO("AFF",    8, AC_HELI_PID, _aff, 0),
+
     AP_GROUPEND
 };
 
@@ -81,7 +89,14 @@ float AC_HELI_PID::get_aff(float requested_rate)
 float AC_HELI_PID::get_leaky_i(float leak_rate)
 {
     if(!is_zero(_ki) && !is_zero(_dt)){
-        _integrator -= (float)_integrator * leak_rate;
+
+        // integrator does not leak down below Leak Min
+        if (_integrator > _leak_min){
+            _integrator -= (float)(_integrator - _leak_min) * leak_rate;
+        } else if (_integrator < -_leak_min) {
+            _integrator -= (float)(_integrator + _leak_min) * leak_rate;
+        }
+
         _integrator += ((float)_input * _ki) * _dt;
         if (_integrator < -_imax) {
             _integrator = -_imax;
diff --git a/libraries/AC_PID/AC_HELI_PID.h b/libraries/AC_PID/AC_HELI_PID.h
index c93dfec036..13df482104 100644
--- a/libraries/AC_PID/AC_HELI_PID.h
+++ b/libraries/AC_PID/AC_HELI_PID.h
@@ -12,6 +12,8 @@
 #include <math.h>
 #include "AC_PID.h"
 
+#define AC_PID_LEAK_MIN     500.0  // Default I-term Leak Minimum
+
 /// @class	AC_HELI_PID
 /// @brief	Heli PID control class
 class AC_HELI_PID : public AC_PID {
@@ -37,6 +39,7 @@ public:
     
 private:
     AP_Float        _vff;
+    AP_Float        _leak_min;
     AP_Float        _aff;
 
     float           _last_requested_rate;       // Requested rate from last iteration, used to calculate rate change of requested rate