diff --git a/libraries/AC_PID/AC_PID_2D.cpp b/libraries/AC_PID/AC_PID_2D.cpp new file mode 100644 index 0000000000..3660299747 --- /dev/null +++ b/libraries/AC_PID/AC_PID_2D.cpp @@ -0,0 +1,256 @@ +/// @file AC_PID_2D.cpp +/// @brief Generic PID algorithm + +#include +#include "AC_PID_2D.h" + +const AP_Param::GroupInfo AC_PID_2D::var_info[] = { + // @Param: P + // @DisplayName: PID Proportional Gain + // @Description: P Gain which produces an output value that is proportional to the current error value + AP_GROUPINFO("P", 0, AC_PID_2D, _kp, 0), + + // @Param: I + // @DisplayName: PID Integral Gain + // @Description: I Gain which produces an output that is proportional to both the magnitude and the duration of the error + AP_GROUPINFO("I", 1, AC_PID_2D, _ki, 0), + + // @Param: IMAX + // @DisplayName: PID Integral Maximum + // @Description: The maximum/minimum value that the I term can output + AP_GROUPINFO("IMAX", 2, AC_PID_2D, _imax, 0), + + // @Param: FILT + // @DisplayName: PID Input filter frequency in Hz + // @Description: Input filter frequency in Hz + // @Units: Hz + AP_GROUPINFO("FILT", 3, AC_PID_2D, _filt_hz, AC_PID_2D_FILT_HZ_DEFAULT), + + // @Param: D + // @DisplayName: PID Derivative Gain + // @Description: D Gain which produces an output that is proportional to the rate of change of the error + AP_GROUPINFO("D", 4, AC_PID_2D, _kd, 0), + + // @Param: D_FILT + // @DisplayName: D term filter frequency in Hz + // @Description: D term filter frequency in Hz + // @Units: Hz + AP_GROUPINFO("D_FILT", 5, AC_PID_2D, _filt_d_hz, AC_PID_2D_FILT_D_HZ_DEFAULT), + + AP_GROUPEND +}; + +// Constructor +AC_PID_2D::AC_PID_2D(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float initial_filt_d_hz, float dt) : + _dt(dt) +{ + // load parameter values from eeprom + AP_Param::setup_object_defaults(this, var_info); + + _kp = initial_p; + _ki = initial_i; + _kd = initial_d; + _imax = fabsf(initial_imax); + filt_hz(initial_filt_hz); + filt_d_hz(initial_filt_d_hz); + + // reset input filter to first value received + _flags._reset_filter = true; +} + +// set_dt - set time step in seconds +void AC_PID_2D::set_dt(float dt) +{ + // set dt and calculate the input filter alpha + _dt = dt; + calc_filt_alpha(); + calc_filt_alpha_d(); +} + +// filt_hz - set input filter hz +void AC_PID_2D::filt_hz(float hz) +{ + _filt_hz.set(fabsf(hz)); + + // sanity check _filt_hz + _filt_hz = MAX(_filt_hz, AC_PID_2D_FILT_HZ_MIN); + + // calculate the input filter alpha + calc_filt_alpha(); +} + +// filt_d_hz - set input filter hz +void AC_PID_2D::filt_d_hz(float hz) +{ + _filt_d_hz.set(fabsf(hz)); + + // sanity check _filt_hz + _filt_d_hz = MAX(_filt_d_hz, AC_PID_2D_FILT_D_HZ_MIN); + + // calculate the input filter alpha + calc_filt_alpha_d(); +} + +// 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 +void AC_PID_2D::set_input(const Vector2f &input) +{ + // don't process inf or NaN + if (!isfinite(input.x) || !isfinite(input.y)) { + return; + } + + // reset input filter to value received + if (_flags._reset_filter) { + _flags._reset_filter = false; + _input = input; + } + + // update filter and calculate derivative + const Vector2f input_delta = (input - _input) * _filt_alpha; + _input = _input + input_delta; + + set_input_filter_d(input_delta); +} + +// 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) +{ + // don't process inf or NaN + if (!isfinite(input_delta.x) && !isfinite(input_delta.y)) { + return; + } + + // reset input filter to value received + if (_flags._reset_filter) { + _flags._reset_filter = false; + _derivative.x = 0.0f; + _derivative.y = 0.0f; + } + + // update filter and calculate derivative + if (is_positive(_dt)) { + Vector2f derivative = input_delta / _dt; + Vector2f delta_derivative = (derivative - _derivative) * _filt_alpha_d; + _derivative += delta_derivative; + } +} + +Vector2f AC_PID_2D::get_p() const +{ + return (_input * _kp); +} + +Vector2f AC_PID_2D::get_i() +{ + if (!is_zero(_ki) && !is_zero(_dt)) { + _integrator += (_input * _ki) * _dt; + const float integrator_length = _integrator.length(); + if ((integrator_length > _imax) && is_positive(integrator_length)) { + _integrator *= (_imax / integrator_length); + } + return _integrator; + } + return Vector2f(); +} + +// get_i_shrink - get_i but do not allow integrator to grow in length (it may shrink) +Vector2f AC_PID_2D::get_i_shrink() +{ + if (!is_zero(_ki) && !is_zero(_dt)) { + const float integrator_length_orig = MIN(_integrator.length(), _imax); + _integrator += (_input * _ki) * _dt; + const float integrator_length_new = _integrator.length(); + if ((integrator_length_new > integrator_length_orig) && is_positive(integrator_length_new)) { + _integrator *= (integrator_length_orig / integrator_length_new); + } + return _integrator; + } + return Vector2f(); +} + +Vector2f AC_PID_2D::get_d() +{ + // derivative component + return Vector2f(_kd * _derivative.x, _kd * _derivative.y); +} + +Vector2f AC_PID_2D::get_pid() +{ + return get_p() + get_i() + get_d(); +} + +void AC_PID_2D::reset_I() +{ + _integrator.zero(); +} + +void AC_PID_2D::load_gains() +{ + _kp.load(); + _ki.load(); + _kd.load(); + _imax.load(); + _imax = fabsf(_imax); + _filt_hz.load(); + _filt_d_hz.load(); + + // calculate the input filter alpha + calc_filt_alpha(); + calc_filt_alpha_d(); +} + +// save_gains - save gains to eeprom +void AC_PID_2D::save_gains() +{ + _kp.save(); + _ki.save(); + _kd.save(); + _imax.save(); + _filt_hz.save(); + _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() +{ + if (is_zero(_filt_hz)) { + _filt_alpha = 1.0f; + return; + } + + // calculate alpha + const float rc = 1/(M_2PI*_filt_hz); + _filt_alpha = _dt / (_dt + rc); +} + +// calc_filt_alpha - recalculate the input filter alpha +void AC_PID_2D::calc_filt_alpha_d() +{ + if (is_zero(_filt_d_hz)) { + _filt_alpha_d = 1.0f; + return; + } + + // calculate alpha + const float rc = 1/(M_2PI*_filt_d_hz); + _filt_alpha_d = _dt / (_dt + rc); +} diff --git a/libraries/AC_PID/AC_PID_2D.h b/libraries/AC_PID/AC_PID_2D.h new file mode 100644 index 0000000000..a095cb2e05 --- /dev/null +++ b/libraries/AC_PID/AC_PID_2D.h @@ -0,0 +1,112 @@ +#pragma once + +/// @file AC_PID_2D.h +/// @brief Generic PID algorithm, with EEPROM-backed storage of constants. + +#include +#include +#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 { +public: + + // Constructor for PID + AC_PID_2D(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float initial_filt_d_hz, float dt); + + // set_dt - set time step in seconds + void set_dt(float dt); + + // 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 + void set_input(const Vector2f &input); + void set_input(const Vector3f &input) { set_input(Vector2f(input.x, input.y)); } + + // get_pi - get results from pid controller + Vector2f get_pid(); + Vector2f get_p() const; + Vector2f get_i(); + Vector2f get_i_shrink(); // get_i but do not allow integrator to grow (it may shrink) + Vector2f get_d(); + + // reset_I - reset the integrator + void reset_I(); + + // reset_filter - input and D term filter will be reset to the next value provided to set_input() + void reset_filter(); + + // load gain from eeprom + void load_gains(); + + // 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; } + float imax() const { return _imax.get(); } + float filt_hz() const { return _filt_hz.get(); } + float get_filt_alpha() const { return _filt_alpha; } + float filt_d_hz() const { return _filt_hz.get(); } + float get_filt_alpha_D() const { return _filt_alpha_d; } + + // set accessors + 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 float v) { _imax.set(fabsf(v)); } + void filt_hz(const float v); + void filt_d_hz(const float v); + + Vector2f get_integrator() const { return _integrator; } + void set_integrator(const Vector2f &i) { _integrator = i; } + void set_integrator(const Vector3f &i) { _integrator.x = i.x; _integrator.y = i.y; } + + // parameter var table + static const struct AP_Param::GroupInfo var_info[]; + +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); + + // calc_filt_alpha - recalculate the input filter alpha + void calc_filt_alpha(); + + // calc_filt_alpha - recalculate the input filter alpha + void calc_filt_alpha_d(); + + // parameters + AP_Float _kp; + AP_Float _ki; + AP_Float _kd; + AP_Float _imax; + AP_Float _filt_hz; // PID Input filter frequency in Hz + AP_Float _filt_d_hz; // D term filter frequency in Hz + + // flags + struct ac_pid_flags { + bool _reset_filter : 1; // true when input filter should be reset during next call to set_input + } _flags; + + // internal variables + float _dt; // timestep in seconds + float _filt_alpha; // input filter alpha + float _filt_alpha_d; // input filter alpha + Vector2f _integrator; // integrator value + Vector2f _input; // last input for derivative + Vector2f _derivative; // last derivative for low-pass filter +};