From 34a5bc8b33cd2a48cc010a8d16fa793c55e682dd Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 29 Jan 2015 15:49:30 +0900 Subject: [PATCH] AC_PI_2D: 2-axis PI controller --- libraries/AC_PID/AC_PI_2D.cpp | 167 ++++++++++++++++++++++++++++++++++ libraries/AC_PID/AC_PI_2D.h | 100 ++++++++++++++++++++ 2 files changed, 267 insertions(+) create mode 100644 libraries/AC_PID/AC_PI_2D.cpp create mode 100644 libraries/AC_PID/AC_PI_2D.h diff --git a/libraries/AC_PID/AC_PI_2D.cpp b/libraries/AC_PID/AC_PI_2D.cpp new file mode 100644 index 0000000000..983bb93541 --- /dev/null +++ b/libraries/AC_PID/AC_PI_2D.cpp @@ -0,0 +1,167 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/// @file AC_PI_2D.cpp +/// @brief Generic PID algorithm + +#include +#include "AC_PI_2D.h" + +const AP_Param::GroupInfo AC_PI_2D::var_info[] PROGMEM = { + // @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_PI_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_PI_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_PI_2D, _imax, 0), + + // @Param: FILT_HZ + // @DisplayName: PID Input filter frequency in Hz + // @Description: Input filter frequency in Hz + // @Unit: Hz + AP_GROUPINFO("FILT_HZ", 3, AC_PI_2D, _filt_hz, AC_PI_2D_FILT_HZ_DEFAULT), + + AP_GROUPEND +}; + +// Constructor +AC_PI_2D::AC_PI_2D(float initial_p, float initial_i, float initial_imax, float initial_filt_hz, float dt) : + _dt(dt) +{ + // load parameter values from eeprom + AP_Param::setup_object_defaults(this, var_info); + + _kp = initial_p; + _ki = initial_i; + _imax = fabs(initial_imax); + _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 +void AC_PI_2D::set_dt(float dt) +{ + // set dt and calculate the input filter alpha + _dt = dt; + calc_filt_alpha(); +} + +// set_filt_hz - set input filter hz +void AC_PI_2D::set_filt_hz(float hz) +{ + _filt_hz.set(hz); + // calculate the input filter alpha + calc_filt_alpha(); +} + +// 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_PI_2D::set_input(const Vector2f &input) +{ + // reset input filter to value received + if (_flags._reset_filter) { + _flags._reset_filter = false; + _input = input; + } + + // update filter and calculate derivative + Vector2f input_filt_change = (input - _input) * _filt_alpha; + _input = _input + input_filt_change; +} + +Vector2f AC_PI_2D::get_p() const +{ + return (_input * _kp); +} + +Vector2f AC_PI_2D::get_i() +{ + if((_ki != 0.0f) && (_dt != 0.0f)) { + _integrator += (_input * _ki) * _dt; + float integrator_length = _integrator.length(); + if ((integrator_length > _imax) && (integrator_length > 0)) { + _integrator *= (_imax / integrator_length); + } + return _integrator; + } + return Vector2f(0,0); +} + +// get_i_shrink - get_i but do not allow integrator to grow in length (it may shrink) +Vector2f AC_PI_2D::get_i_shrink() +{ + if((_ki != 0.0f) && (_dt != 0.0f)) { + float integrator_length_orig = min(_integrator.length(),_imax); + _integrator += (_input * _ki) * _dt; + float integrator_length_new = _integrator.length(); + if ((integrator_length_new > integrator_length_orig) && (integrator_length_new > 0)) { + _integrator *= (integrator_length_orig / integrator_length_new); + } + return _integrator; + } + return Vector2f(0,0); +} + +Vector2f AC_PI_2D::get_pi() +{ + return get_p() + get_i(); +} + +void AC_PI_2D::reset_I() +{ + _integrator.zero(); +} + +void AC_PI_2D::load_gains() +{ + _kp.load(); + _ki.load(); + _imax.load(); + _imax = fabs(_imax); + _filt_hz.load(); + + // calculate the input filter alpha + calc_filt_alpha(); +} + +// save_gains - save gains to eeprom +void AC_PI_2D::save_gains() +{ + _kp.save(); + _ki.save(); + _imax.save(); + _filt_hz.save(); +} + +/// Overload the function call operator to permit easy initialisation +void AC_PI_2D::operator() (float p, float i, float imaxval, float input_filt_hz, float dt) +{ + _kp = p; + _ki = i; + _imax = fabs(imaxval); + _filt_hz = input_filt_hz; + _dt = dt; + // calculate the input filter alpha + calc_filt_alpha(); +} + +// 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 new file mode 100644 index 0000000000..6829c1dcad --- /dev/null +++ b/libraries/AC_PID/AC_PI_2D.h @@ -0,0 +1,100 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/// @file AC_PI_2D.h +/// @brief Generic PID algorithm, with EEPROM-backed storage of constants. + +#ifndef __AC_PI_2D_H__ +#define __AC_PI_2D_H__ + +#include +#include +#include +#include + +#define AC_PI_2D_FILT_HZ_DEFAULT 20.0f // default input filter frequency + +/// @class AC_PI_2D +/// @brief Copter PID control class +class AC_PI_2D { +public: + + // Constructor for PID + AC_PI_2D(float initial_p, float initial_i, float initial_imax, float initial_filt_hz, float dt); + + // 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 + 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_pi(); + Vector2f get_p() const; + Vector2f get_i(); + Vector2f get_i_shrink(); // get_i but do not allow integrator to grow (it may shrink) + + // reset_I - reset the integrator + void reset_I(); + + // reset_filter - input 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 dt ); + + // get accessors + float kP() const { return _kp.get(); } + float kI() const { return _ki.get(); } + float imax() const { return _imax.get(); } + float filt_hz() const { return _filt_hz.get(); } + float get_filt_alpha() const { return _filt_alpha; } + + // set accessors + 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)); } + + 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: + + // calc_filt_alpha - recalculate the input filter alpha + void calc_filt_alpha(); + + // parameters + AP_Float _kp; + AP_Float _ki; + AP_Float _imax; + AP_Float _filt_hz; // PID Input 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 + Vector2f _integrator; // integrator value + Vector2f _input; // last input for derivative + float _filt_alpha; // input filter alpha +}; + +#endif // __AC_PI_2D_H__