From ee820258ad64ac309fce26fc1bb329ed79bc1a9c Mon Sep 17 00:00:00 2001
From: Leonard Hall <leonardthall@gmail.com>
Date: Thu, 27 Jun 2019 19:03:49 +0930
Subject: [PATCH] AC_HELI_PID: support for upgrade to PID object

updates parameters based on new PID library
rename get_leaky_i to update_leaky_i and move FILT locations
---
 libraries/AC_PID/AC_HELI_PID.cpp | 46 +++++++++++++++++++-------------
 libraries/AC_PID/AC_HELI_PID.h   |  6 ++---
 2 files changed, 30 insertions(+), 22 deletions(-)

diff --git a/libraries/AC_PID/AC_HELI_PID.cpp b/libraries/AC_PID/AC_HELI_PID.cpp
index 0480703075..4a5db6ef34 100644
--- a/libraries/AC_PID/AC_HELI_PID.cpp
+++ b/libraries/AC_PID/AC_HELI_PID.cpp
@@ -20,20 +20,19 @@ const AP_Param::GroupInfo AC_HELI_PID::var_info[] = {
     // @Description: D Gain which produces an output that is proportional to the rate of change of the error
     AP_GROUPINFO("D",    2, AC_HELI_PID, _kd, 0),
 
+    // 3 was for uint16 IMAX
+
     // @Param: VFF
     // @DisplayName: Velocity FF FeedForward Gain
     // @Description: Velocity FF Gain which produces an output value that is proportional to the demanded input
-    AP_GROUPINFO("VFF",    4, AC_HELI_PID, _ff, 0),
+    AP_GROUPINFO("VFF",    4, AC_HELI_PID, _kff, 0),
 
     // @Param: IMAX
     // @DisplayName: PID Integral Maximum
     // @Description: The maximum/minimum value that the I term can output
-    AP_GROUPINFO("IMAX", 5, AC_HELI_PID, _imax, 0),
+    AP_GROUPINFO("IMAX", 5, AC_HELI_PID, _kimax, 0),
 
-    // @Param: FILT
-    // @DisplayName: PID Input filter frequency in Hz
-    // @Description: PID Input filter frequency in Hz
-    AP_GROUPINFO("FILT", 6, AC_HELI_PID, _filt_hz, AC_PID_FILT_HZ_DEFAULT),
+    // 6 was for float FILT
 
     // @Param: ILMI
     // @DisplayName: I-term Leak Minimum
@@ -42,14 +41,32 @@ const AP_Param::GroupInfo AC_HELI_PID::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("ILMI", 7, AC_HELI_PID, _leak_min, AC_PID_LEAK_MIN),
 
-    // index 8 was for AFF, now removed
+    // 8 was for float AFF
+
+    // @Param: FLTT
+    // @DisplayName: PID Target filter frequency in Hz
+    // @Description: Target filter frequency in Hz
+    // @Units: Hz
+    AP_GROUPINFO("FLTT", 9, AC_HELI_PID, _filt_T_hz, AC_PID_TFILT_HZ_DEFAULT),
+
+    // @Param: FLTE
+    // @DisplayName: PID Error filter frequency in Hz
+    // @Description: Error filter frequency in Hz
+    // @Units: Hz
+    AP_GROUPINFO("FLTE", 10, AC_HELI_PID, _filt_E_hz, AC_PID_EFILT_HZ_DEFAULT),
+
+    // @Param: FLTD
+    // @DisplayName: PID D term filter frequency in Hz
+    // @Description: Derivative filter frequency in Hz
+    // @Units: Hz
+    AP_GROUPINFO("FLTD", 11, AC_HELI_PID, _filt_D_hz, AC_PID_DFILT_HZ_DEFAULT),
 
     AP_GROUPEND
 };
 
 /// Constructor for PID
-AC_HELI_PID::AC_HELI_PID(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float dt, float initial_ff) :
-    AC_PID(initial_p, initial_i, initial_d, initial_imax, initial_filt_hz, dt, initial_ff)
+AC_HELI_PID::AC_HELI_PID(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_T_hz, float initial_filt_E_hz, float initial_filt_D_hz, float dt) :
+    AC_PID(initial_p, initial_i, initial_d, initial_ff, initial_imax, initial_filt_T_hz, initial_filt_E_hz, initial_filt_D_hz, dt)
 {
     _last_requested_rate = 0;
 }
@@ -57,7 +74,7 @@ AC_HELI_PID::AC_HELI_PID(float initial_p, float initial_i, float initial_d, floa
 // This is an integrator which tends to decay to zero naturally
 // if the error is zero.
 
-float AC_HELI_PID::get_leaky_i(float leak_rate)
+void AC_HELI_PID::update_leaky_i(float leak_rate)
 {
     if(!is_zero(_ki) && !is_zero(_dt)){
 
@@ -68,15 +85,6 @@ float AC_HELI_PID::get_leaky_i(float leak_rate)
             _integrator -= (float)(_integrator + _leak_min) * leak_rate;
         }
 
-        _integrator += ((float)_input * _ki) * _dt;
-        if (_integrator < -_imax) {
-            _integrator = -_imax;
-            } else if (_integrator > _imax) {
-            _integrator = _imax;
-        }
-
         _pid_info.I = _integrator;
-        return _integrator;
     }
-    return 0;
 }
diff --git a/libraries/AC_PID/AC_HELI_PID.h b/libraries/AC_PID/AC_HELI_PID.h
index af2ab4d549..134c519c4d 100644
--- a/libraries/AC_PID/AC_HELI_PID.h
+++ b/libraries/AC_PID/AC_HELI_PID.h
@@ -17,10 +17,10 @@ class AC_HELI_PID : public AC_PID {
 public:
 
     /// Constructor for PID
-    AC_HELI_PID(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float dt, float initial_ff);
+    AC_HELI_PID(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_T_hz, float initial_filt_E_hz, float initial_filt_D_hz, float dt);
 
-    /// get_leaky_i - replacement for get_i but output is leaded at leak_rate
-    float       get_leaky_i(float leak_rate);
+    /// update_leaky_i - replacement for get_i but output is leaded at leak_rate
+    void       update_leaky_i(float leak_rate);
 
     static const struct AP_Param::GroupInfo        var_info[];