From 517448e536dabab6e99ca75e58cf4d701a7b76b6 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 11 Feb 2015 21:06:04 +0900 Subject: [PATCH] AC_PID: add input filtering and restructure --- libraries/AC_PID/AC_PID.cpp | 172 +++++++++++++++++++++++++++--------- libraries/AC_PID/AC_PID.h | 143 +++++++++++++----------------- 2 files changed, 191 insertions(+), 124 deletions(-) diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp index 8eae1ea4d0..23d287c9d5 100644 --- a/libraries/AC_PID/AC_PID.cpp +++ b/libraries/AC_PID/AC_PID.cpp @@ -11,30 +11,122 @@ const AP_Param::GroupInfo AC_PID::var_info[] PROGMEM = { // @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, _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, _ki, 0), + // @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", 2, AC_PID, _kd, 0), + + // 3 was for uint16 IMAX + // 4 is used by TradHeli for FF + // @Param: IMAX // @DisplayName: PID Integral Maximum // @Description: The maximum/minimum value that the I term can output - AP_GROUPINFO("IMAX", 3, AC_PID, _imax, 0), + AP_GROUPINFO("IMAX", 5, AC_PID, _imax, 0), + + // @Param: FILT_HZ + // @DisplayName: PID Input filter frequency in Hz + // @Description: Input filter frequency in Hz + // @Unit: Hz + AP_GROUPINFO("FILT_HZ", 6, AC_PID, _filt_hz, AC_PID_FILT_HZ_DEFAULT), + AP_GROUPEND }; -float AC_PID::get_p(float error) const +// Constructor +AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float dt) : + _dt(dt), + _integrator(0.0f), + _input(0.0f), + _derivative(0.0f) { - return (float)error * _kp; + // load parameter values from eeprom + AP_Param::setup_object_defaults(this, var_info); + + _kp = initial_p; + _ki = initial_i; + _kd = initial_d; + _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(); } -float AC_PID::get_i(float error, float dt) +// set_dt - set time step in seconds +void AC_PID::set_dt(float dt) { - if((_ki != 0) && (dt != 0)) { - _integrator += ((float)error * _ki) * dt; + // set dt and calculate the input filter alpha + _dt = dt; + calc_filt_alpha(); +} + +// set_filt_hz - set input filter hz +void AC_PID::set_filt_hz(float hz) +{ + _filt_hz.set(hz); + // calculate the input filter alpha + calc_filt_alpha(); +} + +// set_input_filter_all - 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::set_input_filter_all(float input) +{ + // reset input filter to value received + if (_flags._reset_filter) { + _flags._reset_filter = false; + _input = input; + _derivative = 0.0f; + } + + // update filter and calculate derivative + float input_filt_change = _filt_alpha * (input - _input); + _input = _input + input_filt_change; + if (_dt > 0.0f) { + _derivative = input_filt_change / _dt; + } +} + +// 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::set_input_filter_d(float input) +{ + // reset input filter to value received + if (_flags._reset_filter) { + _flags._reset_filter = false; + _derivative = 0.0f; + } + + // update filter and calculate derivative + if (_dt > 0.0f) { + float derivative = (input - _input) / _dt; + _derivative = _derivative + _filt_alpha * (derivative-_derivative); + } + + _input = input; +} + +float AC_PID::get_p() const +{ + return (_input * _kp); +} + +float AC_PID::get_i() +{ + if((_ki != 0) && (_dt != 0)) { + _integrator += ((float)_input * _ki) * _dt; if (_integrator < -_imax) { _integrator = -_imax; } else if (_integrator > _imax) { @@ -45,51 +137,27 @@ float AC_PID::get_i(float error, float dt) return 0; } -float AC_PID::get_d(float input, float dt) +float AC_PID::get_d() const { - if ((_kd != 0) && (dt != 0)) { - float derivative; - if (isnan(_last_derivative)) { - // we've just done a reset, suppress the first derivative - // term as we don't want a sudden change in input to cause - // a large D output change - derivative = 0; - _last_derivative = 0; - } else { - // calculate instantaneous derivative - derivative = (input - _last_input) / dt; - } - - // discrete low pass filter, cuts out the - // high frequency noise that can drive the controller crazy - derivative = _last_derivative + _d_lpf_alpha * (derivative - _last_derivative); - - // update state - _last_input = input; - _last_derivative = derivative; - - // add in derivative component - return _kd * derivative; - } - return 0; + // add in derivative component + return (_kd * _derivative); } -float AC_PID::get_pi(float error, float dt) +float AC_PID::get_pi() { - return get_p(error) + get_i(error, dt); + return get_p() + get_i(); } -float AC_PID::get_pid(float error, float dt) +float AC_PID::get_pid() { - return get_p(error) + get_i(error, dt) + get_d(error, dt); + return get_p() + get_i() + get_d(); } + void AC_PID::reset_I() { _integrator = 0; - // mark derivative as invalid - _last_derivative = NAN; } void AC_PID::load_gains() @@ -98,20 +166,40 @@ void AC_PID::load_gains() _ki.load(); _kd.load(); _imax.load(); - _imax = abs(_imax); + _imax = fabs(_imax); + _filt_hz.load(); + + // calculate the input filter alpha + calc_filt_alpha(); } +// save_gains - save gains to eeprom void AC_PID::save_gains() { _kp.save(); _ki.save(); _kd.save(); _imax.save(); + _filt_hz.save(); } -void AC_PID::set_d_lpf_alpha(int16_t cutoff_frequency, float time_step) +/// Overload the function call operator to permit easy initialisation +void AC_PID::operator() (float p, float i, float d, float imaxval, float input_filt_hz, float dt) +{ + _kp = p; + _ki = i; + _kd = d; + _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_PID::calc_filt_alpha() { // calculate alpha - float rc = 1/(2*PI*cutoff_frequency); - _d_lpf_alpha = time_step / (time_step + rc); + float rc = 1/(2*PI*_filt_hz); + _filt_alpha = _dt / (_dt + rc); } diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h index 0c2e68d37b..b828e6cd36 100644 --- a/libraries/AC_PID/AC_PID.h +++ b/libraries/AC_PID/AC_PID.h @@ -9,121 +9,100 @@ #include #include #include -#include // for fabs() +#include -// Examples for _filter: -// f_cut = 10 Hz -> _alpha = 0.385869 -// f_cut = 15 Hz -> _alpha = 0.485194 -// f_cut = 20 Hz -> _alpha = 0.556864 -// f_cut = 25 Hz -> _alpha = 0.611015 -// f_cut = 30 Hz -> _alpha = 0.653373 -#define AC_PID_D_TERM_FILTER 0.556864f // Default 100Hz Filter Rate with 20Hz Cutoff Frequency +#define AC_PID_FILT_HZ_DEFAULT 20.0f // default input filter frequency /// @class AC_PID -/// @brief Object managing one PID control +/// @brief Copter PID control class 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 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 float & initial_p = 0.0, - const float & initial_i = 0.0, - const float & initial_d = 0.0, - const int16_t & initial_imax = 0.0): - _integrator(0), - _last_input(0), - _last_derivative(0), - _d_lpf_alpha(AC_PID_D_TERM_FILTER) - { - AP_Param::setup_object_defaults(this, var_info); + // Constructor for PID + AC_PID(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float dt); - _kp = initial_p; - _ki = initial_i; - _kd = initial_d; - _imax = abs(initial_imax); + // set_dt - set time step in seconds + void set_dt(float dt); - // derivative is invalid on startup - _last_derivative = NAN; - } + // set_filt_hz - set input filter hz + void set_filt_hz(float hz); - /// 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. - /// - 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); + // set_input_filter_all - 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_filter_all(float input); - /// Reset the PID integrator - /// + // 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(float input); + + // get_pid - get results from pid controller + float get_pid(); + float get_pi(); + float get_p() const; + float get_i(); + float get_d() const; + + // reset_I - reset the integrator void reset_I(); - /// Load gain properties - /// + // 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 properties - /// + // save gain to eeprom void save_gains(); - - /// Sets filter Alpha for D-term LPF - void set_d_lpf_alpha(int16_t cutoff_frequency, float time_step); - /// @name parameter accessors - //@{ + /// operator function call for easy initialisation + void operator() (float p, float i, float d, float imaxval, float input_filt_hz, float dt ); - /// 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 = abs(imaxval); - } - - // accessors + // get 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(); } + 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 kD(const float v) { _kd.set(v); } - void imax(const int16_t v) { _imax.set(abs(v)); } + void imax(const float v) { _imax.set(fabs(v)); } + void filt_hz(const float v) { _filt_hz.set(fabs(v)); } + float get_integrator() const { return _integrator; } void set_integrator(float i) { _integrator = i; } + // 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 _kd; - AP_Int16 _imax; + AP_Float _imax; + AP_Float _filt_hz; // PID Input filter frequency in Hz - float _integrator; ///< integrator value - float _last_input; ///< last input for derivative - float _last_derivative; ///< last derivative for low-pass filter - float _d_lpf_alpha; ///< alpha used in D-term LPF + // 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 _integrator; // integrator value + float _input; // last input for derivative + float _derivative; // last derivative for low-pass filter + float _filt_alpha; // input filter alpha }; #endif // __AC_PID_H__