diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp new file mode 100755 index 0000000000..a50f10d6a0 --- /dev/null +++ b/libraries/AC_PID/AC_PID.cpp @@ -0,0 +1,118 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file AC_PID.cpp +/// @brief Generic PID algorithm + +#include +#include "AC_PID.h" + + +int32_t AC_PID::get_p(int32_t error) +{ + return (float)error * _kp; +} + +int32_t AC_PID::get_i(int32_t error, float dt) +{ + if((_ki != 0) && (dt != 0)){ + _integrator += ((float)error * _ki) * dt; + if (_integrator < -_imax) { + _integrator = -_imax; + } else if (_integrator > _imax) { + _integrator = _imax; + } + } + return _integrator; +} + +int32_t AC_PID::get_d(int32_t input, float dt) +{ + if ((_kd != 0) && (dt != 0)) { + _derivative = (input - _last_input) / 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); + + // update state + _last_input = input; + _last_derivative = _derivative; + + // add in derivative component + return _kd * _derivative; + } +} + +int32_t AC_PID::get_pi(int32_t error, float dt) +{ + return get_p(error) + get_i(error, dt); +} + + +int32_t AC_PID::get_pid(int32_t error, float dt) +{ + return get_p(error) + get_i(error, dt) + get_d(error, dt); +} + + + +/* +int32_t AC_PID::get_pid(int32_t error, float dt) +{ + // Compute proportional component + _output = error * _kp; + + // Compute derivative component if time has elapsed + if ((fabs(_kd) > 0) && (dt > 0)) { + + _derivative = (error - _last_input) / 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); + + // update state + _last_input = error; + _last_derivative = _derivative; + + // add in derivative component + _output += _kd * _derivative; + } + + // Compute integral component if time has elapsed + if ((fabs(_ki) > 0) && (dt > 0)) { + _integrator += (error * _ki) * dt; + if (_integrator < -_imax) { + _integrator = -_imax; + } else if (_integrator > _imax) { + _integrator = _imax; + } + _output += _integrator; + } + + return _output; +} +*/ + + +void +AC_PID::reset_I() +{ + _integrator = 0; + _last_input = 0; + _last_derivative = 0; +} + +void +AC_PID::load_gains() +{ + _group.load(); +} + +void +AC_PID::save_gains() +{ + _group.save(); +} diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h new file mode 100755 index 0000000000..f9b4e359de --- /dev/null +++ b/libraries/AC_PID/AC_PID.h @@ -0,0 +1,152 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file AC_PID.h +/// @brief Generic PID algorithm, with EEPROM-backed storage of constants. + +#ifndef AC_PID_h +#define AC_PID_h + +#include +#include // for fabs() + +/// @class AC_PID +/// @brief Object managing one PID control +class AC_PID { +public: + + /// Constructor for PID that saves its settings to EEPROM + /// + /// @note PIDs must be named to avoid either multiple parameters with the + /// same name, or an overly complex constructor. + /// + /// @param key Storage key assigned to this PID. Should be unique. + /// @param name Name by which the PID is known, or NULL for an anonymous PID. + /// The name is prefixed to the P, I, D, IMAX variable names when + /// they are reported. + /// @param initial_p Initial value for the P term. + /// @param initial_i Initial value for the I term. + /// @param initial_d Initial value for the D term. + /// @param initial_imax Initial value for the imax term.4 + /// + AC_PID(AP_Var::Key key, + const prog_char_t *name, + const float &initial_p = 0.0, + const float &initial_i = 0.0, + const float &initial_d = 0.0, + const int16_t &initial_imax = 0.0) : + + _group(key, name), + // group, index, initial value, name + _kp (&_group, 0, initial_p, PSTR("P")), + _ki (&_group, 1, initial_i, PSTR("I")), + _kd (&_group, 2, initial_d, PSTR("D")), + _imax(&_group, 3, initial_imax, PSTR("IMAX")) + { + // no need for explicit load, assuming that the main code uses AP_Var::load_all. + } + + /// Constructor for PID that does not save its settings. + /// + /// @param name Name by which the PID is known, or NULL for an anonymous PID. + /// The name is prefixed to the P, I, D, IMAX variable names when + /// they are reported. + /// @param initial_p Initial value for the P term. + /// @param initial_i Initial value for the I term. + /// @param initial_d Initial value for the D term. + /// @param initial_imax Initial value for the imax term.4 + /// + AC_PID(const prog_char_t *name, + const float &initial_p = 0.0, + const float &initial_i = 0.0, + const float &initial_d = 0.0, + const int16_t &initial_imax = 0.0) : + + _group(AP_Var::k_key_none, name), + // group, index, initial value, name + _kp (&_group, 0, initial_p, PSTR("P")), + _ki (&_group, 1, initial_i, PSTR("I")), + _kd (&_group, 2, initial_d, PSTR("D")), + _imax(&_group, 3, initial_imax, PSTR("IMAX")) + { + } + + /// Iterate the PID, return the new control value + /// + /// Positive error produces positive output. + /// + /// @param error The measured error value + /// @param dt The time delta in milliseconds (note + /// that update interval cannot be more + /// than 65.535 seconds due to limited range + /// of the data type). + /// @param scaler An arbitrary scale factor + /// + /// @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); + + + /// Reset the PID integrator + /// + void reset_I(); + + /// Load gain properties + /// + void load_gains(); + + /// Save gain properties + /// + void save_gains(); + + /// @name parameter accessors + //@{ + + /// Overload the function call operator to permit relatively easy initialisation + void operator() (const float p, + const float i, + const float d, + const int16_t imaxval) { + _kp = p; _ki = i; _kd = d; _imax = 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; } + +private: + AP_Var_group _group; + AP_Float16 _kp; + AP_Float16 _ki; + AP_Float16 _kd; + AP_Int16 _imax; + + float _integrator; ///< integrator value + int32_t _last_input; ///< last input for derivative + float _last_derivative; ///< last derivative for low-pass filter + float _output; + float _derivative; + + /// Low pass filter cut frequency for derivative calculation. + /// + static const float _filter = 7.9577e-3; // Set to "1 / ( 2 * PI * f_cut )"; + // 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 +}; + +#endif diff --git a/libraries/AC_PID/keywords.txt b/libraries/AC_PID/keywords.txt new file mode 100755 index 0000000000..68e861fd00 --- /dev/null +++ b/libraries/AC_PID/keywords.txt @@ -0,0 +1,9 @@ +PID KEYWORD1 +get_pid KEYWORD2 +reset_I KEYWORD2 +kP KEYWORD2 +kD KEYWORD2 +kI KEYWORD2 +imax KEYWORD2 +load_gains KEYWORD2 +save_gains KEYWORD2