From fbc5af270592ba39780046663538b7a6ab83846e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 14 Feb 2014 15:52:24 +0900 Subject: [PATCH] AC_PID: results returned as float --- libraries/AC_PID/AC_PID.cpp | 31 ++++++---------- libraries/AC_PID/AC_PID.h | 70 ++++++++++++++----------------------- 2 files changed, 36 insertions(+), 65 deletions(-) diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp index 4beb52fa9d..fa4c385fc4 100644 --- a/libraries/AC_PID/AC_PID.cpp +++ b/libraries/AC_PID/AC_PID.cpp @@ -6,14 +6,6 @@ #include #include "AC_PID.h" -// Examples for _filter: -// f_cut = 10 Hz -> _filter = 15.9155e-3 -// f_cut = 15 Hz -> _filter = 10.6103e-3 -// f_cut = 20 Hz -> _filter = 7.9577e-3 -// f_cut = 25 Hz -> _filter = 6.3662e-3 -// f_cut = 30 Hz -> _filter = 5.3052e-3 -const float AC_PID::_filter = 7.9577e-3; // Set to "1 / ( 2 * PI * f_cut )"; - const AP_Param::GroupInfo AC_PID::var_info[] PROGMEM = { // @Param: P // @DisplayName: PID Proportional Gain @@ -34,12 +26,12 @@ const AP_Param::GroupInfo AC_PID::var_info[] PROGMEM = { AP_GROUPEND }; -int32_t AC_PID::get_p(int32_t error) +float AC_PID::get_p(float error) const { return (float)error * _kp; } -int32_t AC_PID::get_i(int32_t error, float dt) +float AC_PID::get_i(float error, float dt) { if((_ki != 0) && (dt != 0)) { _integrator += ((float)error * _ki) * dt; @@ -56,7 +48,7 @@ int32_t AC_PID::get_i(int32_t error, float dt) // This is an integrator which tends to decay to zero naturally // if the error is zero. -int32_t AC_PID::get_leaky_i(int32_t error, float dt, float leak_rate) +float AC_PID::get_leaky_i(float error, float dt, float leak_rate) { if((_ki != 0) && (dt != 0)){ _integrator -= (float)_integrator * leak_rate; @@ -72,7 +64,7 @@ int32_t AC_PID::get_leaky_i(int32_t error, float dt, float leak_rate) return 0; } -int32_t AC_PID::get_d(int32_t input, float dt) +float AC_PID::get_d(float input, float dt) { if ((_kd != 0) && (dt != 0)) { float derivative; @@ -90,7 +82,7 @@ int32_t AC_PID::get_d(int32_t input, float dt) // discrete low pass filter, cuts out the // high frequency noise that can drive the controller crazy derivative = _last_derivative + - (dt / ( _filter + dt)) * (derivative - _last_derivative); + (dt / ( AC_PID_D_TERM_FILTER + dt)) * (derivative - _last_derivative); // update state _last_input = input; @@ -102,27 +94,25 @@ int32_t AC_PID::get_d(int32_t input, float dt) return 0; } -int32_t AC_PID::get_pi(int32_t error, float dt) +float AC_PID::get_pi(float error, float dt) { return get_p(error) + get_i(error, dt); } -int32_t AC_PID::get_pid(int32_t error, float dt) +float AC_PID::get_pid(float error, float dt) { return get_p(error) + get_i(error, dt) + get_d(error, dt); } -void -AC_PID::reset_I() +void AC_PID::reset_I() { _integrator = 0; // mark derivative as invalid _last_derivative = NAN; } -void -AC_PID::load_gains() +void AC_PID::load_gains() { _kp.load(); _ki.load(); @@ -131,8 +121,7 @@ AC_PID::load_gains() _imax = abs(_imax); } -void -AC_PID::save_gains() +void AC_PID::save_gains() { _kp.save(); _ki.save(); diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h index b774b779ef..dd07d05f51 100644 --- a/libraries/AC_PID/AC_PID.h +++ b/libraries/AC_PID/AC_PID.h @@ -11,6 +11,14 @@ #include #include // for fabs() +// Examples for _filter: +// f_cut = 10 Hz -> _filter = 15.9155e-3 +// f_cut = 15 Hz -> _filter = 10.6103e-3 +// f_cut = 20 Hz -> _filter = 7.9577e-3 +// f_cut = 25 Hz -> _filter = 6.3662e-3 +// f_cut = 30 Hz -> _filter = 5.3052e-3 +#define AC_PID_D_TERM_FILTER 0.00795770f // 20hz filter on D term + /// @class AC_PID /// @brief Object managing one PID control class AC_PID { @@ -56,13 +64,12 @@ public: /// /// @returns The updated control output. /// - int32_t get_pid(int32_t error, float dt); - int32_t get_pi(int32_t error, float dt); - int32_t get_p(int32_t error); - int32_t get_i(int32_t error, float dt); - int32_t get_d(int32_t error, float dt); - int32_t get_leaky_i(int32_t error, float dt, float leak_rate); - + float get_pid(float error, float dt); + float get_pi(float error, float dt); + float get_p(float error) const; + float get_i(float error, float dt); + float get_d(float error, float dt); + float get_leaky_i(float error, float dt, float leak_rate); /// Reset the PID integrator /// @@ -87,38 +94,17 @@ public: _kp = p; _ki = i; _kd = d; _imax = abs(imaxval); } - float kP() const { - return _kp.get(); - } - float kI() const { - return _ki.get(); - } - float kD() const { - return _kd.get(); - } - int16_t imax() const { - return _imax.get(); - } - - void kP(const float v) { - _kp.set(v); - } - void kI(const float v) { - _ki.set(v); - } - void kD(const float v) { - _kd.set(v); - } - void imax(const int16_t v) { - _imax.set(abs(v)); - } - - float get_integrator() const { - return _integrator; - } - void set_integrator(float i) { - _integrator = i; - } + // accessors + float kP() const { return _kp.get(); } + float kI() const { return _ki.get(); } + float kD() const { return _kd.get(); } + int16_t imax() const { return _imax.get(); } + void kP(const float v) { _kp.set(v); } + void kI(const float v) { _ki.set(v); } + void kD(const float v) { _kd.set(v); } + void imax(const int16_t v) { _imax.set(abs(v)); } + float get_integrator() const { return _integrator; } + void set_integrator(float i) { _integrator = i; } static const struct AP_Param::GroupInfo var_info[]; @@ -129,12 +115,8 @@ private: AP_Int16 _imax; float _integrator; ///< integrator value - int32_t _last_input; ///< last input for derivative + float _last_input; ///< last input for derivative float _last_derivative; ///< last derivative for low-pass filter - - /// Low pass filter cut frequency for derivative calculation. - /// - static const float _filter; }; #endif // __AC_PID_H__