diff --git a/libraries/AC_PID/AC_P.h b/libraries/AC_PID/AC_P.h index e173d296e9..76abdf3424 100644 --- a/libraries/AC_PID/AC_P.h +++ b/libraries/AC_PID/AC_P.h @@ -1,7 +1,7 @@ #pragma once /// @file AC_PD.h -/// @brief Generic PID algorithm, with EEPROM-backed storage of constants. +/// @brief Generic P controller with EEPROM-backed storage of constants. #include #include diff --git a/libraries/AC_PID/AC_PID_2D.cpp b/libraries/AC_PID/AC_PID_2D.cpp index bd5bb8061d..8be7857a13 100644 --- a/libraries/AC_PID/AC_PID_2D.cpp +++ b/libraries/AC_PID/AC_PID_2D.cpp @@ -23,13 +23,13 @@ const AP_Param::GroupInfo AC_PID_2D::var_info[] = { // @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), + AP_GROUPINFO("IMAX", 2, AC_PID_2D, _kimax, 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), + AP_GROUPINFO("FILT", 3, AC_PID_2D, _filt_E_hz, AC_PID_2D_FILT_HZ_DEFAULT), // @Param: D // @DisplayName: PID Derivative Gain @@ -40,173 +40,105 @@ const AP_Param::GroupInfo AC_PID_2D::var_info[] = { // @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_GROUPINFO("D_FILT", 5, AC_PID_2D, _filt_D_hz, AC_PID_2D_FILT_D_HZ_DEFAULT), + + // @Param: FF + // @DisplayName: PID Feed Forward Gain + // @Description: FF Gain which produces an output that is proportional to the magnitude of the target + AP_GROUPINFO("FF", 6, AC_PID_2D, _kff, 0), 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) : +AC_PID_2D::AC_PID_2D(float initial_kP, float initial_kI, float initial_kD, float initial_kFF, float initial_imax, float initial_filt_E_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); + _kp = initial_kP; + _ki = initial_kI; + _kd = initial_kD; + _kff = initial_kFF; + _kimax = fabsf(initial_imax); + filt_E_hz(initial_filt_E_hz); + filt_D_hz(initial_filt_D_hz); - // reset input filter to first value received and derivitive to zero - reset_filter(); + // reset input filter to first value received + _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) +// update_all - set target and measured inputs to PID controller and calculate outputs +// target and error are filtered +// the derivative is then calculated and filtered +// the integral is then updated based on the setting of the limit flag +Vector2f AC_PID_2D::update_all(const Vector2f &target, const Vector2f &measurement, bool limit) { // don't process inf or NaN - if (!isfinite(input.x) || !isfinite(input.y)) { - return; + if (target.is_nan() || target.is_inf() || + measurement.is_nan() || measurement.is_inf()) { + return Vector2f{}; } + _target = target; + // reset input filter to value received - if (_flags._reset_filter) { - _flags._reset_filter = false; - _input = input; - } + if (_reset_filter) { + _reset_filter = false; + _error = _target - measurement; + _derivative.zero(); + } else { + Vector2f error_last{_error}; + _error += ((_target - measurement) - _error) * get_filt_E_alpha(); - // update filter and calculate derivative - const Vector2f input_delta = (input - _input) * _filt_alpha; - _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(const Vector2f& input_delta) -{ - // don't process inf or NaN - if (!isfinite(input_delta.x) && !isfinite(input_delta.y)) { - return; - } - - // update filter and calculate derivative - if (is_positive(_dt)) { - const Vector2f derivative = input_delta / _dt; - const 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); + // calculate and filter derivative + if (_dt > 0.0f) { + const Vector2f derivative{(_error - error_last) / _dt}; + _derivative += (derivative - _derivative) * get_filt_D_alpha(); } - return _integrator; } - return Vector2f(); + + // update I term + update_i(limit); + + _pid_info_x.target = _target.x; + _pid_info_x.actual = measurement.x; + _pid_info_x.error = _error.x; + _pid_info_x.P = _error.x * _kp; + _pid_info_x.I = _integrator.x; + _pid_info_x.D = _derivative.x * _kd; + _pid_info_x.FF = _target.x * _kff; + + _pid_info_y.target = _target.y; + _pid_info_y.actual = measurement.y; + _pid_info_y.error = _error.y; + _pid_info_y.P = _error.y * _kp; + _pid_info_y.I = _integrator.y; + _pid_info_y.D = _derivative.y * _kd; + _pid_info_y.FF = _target.y * _kff; + + return _error * _kp + _integrator + _derivative * _kd + _target * _kff; } -// get_i_shrink - get_i but do not allow integrator to grow in length (it may shrink) -Vector2f AC_PID_2D::get_i_shrink() +Vector2f AC_PID_2D::update_all(const Vector3f &target, const Vector3f &measurement, bool limit) { - 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 update_all(Vector2f{target.x, target.y}, Vector2f{measurement.x, measurement.y}, limit); +} + +// update_i - update the integral +// If the limit flag is set the integral is only allowed to shrink +void AC_PID_2D::update_i(bool limit) +{ + float integrator_length_orig = _kimax; + if (limit) { + integrator_length_orig = MIN(integrator_length_orig, _integrator.length()); + } + _integrator += (_error * _ki) * _dt; + const float integrator_length_new = _integrator.length(); + if (integrator_length_new > integrator_length_orig) { + _integrator *= (integrator_length_orig / integrator_length_new); } - 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::reset_filter() -{ - _flags._reset_filter = true; - _derivative.x = 0.0f; - _derivative.y = 0.0f; - _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 @@ -215,33 +147,40 @@ void AC_PID_2D::save_gains() _kp.save(); _ki.save(); _kd.save(); - _imax.save(); - _filt_hz.save(); - _filt_d_hz.save(); + _kff.save(); + _kimax.save(); + _filt_E_hz.save(); + _filt_D_hz.save(); } -// calc_filt_alpha - recalculate the input filter alpha -void AC_PID_2D::calc_filt_alpha() +// get the target filter alpha +float AC_PID_2D::get_filt_E_alpha() const { - 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); + return calc_lowpass_alpha_dt(_dt, _filt_E_hz); } -// calc_filt_alpha - recalculate the input filter alpha -void AC_PID_2D::calc_filt_alpha_d() +// get the derivative filter alpha +float AC_PID_2D::get_filt_D_alpha() const { - 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); + return calc_lowpass_alpha_dt(_dt, _filt_D_hz); } + +void AC_PID_2D::set_integrator(const Vector2f& target, const Vector2f& measurement, const Vector2f& i) +{ + set_integrator(target - measurement, i); +} + +void AC_PID_2D::set_integrator(const Vector2f& error, const Vector2f& i) +{ + set_integrator(i - error * _kp); +} + +void AC_PID_2D::set_integrator(const Vector2f& i) +{ + _integrator = i; + const float integrator_length = _integrator.length(); + if (integrator_length > _kimax) { + _integrator *= (_kimax / integrator_length); + } +} + diff --git a/libraries/AC_PID/AC_PID_2D.h b/libraries/AC_PID/AC_PID_2D.h index 26e4a6a4e0..149300c833 100644 --- a/libraries/AC_PID/AC_PID_2D.h +++ b/libraries/AC_PID/AC_PID_2D.h @@ -7,6 +7,7 @@ #include #include #include +#include /// @class AC_PID_2D /// @brief Copter PID control class @@ -14,91 +15,88 @@ 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); + AC_PID_2D(float initial_kP, float initial_kI, float initial_kD, float initial_kFF, 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 time step in seconds + void set_dt(float dt) { _dt = 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)); } + // update_all - set target and measured inputs to PID controller and calculate outputs + // target and error are filtered + // the derivative is then calculated and filtered + // the integral is then updated based on the setting of the limit flag + Vector2f update_all(const Vector2f &target, const Vector2f &measurement, bool limit = false); + Vector2f update_all(const Vector3f &target, const Vector3f &measurement, bool limit = false); - // 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(); + // update the integral + // if the limit flag is set the integral is only allowed to shrink + void update_i(bool limit); - // reset_I - reset the integrator - void reset_I(); + // get results from pid controller + Vector2f get_p() const; + Vector2f get_i() const; + Vector2f get_d() const; + Vector2f get_ff(); + + // reset the integrator + void reset_I() { _integrator.zero(); }; // 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(); + void reset_filter() { _reset_filter = true; } // save gain to eeprom - void save_gains(); + void save_gains(); // 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_d_hz.get(); } - float get_filt_alpha_D() const { return _filt_alpha_d; } + AP_Float &kP() { return _kp; } + AP_Float &kI() { return _ki; } + AP_Float &kD() { return _kd; } + AP_Float &ff() { return _kff;} + AP_Float &filt_E_hz() { return _filt_E_hz; } + AP_Float &filt_D_hz() { return _filt_D_hz; } + float imax() const { return _kimax.get(); } + float get_filt_E_alpha() const; + float get_filt_D_alpha() const; // 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); + void kP(float v) { _kp.set(v); } + void kI(float v) { _ki.set(v); } + void kD(float v) { _kd.set(v); } + void ff(float v) { _kff.set(v); } + void imax(float v) { _kimax.set(fabsf(v)); } + void filt_E_hz(float hz) { _filt_E_hz.set(fabsf(hz)); } + void filt_D_hz(float hz) { _filt_D_hz.set(fabsf(hz)); } - 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; } + // integrator setting functions + void set_integrator(const Vector2f& target, const Vector2f& measurement, const Vector2f& i); + void set_integrator(const Vector2f& error, const Vector2f& i); + void set_integrator(const Vector3f& i) { set_integrator(Vector2f(i.x, i.y)); } + void set_integrator(const Vector2f& i); + + const AP_Logger::PID_Info& get_pid_info_x(void) const { return _pid_info_x; } + const AP_Logger::PID_Info& get_pid_info_y(void) const { return _pid_info_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(const 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; + AP_Float _kp; + AP_Float _ki; + AP_Float _kd; + AP_Float _kff; + AP_Float _kimax; + AP_Float _filt_E_hz; // PID error filter frequency in Hz + AP_Float _filt_D_hz; // PID derivative filter frequency in Hz // 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 + float _dt; // timestep in seconds + Vector2f _target; // target value to enable filtering + Vector2f _error; // error value to enable filtering + Vector2f _derivative; // last derivative from low-pass filter + Vector2f _integrator; // integrator value + bool _reset_filter; // true when input filter should be reset during next call to update_all + + AP_Logger::PID_Info _pid_info_x; + AP_Logger::PID_Info _pid_info_y; }; diff --git a/libraries/AC_PID/AC_PID_Basic.cpp b/libraries/AC_PID/AC_PID_Basic.cpp new file mode 100644 index 0000000000..8fade0595c --- /dev/null +++ b/libraries/AC_PID/AC_PID_Basic.cpp @@ -0,0 +1,179 @@ +/// @file AC_PID_Basic.cpp +/// @brief Generic PID algorithm + +#include +#include +#include "AC_PID_Basic.h" + +#define AC_PID_Basic_FILT_E_HZ_DEFAULT 20.0f // default input filter frequency +#define AC_PID_Basic_FILT_E_HZ_MIN 0.01f // minimum input filter frequency +#define AC_PID_Basic_FILT_D_HZ_DEFAULT 10.0f // default input filter frequency +#define AC_PID_Basic_FILT_D_HZ_MIN 0.005f // minimum input filter frequency + +const AP_Param::GroupInfo AC_PID_Basic::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_Basic, _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_Basic, _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_Basic, _kimax, 0), + + // @Param: FLTE + // @DisplayName: PID Error filter frequency in Hz + // @Description: Error filter frequency in Hz + // @Units: Hz + AP_GROUPINFO("FLTE", 3, AC_PID_Basic, _filt_E_hz, AC_PID_Basic_FILT_E_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_Basic, _kd, 0), + + // @Param: FLTD + // @DisplayName: D term filter frequency in Hz + // @Description: D term filter frequency in Hz + // @Units: Hz + AP_GROUPINFO("FLTD", 5, AC_PID_Basic, _filt_D_hz, AC_PID_Basic_FILT_D_HZ_DEFAULT), + + // @Param: FF + // @DisplayName: PID Feed Forward Gain + // @Description: FF Gain which produces an output that is proportional to the magnitude of the target + AP_GROUPINFO("FF", 6, AC_PID_Basic, _kff, 0), + + AP_GROUPEND +}; + +// Constructor +AC_PID_Basic::AC_PID_Basic(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_E_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; + _kff = initial_ff; + _kimax = fabsf(initial_imax); + filt_E_hz(initial_filt_E_hz); + filt_D_hz(initial_filt_D_hz); + + // reset input filter to first value received + _reset_filter = true; +} + +float AC_PID_Basic::update_all(float target, float measurement, bool limit) +{ + return update_all(target, measurement, (limit && is_negative(_integrator)), (limit && is_positive(_integrator))); +} + +// update_all - set target and measured inputs to PID controller and calculate outputs +// target and error are filtered +// the derivative is then calculated and filtered +// the integral is then updated based on the setting of the limit flag +float AC_PID_Basic::update_all(float target, float measurement, bool limit_neg, bool limit_pos) +{ + // don't process inf or NaN + if (!isfinite(target) || isnan(target) || + !isfinite(measurement) || isnan(measurement)) { + INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result); + return 0.0f; + } + + _target = target; + + // reset input filter to value received + if (_reset_filter) { + _reset_filter = false; + _error = _target - measurement; + _derivative = 0.0f; + } else { + float error_last = _error; + _error += get_filt_E_alpha() * ((_target - measurement) - _error); + + // calculate and filter derivative + if (is_positive(_dt)) { + float derivative = (_error - error_last) / _dt; + _derivative += get_filt_D_alpha() * (derivative - _derivative); + } + } + + // update I term + update_i(limit_neg, limit_pos); + + const float P_out = _error * _kp; + const float D_out = _derivative * _kd; + + _pid_info.target = _target; + _pid_info.actual = measurement; + _pid_info.error = _error; + _pid_info.P = _error * _kp; + _pid_info.I = _integrator; + _pid_info.D = _derivative * _kd; + _pid_info.FF = _target * _kff; + + return P_out + _integrator + D_out + _target * _kff; +} + +// update_i - update the integral +// if limit_neg is true, the integral can only increase +// if limit_pos is true, the integral can only decrease +void AC_PID_Basic::update_i(bool limit_neg, bool limit_pos) +{ + if (!is_zero(_ki)) { + // Ensure that integrator can only be reduced if the output is saturated + if (!((limit_neg && is_negative(_error)) || (limit_pos && is_positive(_error)))) { + _integrator += ((float)_error * _ki) * _dt; + _integrator = constrain_float(_integrator, -_kimax, _kimax); + } + } else { + _integrator = 0.0f; + } +} + +// save_gains - save gains to eeprom +void AC_PID_Basic::save_gains() +{ + _kp.save(); + _ki.save(); + _kd.save(); + _kff.save(); + _kimax.save(); + _filt_E_hz.save(); + _filt_D_hz.save(); +} + +// get_filt_T_alpha - get the target filter alpha +float AC_PID_Basic::get_filt_E_alpha() const +{ + return calc_lowpass_alpha_dt(_dt, _filt_E_hz); +} + +// get_filt_D_alpha - get the derivative filter alpha +float AC_PID_Basic::get_filt_D_alpha() const +{ + return calc_lowpass_alpha_dt(_dt, _filt_D_hz); +} + +void AC_PID_Basic::set_integrator(float target, float measurement, float i) +{ + set_integrator(target - measurement, i); +} + +void AC_PID_Basic::set_integrator(float error, float i) +{ + set_integrator(i - error * _kp); +} + +void AC_PID_Basic::set_integrator(float i) +{ + _integrator = constrain_float(i, -_kimax, _kimax); +} diff --git a/libraries/AC_PID/AC_PID_Basic.h b/libraries/AC_PID/AC_PID_Basic.h new file mode 100644 index 0000000000..2229922542 --- /dev/null +++ b/libraries/AC_PID/AC_PID_Basic.h @@ -0,0 +1,97 @@ +#pragma once + +/// @file AC_PID_Basic.h +/// @brief Generic PID algorithm, with EEPROM-backed storage of constants. + +#include +#include +#include + +/// @class AC_PID_Basic +/// @brief Copter PID control class +class AC_PID_Basic { +public: + + // Constructor for PID + AC_PID_Basic(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz, float dt); + + // set time step in seconds + void set_dt(float dt) { _dt = dt; } + + // set target and measured inputs to PID controller and calculate outputs + // target and error are filtered + // the derivative is then calculated and filtered + // the integral is then updated based on the setting of the limit flag + float update_all(float target, float measurement, bool limit = false) WARN_IF_UNUSED; + float update_all(float target, float measurement, bool limit_neg, bool limit_pos) WARN_IF_UNUSED; + + // update the integral + // if the limit flags are set the integral is only allowed to shrink + void update_i(bool limit_neg, bool limit_pos); + + // get results from pid controller + float get_p() const WARN_IF_UNUSED { return _error * _kp; } + float get_i() const WARN_IF_UNUSED { return _integrator; } + float get_d() const WARN_IF_UNUSED { return _derivative * _kd; } + float get_ff() const WARN_IF_UNUSED { return _target * _kff; } + + // reset the integrator + void reset_I() { _integrator = 0.0f; } + + // input and D term filter will be reset to the next value provided to set_input() + void reset_filter() { _reset_filter = true; } + + // save gain to eeprom + void save_gains(); + + // get accessors + AP_Float &kP() WARN_IF_UNUSED { return _kp; } + AP_Float &kI() WARN_IF_UNUSED { return _ki; } + AP_Float &kD() WARN_IF_UNUSED { return _kd; } + AP_Float &ff() WARN_IF_UNUSED { return _kff;} + AP_Float &filt_E_hz() WARN_IF_UNUSED { return _filt_E_hz; } + AP_Float &filt_D_hz() WARN_IF_UNUSED { return _filt_D_hz; } + float imax() const WARN_IF_UNUSED { return _kimax.get(); } + float get_filt_E_alpha() const WARN_IF_UNUSED; + float get_filt_D_alpha() const WARN_IF_UNUSED; + + // set accessors + void kP(float v) { _kp.set(v); } + void kI(float v) { _ki.set(v); } + void kD(float v) { _kd.set(v); } + void ff(float v) { _kff.set(v); } + void imax(float v) { _kimax.set(fabsf(v)); } + void filt_E_hz(float hz) { _filt_E_hz.set(fabsf(hz)); } + void filt_D_hz(float hz) { _filt_D_hz.set(fabsf(hz)); } + + // integrator setting functions + void set_integrator(float target, float measurement, float i); + void set_integrator(float error, float i); + void set_integrator(float i); + + const AP_Logger::PID_Info& get_pid_info(void) const WARN_IF_UNUSED { return _pid_info; } + + // parameter var table + static const struct AP_Param::GroupInfo var_info[]; + +protected: + + // parameters + AP_Float _kp; + AP_Float _ki; + AP_Float _kd; + AP_Float _kff; + AP_Float _kimax; + AP_Float _filt_E_hz; // PID error filter frequency in Hz + AP_Float _filt_D_hz; // PID derivative filter frequency in Hz + + // internal variables + float _dt; // timestep in seconds + float _target; // target value to enable filtering + float _error; // error value to enable filtering + float _derivative; // last derivative for low-pass filter + float _integrator; // integrator value + bool _reset_filter; // true when input filter should be reset during next call to set_input + + AP_Logger::PID_Info _pid_info; +}; diff --git a/libraries/AC_PID/AC_PI_2D.cpp b/libraries/AC_PID/AC_PI_2D.cpp index 331d6f1734..45c38145d0 100644 --- a/libraries/AC_PID/AC_PI_2D.cpp +++ b/libraries/AC_PID/AC_PI_2D.cpp @@ -1,27 +1,27 @@ /// @file AC_PI_2D.cpp -/// @brief Generic PID algorithm +/// @brief 2-axis PI controller #include #include "AC_PI_2D.h" const AP_Param::GroupInfo AC_PI_2D::var_info[] = { // @Param: P - // @DisplayName: PID Proportional Gain + // @DisplayName: PI 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 + // @DisplayName: PI 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 + // @DisplayName: PI 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 + // @DisplayName: PI Input filter frequency in Hz // @Description: Input filter frequency in Hz // @Units: Hz AP_GROUPINFO("FILT_HZ", 3, AC_PI_2D, _filt_hz, AC_PI_2D_FILT_HZ_DEFAULT), @@ -101,7 +101,7 @@ Vector2f AC_PI_2D::get_i() } return _integrator; } - return Vector2f(); + return Vector2f{}; } // get_i_shrink - get_i but do not allow integrator to grow in length (it may shrink) @@ -116,7 +116,7 @@ Vector2f AC_PI_2D::get_i_shrink() } return _integrator; } - return Vector2f(); + return Vector2f{}; } Vector2f AC_PI_2D::get_pi() diff --git a/libraries/AC_PID/AC_PI_2D.h b/libraries/AC_PID/AC_PI_2D.h index aece5c011b..d774804578 100644 --- a/libraries/AC_PID/AC_PI_2D.h +++ b/libraries/AC_PID/AC_PI_2D.h @@ -1,7 +1,7 @@ #pragma once /// @file AC_PI_2D.h -/// @brief Generic PID algorithm, with EEPROM-backed storage of constants. +/// @brief 2-axis PI controller with EEPROM-backed storage of constants. #include #include @@ -12,18 +12,18 @@ #define AC_PI_2D_FILT_HZ_MIN 0.01f // minimum input filter frequency /// @class AC_PI_2D -/// @brief Copter PID control class +/// @brief 2-axis PI controller class AC_PI_2D { public: - // Constructor for PID + // constructor AC_PI_2D(float initial_p, float initial_i, float initial_imax, float initial_filt_hz, float 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 + // set_input - set input to PI controller + // input is filtered before the PI 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)); } @@ -78,7 +78,7 @@ private: AP_Float _kp; AP_Float _ki; AP_Float _imax; - AP_Float _filt_hz; // PID Input filter frequency in Hz + AP_Float _filt_hz; // PI Input filter frequency in Hz // flags struct ac_pid_flags { diff --git a/libraries/AC_PID/AC_P_1D.cpp b/libraries/AC_PID/AC_P_1D.cpp new file mode 100644 index 0000000000..926c68b879 --- /dev/null +++ b/libraries/AC_PID/AC_P_1D.cpp @@ -0,0 +1,65 @@ +/// @file AC_P_1D.cpp +/// @brief Generic P algorithm + +#include +#include "AC_P_1D.h" + +const AP_Param::GroupInfo AC_P_1D::var_info[] = { + // @Param: P + // @DisplayName: P Proportional Gain + // @Description: P Gain which produces an output value that is proportional to the current error value + AP_GROUPINFO("P", 0, AC_P_1D, _kp, 0), + AP_GROUPEND +}; + +// Constructor +AC_P_1D::AC_P_1D(float initial_p, float dt) : + _dt(dt) +{ + // load parameter values from eeprom + AP_Param::setup_object_defaults(this, var_info); + + _kp = initial_p; + _lim_D_Out = 10.0f; // maximum first differential of output +} + +// update_all - set target and measured inputs to P controller and calculate outputs +// target and measurement are filtered +// if measurement is further than error_min or error_max (see set_limits_error method) +// the target is moved closer to the measurement and limit_min or limit_max will be set true +float AC_P_1D::update_all(float &target, float measurement, bool &limit_min, bool &limit_max) +{ + limit_min = false; + limit_max = false; + + // calculate distance _error + float error = target - measurement; + + if (error < _lim_err_min) { + error = _lim_err_min; + target = measurement + error; + limit_min = true; + } else if (error > _lim_err_max) { + error = _lim_err_max; + target = measurement + error; + limit_max = true; + } + + // ToDo: Replace sqrt_controller with optimal acceleration and jerk limited curve + // MIN(_Dxy_max, _D2xy_max / _kxy_P) limits the max accel to the point where max jerk is exceeded + return sqrt_controller(error, _kp, _lim_D_Out, _dt); +} + +// set limits on error, output and output from D term +// in normal use the lim_err_min and lim_out_min will be negative +// when using for a position controller, lim_err will be position error, lim_out will be correction velocity, lim_D will be acceleration, lim_D2 will be jerk +void AC_P_1D::set_limits_error(float lim_err_min, float lim_err_max, float lim_out_min, float lim_out_max, float lim_D_Out, float lim_D2_Out) +{ + _lim_D_Out = lim_D_Out; + if (is_positive(lim_D2_Out)) { + // limit the first derivative so as not to exceed the second derivative + _lim_D_Out = MIN(_lim_D_Out, lim_D2_Out / _kp); + } + _lim_err_min = MAX(inv_sqrt_controller(lim_out_min, _kp, _lim_D_Out), lim_err_min); + _lim_err_max = MAX(inv_sqrt_controller(lim_out_max, _kp, _lim_D_Out), lim_err_max); +} diff --git a/libraries/AC_PID/AC_P_1D.h b/libraries/AC_PID/AC_P_1D.h new file mode 100644 index 0000000000..45fae78c36 --- /dev/null +++ b/libraries/AC_PID/AC_P_1D.h @@ -0,0 +1,50 @@ +#pragma once + +/// @file AC_P_1D.h +/// @brief Generic P controller, with EEPROM-backed storage of constants. + +#include +#include + +/// @class AC_P_1D +/// @brief Object managing one P controller +class AC_P_1D { +public: + + // constructor + AC_P_1D(float initial_p, float dt); + + // set time step in seconds + void set_dt(float dt) { _dt = dt; } + + // update_all - set target and measured inputs to P controller and calculate outputs + // target and measurement are filtered + // if measurement is further than error_min or error_max (see set_limits_error method) + // the target is moved closer to the measurement and limit_min or limit_max will be set true + float update_all(float &target, float measurement, bool &limit_min, bool &limit_max) WARN_IF_UNUSED; + + // save gain to eeprom + void save_gains() { _kp.save(); } + + // set limits on error, output and output from D term + void set_limits_error(float error_min, float error_max, float output_min, float output_max, float D_Out_max = 0.0f, float D2_Out_max = 0.0f); + + // accessors + AP_Float &kP() WARN_IF_UNUSED { return _kp; } + const AP_Float &kP() const WARN_IF_UNUSED { return _kp; } + void kP(float v) { _kp.set(v); } + + // parameter var table + static const struct AP_Param::GroupInfo var_info[]; + +private: + + // parameters + AP_Float _kp; + + // internal variables + float _dt; // time step in seconds + float _lim_err_min; // error limit in negative direction + float _lim_err_max; // error limit in positive direction + float _lim_D_Out; // maximum first differential of output +}; diff --git a/libraries/AC_PID/AC_P_2D.cpp b/libraries/AC_PID/AC_P_2D.cpp new file mode 100644 index 0000000000..2309e7e840 --- /dev/null +++ b/libraries/AC_PID/AC_P_2D.cpp @@ -0,0 +1,41 @@ +/// @file AC_P_2D.cpp +/// @brief 2-axis P controller + +#include +#include "AC_P_2D.h" + +const AP_Param::GroupInfo AC_P_2D::var_info[] = { + // @Param: P + // @DisplayName: Proportional Gain + // @Description: P Gain which produces an output value that is proportional to the current error value + AP_GROUPINFO("P", 0, AC_P_2D, _kp, 0), + AP_GROUPEND +}; + +// Constructor +AC_P_2D::AC_P_2D(float initial_p, float dt) : + _dt(dt) +{ + // load parameter values from eeprom + AP_Param::setup_object_defaults(this, var_info); + + _kp = initial_p; +} + +// set target and measured inputs to P controller and calculate outputs +Vector2f AC_P_2D::update_all(float &target_x, float &target_y, const Vector2f &measurement, float error_max, float D2_max) +{ + // calculate distance _error + Vector2f error = Vector2f(target_x, target_y) - measurement; + + // Constrain _error and target position + // Constrain the maximum length of _vel_target to the maximum position correction velocity + if (error.limit_length(error_max)) { + target_x = measurement.x + error.x; + target_y = measurement.y + error.y; + } + + // MIN(_Dmax, _D2max / _kp) limits the max accel to the point where max jerk is exceeded + // return sqrt_controller(Vector2f(_error.x, _error.y), _kp, MIN(_D_max, _D2_max / _kp), _dt); + return sqrt_controller(error, _kp, D2_max, _dt); +} diff --git a/libraries/AC_PID/AC_P_2D.h b/libraries/AC_PID/AC_P_2D.h new file mode 100644 index 0000000000..d42f9b95f7 --- /dev/null +++ b/libraries/AC_PID/AC_P_2D.h @@ -0,0 +1,45 @@ +#pragma once + +/// @file AC_P_2D.h +/// @brief 2-axis P controller with EEPROM-backed storage of constants. + +#include +#include + +/// @class AC_P_2D +/// @brief 2-axis P controller +class AC_P_2D { +public: + + // constructor + AC_P_2D(float initial_p, float dt); + + // set time step in seconds + void set_dt(float dt) { _dt = dt; } + + // set target and measured inputs to P controller and calculate outputs + Vector2f update_all(float &target_x, float &target_y, const Vector2f &measurement, float error_max, float D2_max) WARN_IF_UNUSED; + + // set target and measured inputs to P controller and calculate outputs + // measurement is provided as 3-axis vector but only x and y are used + Vector2f update_all(float &target_x, float &target_y, const Vector3f &measurement, float error_max, float D2_max) WARN_IF_UNUSED { + return update_all(target_x, target_y, Vector2f(measurement.x, measurement.y), error_max, D2_max); + } + + // get accessors + AP_Float &kP() WARN_IF_UNUSED { return _kp; } + const AP_Float &kP() const WARN_IF_UNUSED { return _kp; } + + // set accessor + void kP(float v) { _kp.set(v); } + + // parameter var table + static const struct AP_Param::GroupInfo var_info[]; + +private: + // parameters + AP_Float _kp; + + // internal variables + float _dt; // time step in seconds +};