From 6ea91d012eb187c682fc90aa27fbf54a9db3bfeb Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Fri, 27 Mar 2015 17:19:24 -0700
Subject: [PATCH] AC_PID: calculate filt_alpha every time the filter is run

---
 libraries/AC_PID/AC_PID.cpp | 23 ++++++++---------------
 libraries/AC_PID/AC_PID.h   |  6 +-----
 2 files changed, 9 insertions(+), 20 deletions(-)

diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp
index 57edf745e6..6b20cb9616 100644
--- a/libraries/AC_PID/AC_PID.cpp
+++ b/libraries/AC_PID/AC_PID.cpp
@@ -64,7 +64,6 @@ void AC_PID::set_dt(float dt)
 {
     // set dt and calculate the input filter alpha
     _dt = dt;
-    calc_filt_alpha();
 }
 
 // filt_hz - set input filter hz
@@ -74,9 +73,6 @@ void AC_PID::filt_hz(float hz)
 
     // sanity check _filt_hz
     _filt_hz = max(_filt_hz, AC_PID_FILT_HZ_MIN);
-
-    // calculate the input filter alpha
-    calc_filt_alpha();
 }
 
 // set_input_filter_all - set input to PID controller
@@ -97,7 +93,7 @@ void AC_PID::set_input_filter_all(float input)
     }
 
     // update filter and calculate derivative
-    float input_filt_change = _filt_alpha * (input - _input);
+    float input_filt_change = get_filt_alpha() * (input - _input);
     _input = _input + input_filt_change;
     if (_dt > 0.0f) {
         _derivative = input_filt_change / _dt;
@@ -123,7 +119,7 @@ void AC_PID::set_input_filter_d(float input)
     // update filter and calculate derivative
     if (_dt > 0.0f) {
         float derivative = (input - _input) / _dt;
-        _derivative = _derivative + _filt_alpha * (derivative-_derivative);
+        _derivative = _derivative + get_filt_alpha() * (derivative-_derivative);
     }
 
     _input = input;
@@ -159,13 +155,11 @@ float AC_PID::get_pi()
     return get_p() + get_i();
 }
 
-
 float AC_PID::get_pid()
 {
     return get_p() + get_i() + get_d();
 }
 
-
 void AC_PID::reset_I()
 {
     _integrator = 0;
@@ -179,9 +173,6 @@ void AC_PID::load_gains()
     _imax.load();
     _imax = fabs(_imax);
     _filt_hz.load();
-
-    // calculate the input filter alpha
-    calc_filt_alpha();
 }
 
 // save_gains - save gains to eeprom
@@ -203,14 +194,16 @@ void AC_PID::operator() (float p, float i, float d, float imaxval, float input_f
     _imax = fabs(imaxval);
     _filt_hz = input_filt_hz;
     _dt = dt;
-    // calculate the input filter alpha
-    calc_filt_alpha();
 }
 
 // calc_filt_alpha - recalculate the input filter alpha
-void AC_PID::calc_filt_alpha()
+float AC_PID::get_filt_alpha() const
 {
+    if (_filt_hz == 0.0f) {
+        return 1.0f;
+    }
+
     // calculate alpha
     float rc = 1/(2*PI*_filt_hz);
-    _filt_alpha = _dt / (_dt + rc);
+    return _dt / (_dt + rc);
 }
diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h
index 8b91a2744d..2fdec1d745 100644
--- a/libraries/AC_PID/AC_PID.h
+++ b/libraries/AC_PID/AC_PID.h
@@ -63,7 +63,7 @@ public:
     float       kD() const { return _kd.get(); }
     float       imax() const { return _imax.get(); }
     float       filt_hz() const { return _filt_hz.get(); }
-    float       get_filt_alpha() const { return _filt_alpha; }
+    float       get_filt_alpha() const;
 
     // set accessors
     void        kP(const float v) { _kp.set(v); }
@@ -80,9 +80,6 @@ public:
 
 protected:
 
-    // calc_filt_alpha - recalculate the input filter alpha
-    void        calc_filt_alpha();
-
     // parameters
     AP_Float        _kp;
     AP_Float        _ki;
@@ -100,7 +97,6 @@ protected:
     float           _integrator;                // integrator value
     float           _input;                // last input for derivative
     float           _derivative;           // last derivative for low-pass filter
-    float           _filt_alpha;          // input filter alpha
 };
 
 #endif // __AC_PID_H__