AC_PI_2D: replace set_filt_hz method with filt_hz

Thanks to Jonathan Challinger for spotting this bug

libraries/AC_PID/AC_PI_2D.cpp

@@ -41,13 +41,10 @@ AC_PI_2D::AC_PI_2D(float initial_p, float initial_i, float initial_imax, float i
     _kp = initial_p;
     _ki = initial_i;
     _imax = fabs(initial_imax);
-    _filt_hz = initial_filt_hz;
+    filt_hz(initial_filt_hz);
 
     // reset input filter to first value received
     _flags._reset_filter = true;
-
-    // calculate the input filter alpha
-    calc_filt_alpha();
 }
 
 // set_dt - set time step in seconds
@@ -58,10 +55,14 @@ void AC_PI_2D::set_dt(float dt)
     calc_filt_alpha();
 }
 
-// set_filt_hz - set input filter hz
-void AC_PI_2D::set_filt_hz(float hz)
+// filt_hz - set input filter hz
+void AC_PI_2D::filt_hz(float hz)
 {
-    _filt_hz.set(hz);
+    _filt_hz.set(fabs(hz));
+
+    // sanity check _filt_hz
+    _filt_hz = max(_filt_hz, AC_PI_2D_FILT_HZ_MIN);
+
     // calculate the input filter alpha
     calc_filt_alpha();
 }
@@ -160,7 +161,7 @@ void AC_PI_2D::operator() (float p, float i, float imaxval, float input_filt_hz,
 
 // calc_filt_alpha - recalculate the input filter alpha
 void AC_PI_2D::calc_filt_alpha()
-{    
+{
     // calculate alpha
     float rc = 1/(2*PI*_filt_hz);
     _filt_alpha = _dt / (_dt + rc);

libraries/AC_PID/AC_PI_2D.h

@@ -12,6 +12,7 @@
 #include <math.h>
 
 #define AC_PI_2D_FILT_HZ_DEFAULT  20.0f   // default input filter frequency
+#define AC_PI_2D_FILT_HZ_MIN      0.01f   // minimum input filter frequency
 
 /// @class	AC_PI_2D
 /// @brief	Copter PID control class
@@ -24,9 +25,6 @@ public:
     // set_dt - set time step in seconds
     void        set_dt(float dt);
 
-    // set_filt_hz - set input filter hz
-    void        set_filt_hz(float hz);
-
     // set_input - set input to PID controller
     //  input is filtered before the PID controllers are run
     //  this should be called before any other calls to get_p, get_i or get_d
@@ -65,7 +63,7 @@ public:
     void        kP(const float v) { _kp.set(v); }
     void        kI(const float v) { _ki.set(v); }
     void        imax(const float v) { _imax.set(fabs(v)); }
-    void        filt_hz(const float v) { _filt_hz.set(fabs(v)); }
+    void        filt_hz(const float v);
 
     Vector2f    get_integrator() const { return _integrator; }
     void        set_integrator(const Vector2f &i) { _integrator = i; }