From cc0d5b9ced728c0790b98a63dfe35343f5adbf02 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 11 Mar 2015 14:17:01 +0900 Subject: [PATCH] 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 | 17 +++++++++-------- libraries/AC_PID/AC_PI_2D.h | 6 ++---- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/libraries/AC_PID/AC_PI_2D.cpp b/libraries/AC_PID/AC_PI_2D.cpp index 983bb93541..4be57f3e53 100644 --- a/libraries/AC_PID/AC_PI_2D.cpp +++ b/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); diff --git a/libraries/AC_PID/AC_PI_2D.h b/libraries/AC_PID/AC_PI_2D.h index 6829c1dcad..eba98f9c6f 100644 --- a/libraries/AC_PID/AC_PI_2D.h +++ b/libraries/AC_PID/AC_PI_2D.h @@ -12,6 +12,7 @@ #include #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; }