From 04822152dabb1f2a41be174737a7ea777f912407 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 22 Jan 2018 11:41:46 +0900 Subject: [PATCH] AC_PID: fixes after peer review of AC_PID_2D --- libraries/AC_PID/AC_PID_2D.cpp | 30 ++++++++++-------------------- libraries/AC_PID/AC_PID_2D.h | 10 +--------- 2 files changed, 11 insertions(+), 29 deletions(-) diff --git a/libraries/AC_PID/AC_PID_2D.cpp b/libraries/AC_PID/AC_PID_2D.cpp index 3660299747..325fa2948b 100644 --- a/libraries/AC_PID/AC_PID_2D.cpp +++ b/libraries/AC_PID/AC_PID_2D.cpp @@ -4,6 +4,11 @@ #include #include "AC_PID_2D.h" +#define AC_PID_2D_FILT_HZ_DEFAULT 20.0f // default input filter frequency +#define AC_PID_2D_FILT_HZ_MIN 0.01f // minimum input filter frequency +#define AC_PID_2D_FILT_D_HZ_DEFAULT 10.0f // default input filter frequency +#define AC_PID_2D_FILT_D_HZ_MIN 0.005f // minimum input filter frequency + const AP_Param::GroupInfo AC_PID_2D::var_info[] = { // @Param: P // @DisplayName: PID Proportional Gain @@ -109,7 +114,7 @@ void AC_PID_2D::set_input(const Vector2f &input) // update filter and calculate derivative const Vector2f input_delta = (input - _input) * _filt_alpha; - _input = _input + input_delta; + _input += input_delta; set_input_filter_d(input_delta); } @@ -117,14 +122,14 @@ void AC_PID_2D::set_input(const Vector2f &input) // set_input_filter_d - set input to PID controller // only input to the D portion of the controller is filtered // this should be called before any other calls to get_p, get_i or get_d -void AC_PID_2D::set_input_filter_d(Vector2f input_delta) +void AC_PID_2D::set_input_filter_d(const Vector2f& input_delta) { // don't process inf or NaN if (!isfinite(input_delta.x) && !isfinite(input_delta.y)) { return; } - // reset input filter to value received + // reset input filter if (_flags._reset_filter) { _flags._reset_filter = false; _derivative.x = 0.0f; @@ -133,8 +138,8 @@ void AC_PID_2D::set_input_filter_d(Vector2f input_delta) // update filter and calculate derivative if (is_positive(_dt)) { - Vector2f derivative = input_delta / _dt; - Vector2f delta_derivative = (derivative - _derivative) * _filt_alpha_d; + const Vector2f derivative = input_delta / _dt; + const Vector2f delta_derivative = (derivative - _derivative) * _filt_alpha_d; _derivative += delta_derivative; } } @@ -214,21 +219,6 @@ void AC_PID_2D::save_gains() _filt_d_hz.save(); } -/// Overload the function call operator to permit easy initialisation -void AC_PID_2D::operator() (float p, float i, float imaxval, float input_filt_hz, float input_filt_d_hz, float dt) -{ - _kp = p; - _ki = i; - _kd = i; - _imax = fabsf(imaxval); - _filt_hz = input_filt_hz; - _filt_d_hz = input_filt_d_hz; - _dt = dt; - // calculate the input filter alpha - calc_filt_alpha(); - calc_filt_alpha_d(); -} - // calc_filt_alpha - recalculate the input filter alpha void AC_PID_2D::calc_filt_alpha() { diff --git a/libraries/AC_PID/AC_PID_2D.h b/libraries/AC_PID/AC_PID_2D.h index a095cb2e05..2ad36569f7 100644 --- a/libraries/AC_PID/AC_PID_2D.h +++ b/libraries/AC_PID/AC_PID_2D.h @@ -8,11 +8,6 @@ #include #include -#define AC_PID_2D_FILT_HZ_DEFAULT 20.0f // default input filter frequency -#define AC_PID_2D_FILT_HZ_MIN 0.01f // minimum input filter frequency -#define AC_PID_2D_FILT_D_HZ_DEFAULT 10.0f // default input filter frequency -#define AC_PID_2D_FILT_D_HZ_MIN 0.005f // minimum input filter frequency - /// @class AC_PID_2D /// @brief Copter PID control class class AC_PID_2D { @@ -49,9 +44,6 @@ public: // save gain to eeprom void save_gains(); - /// operator function call for easy initialisation - void operator() (float p, float i, float imaxval, float input_filt_hz, float input_filt_D_hz, float dt); - // get accessors AP_Float &kP() { return _kp; } AP_Float &kI() { return _ki; } @@ -81,7 +73,7 @@ protected: // set_input_filter_d - set input to PID controller // only input to the D portion of the controller is filtered // this should be called before any other calls to get_p, get_i or get_d - void set_input_filter_d(Vector2f input_delta); + void set_input_filter_d(const Vector2f& input_delta); // calc_filt_alpha - recalculate the input filter alpha void calc_filt_alpha();