diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp index 60184d9702..9275cf2fb5 100644 --- a/libraries/AC_PID/AC_PID.cpp +++ b/libraries/AC_PID/AC_PID.cpp @@ -8,45 +8,62 @@ const AP_Param::GroupInfo AC_PID::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, _kp, 0), + 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), + 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), + 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", 5, AC_PID, _imax, 0), - - // @Param: FILT - // @DisplayName: PID Input filter frequency in Hz - // @Description: Input filter frequency in Hz - // @Units: Hz - AP_GROUPINFO("FILT", 6, AC_PID, _filt_hz, AC_PID_FILT_HZ_DEFAULT), // @Param: FF // @DisplayName: FF FeedForward Gain // @Description: FF Gain which produces an output value that is proportional to the demanded input - AP_GROUPINFO("FF", 7, AC_PID, _ff, 0), - + AP_GROUPINFO("FF", 4, AC_PID, _kff, 0), + + // @Param: IMAX + // @DisplayName: PID Integral Maximum + // @Description: The maximum/minimum value that the I term can output + AP_GROUPINFO("IMAX", 5, AC_PID, _kimax, 0), + + // 6 was for float FILT + + // 7 is for float ILMI and FF + + // index 8 was for AFF + + // @Param: FLTT + // @DisplayName: PID Target filter frequency in Hz + // @Description: Target filter frequency in Hz + // @Units: Hz + AP_GROUPINFO("FLTT", 9, AC_PID, _filt_T_hz, AC_PID_TFILT_HZ_DEFAULT), + + // @Param: FLTE + // @DisplayName: PID Error filter frequency in Hz + // @Description: Error filter frequency in Hz + // @Units: Hz + AP_GROUPINFO("FLTE", 10, AC_PID, _filt_E_hz, AC_PID_EFILT_HZ_DEFAULT), + + // @Param: FLTD + // @DisplayName: PID Derivative term filter frequency in Hz + // @Description: Derivative filter frequency in Hz + // @Units: Hz + AP_GROUPINFO("FLTD", 11, AC_PID, _filt_D_hz, AC_PID_DFILT_HZ_DEFAULT), + AP_GROUPEND }; // Constructor -AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float dt, float initial_ff) : +AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_T_hz, float initial_filt_E_hz, float initial_filt_D_hz, float dt) : _dt(dt), _integrator(0.0f), - _input(0.0f), + _error(0.0f), _derivative(0.0f) { // load parameter values from eeprom @@ -55,9 +72,11 @@ AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_ _kp = initial_p; _ki = initial_i; _kd = initial_d; - _imax = fabsf(initial_imax); - filt_hz(initial_filt_hz); - _ff = initial_ff; + _kff = initial_ff; + _kimax = fabsf(initial_imax); + filt_T_hz(initial_filt_T_hz); + filt_E_hz(initial_filt_E_hz); + filt_D_hz(initial_filt_D_hz); // reset input filter to first value received _flags._reset_filter = true; @@ -72,109 +91,157 @@ void AC_PID::set_dt(float dt) _dt = dt; } -// filt_hz - set input filter hz -void AC_PID::filt_hz(float hz) +// filt_T_hz - set target filter hz +void AC_PID::filt_T_hz(float hz) { - _filt_hz.set(fabsf(hz)); - - // sanity check _filt_hz - _filt_hz = MAX(_filt_hz, AC_PID_FILT_HZ_MIN); + _filt_T_hz.set(fabsf(hz)); } -// 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) +// filt_E_hz - set error filter hz +void AC_PID::filt_E_hz(float hz) +{ + _filt_E_hz.set(fabsf(hz)); +} + +// filt_D_hz - set derivative filter hz +void AC_PID::filt_D_hz(float hz) +{ + _filt_D_hz.set(fabsf(hz)); +} + +// 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::update_all(float target, float measurement, bool limit) { // don't process inf or NaN - if (!isfinite(input)) { - return; + if (!isfinite(target) || !isfinite(measurement)) { + return 0.0f; } // reset input filter to value received if (_flags._reset_filter) { _flags._reset_filter = false; - _input = input; + _target = target; + _error = _target - measurement; _derivative = 0.0f; - } + } else { + float error_last = _error; + _target += get_filt_T_alpha() * (target - _target); + _error += get_filt_E_alpha() * ((_target - measurement) - _error); - // update filter and calculate derivative - float input_filt_change = get_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) -{ - // don't process inf or NaN - if (!isfinite(input)) { - return; - } - - // reset input filter to value received - if (_flags._reset_filter) { - _flags._reset_filter = false; - _input = input; - _derivative = 0.0f; - } - - // update filter and calculate derivative - if (_dt > 0.0f) { - float derivative = (input - _input) / _dt; - _derivative = _derivative + get_filt_alpha() * (derivative-_derivative); - } - - _input = input; -} - -float AC_PID::get_p() -{ - _pid_info.P = (_input * _kp); - return _pid_info.P; -} - -float AC_PID::get_i() -{ - if(!is_zero(_ki) && !is_zero(_dt)) { - _integrator += ((float)_input * _ki) * _dt; - if (_integrator < -_imax) { - _integrator = -_imax; - } else if (_integrator > _imax) { - _integrator = _imax; + // calculate and filter derivative + if (_dt > 0.0f) { + float derivative = (_error - error_last) / _dt; + _derivative += get_filt_D_alpha() * (derivative - _derivative); } - _pid_info.I = _integrator; - return _integrator; } - return 0; + + // update I term + update_i(limit); + + float P_out = (_error * _kp); + float D_out = (_derivative * _kd); + + _pid_info.target = _target; + _pid_info.actual = measurement; + _pid_info.error = _error; + _pid_info.P = P_out; + _pid_info.D = D_out; + + return P_out + _integrator + D_out; } -float AC_PID::get_d() +// update_error - set error input to PID controller and calculate outputs +// target is set to zero and error is set and filtered +// the derivative then is calculated and filtered +// the integral is then updated based on the setting of the limit flag +// Target and Measured must be set manually for logging purposes. +// todo: remove function when it is no longer used. +float AC_PID::update_error(float error, bool limit) { - // derivative component - _pid_info.D = (_kd * _derivative); - return _pid_info.D; + // don't process inf or NaN + if (!isfinite(error)) { + return 0.0f; + } + + _target = 0.0f; + + // reset input filter to value received + if (_flags._reset_filter) { + _flags._reset_filter = false; + _error = error; + _derivative = 0.0f; + } else { + float error_last = _error; + _error += get_filt_E_alpha() * (error - _error); + + // calculate and filter derivative + if (_dt > 0.0f) { + float derivative = (_error - error_last) / _dt; + _derivative += get_filt_D_alpha() * (derivative - _derivative); + } + } + + // update I term + update_i(limit); + + float P_out = (_error * _kp); + float D_out = (_derivative * _kd); + + _pid_info.target = 0.0f; + _pid_info.actual = 0.0f; + _pid_info.error = _error; + _pid_info.P = P_out; + _pid_info.D = D_out; + + return P_out + _integrator + D_out; } -float AC_PID::get_ff(float requested_rate) +// update_i - update the integral +// If the limit flag is set the integral is only allowed to shrink +void AC_PID::update_i(bool limit) { - _pid_info.FF = (float)requested_rate * _ff; - return _pid_info.FF; + if (!is_zero(_ki) && is_positive(_dt)) { + // Ensure that integrator can only be reduced if the output is saturated + if (!limit || ((is_positive(_integrator) && is_negative(_error)) || (is_negative(_integrator) && is_positive(_error)))) { + _integrator += ((float)_error * _ki) * _dt; + _integrator = constrain_float(_integrator, -_kimax, _kimax); + } + } else { + _integrator = 0.0f; + } + _pid_info.I = _integrator; } - -float AC_PID::get_pi() +float AC_PID::get_p() const { - return get_p() + get_i(); + return _error * _kp; } -float AC_PID::get_pid() +float AC_PID::get_i() const { - return get_p() + get_i() + get_d(); + return _integrator; +} + +float AC_PID::get_d() const +{ + return _kd * _derivative; +} + +float AC_PID::get_ff() +{ + _pid_info.FF = _target * _kff; + return _target * _kff; +} + +// todo: remove function when it is no longer used. +float AC_PID::get_ff(float target) +{ + float FF_out = (target * _kff); + _pid_info.FF = FF_out; + return FF_out; } void AC_PID::reset_I() @@ -187,9 +254,12 @@ void AC_PID::load_gains() _kp.load(); _ki.load(); _kd.load(); - _imax.load(); - _imax = fabsf(_imax); - _filt_hz.load(); + _kff.load(); + _kimax.load(); + _kimax = fabsf(_kimax); + _filt_T_hz.load(); + _filt_E_hz.load(); + _filt_D_hz.load(); } // save_gains - save gains to eeprom @@ -198,30 +268,63 @@ void AC_PID::save_gains() _kp.save(); _ki.save(); _kd.save(); - _imax.save(); - _filt_hz.save(); + _kff.save(); + _kimax.save(); + _filt_T_hz.save(); + _filt_E_hz.save(); + _filt_D_hz.save(); } /// 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, float ffval) +void AC_PID::operator()(float p_val, float i_val, float d_val, float ff_val, float imax_val, float input_filt_T_hz, float input_filt_E_hz, float input_filt_D_hz, float dt) { - _kp = p; - _ki = i; - _kd = d; - _imax = fabsf(imaxval); - _filt_hz = input_filt_hz; + _kp = p_val; + _ki = i_val; + _kd = d_val; + _kff = ff_val; + _kimax = fabsf(imax_val); + _filt_T_hz = input_filt_T_hz; + _filt_E_hz = input_filt_E_hz; + _filt_D_hz = input_filt_D_hz; _dt = dt; - _ff = ffval; } -// calc_filt_alpha - recalculate the input filter alpha -float AC_PID::get_filt_alpha() const +// get_filt_T_alpha - get the target filter alpha +float AC_PID::get_filt_T_alpha() const { - if (is_zero(_filt_hz)) { + return get_filt_alpha(_filt_T_hz); +} + +// get_filt_E_alpha - get the error filter alpha +float AC_PID::get_filt_E_alpha() const +{ + return get_filt_alpha(_filt_E_hz); +} + +// get_filt_D_alpha - get the derivative filter alpha +float AC_PID::get_filt_D_alpha() const +{ + return get_filt_alpha(_filt_D_hz); +} + +// get_filt_alpha - calculate a filter alpha +float AC_PID::get_filt_alpha(float filt_hz) const +{ + if (is_zero(filt_hz)) { return 1.0f; } // calculate alpha - float rc = 1/(M_2PI*_filt_hz); + float rc = 1 / (M_2PI * filt_hz); return _dt / (_dt + rc); } + +void AC_PID::set_integrator(float target, float measurement, float i) +{ + set_integrator(target - measurement, i); +} + +void AC_PID::set_integrator(float error, float i) +{ + _integrator = constrain_float(i - error * _kp, -_kimax, _kimax); +} diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h index 8eec05bdb2..2f2529a59e 100644 --- a/libraries/AC_PID/AC_PID.h +++ b/libraries/AC_PID/AC_PID.h @@ -9,8 +9,9 @@ #include #include -#define AC_PID_FILT_HZ_DEFAULT 20.0f // default input filter frequency -#define AC_PID_FILT_HZ_MIN 0.01f // minimum input filter frequency +#define AC_PID_TFILT_HZ_DEFAULT 0.0f // default input filter frequency +#define AC_PID_EFILT_HZ_DEFAULT 0.0f // default input filter frequency +#define AC_PID_DFILT_HZ_DEFAULT 20.0f // default input filter frequency /// @class AC_PID /// @brief Copter PID control class @@ -18,93 +19,118 @@ class AC_PID { public: // Constructor for PID - AC_PID(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float dt, float initial_ff = 0); + AC_PID(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_T_hz, float initial_filt_E_hz, float initial_filt_D_hz, float dt); // set_dt - set time step in seconds - void set_dt(float dt); + void set_dt(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); + // 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 update_all(float target, float measurement, bool limit = false); - // 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); + // update_error - set error input to PID controller and calculate outputs + // target is set to zero and error is set and filtered + // the derivative then is calculated and filtered + // the integral is then updated based on the setting of the limit flag + // Target and Measured must be set manually for logging purposes. + // todo: remove function when it is no longer used. + float update_error(float error, bool limit = false); + + // update_i - update the integral + // if the limit flag is set the integral is only allowed to shrink + void update_i(bool limit); // get_pid - get results from pid controller - float get_pid(); - float get_pi(); - float get_p(); - float get_i(); - float get_d(); - float get_ff(float requested_rate); + float get_pid() const; + float get_pi() const; + float get_p() const; + float get_i() const; + float get_d() const; + float get_ff(); + + // todo: remove function when it is no longer used. + float get_ff(float target); // reset_I - reset the integrator - void reset_I(); + void reset_I(); // reset_filter - input filter will be reset to the next value provided to set_input() - void reset_filter() { _flags._reset_filter = true; } + void reset_filter() { + _flags._reset_filter = true; + } // load gain from eeprom - void load_gains(); + void load_gains(); // save gain to eeprom - void save_gains(); + void save_gains(); /// operator function call for easy initialisation - void operator() (float p, float i, float d, float imaxval, float input_filt_hz, float dt, float ffval = 0); + void operator()(float p_val, float i_val, float d_val, float ff_val, float imax_val, float input_filt_T_hz, float input_filt_E_hz, float input_filt_D_hz, float dt); // get accessors - AP_Float &kP() { return _kp; } - AP_Float &kI() { return _ki; } - AP_Float &kD() { return _kd; } - AP_Float &filt_hz() { return _filt_hz; } - float imax() const { return _imax.get(); } - float get_filt_alpha() const; - float ff() const { return _ff.get(); } + AP_Float &kP() { return _kp; } + AP_Float &kI() { return _ki; } + AP_Float &kD() { return _kd; } + AP_Float &ff() { return _kff;} + AP_Float &filt_T_hz() { return _filt_T_hz; } + 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_alpha(float filt_hz) const; + float get_filt_T_alpha() const; + 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 ff(const float v) { _ff.set(v); } - - float get_integrator() const { return _integrator; } - void set_integrator(float i) { _integrator = i; } + 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 ff(const float v) { _kff.set(v); } + void imax(const float v) { _kimax.set(fabsf(v)); } + void filt_T_hz(const float v); + void filt_E_hz(const float v); + void filt_D_hz(const float v); // set the desired and actual rates (for logging purposes) - void set_desired_rate(float desired) { _pid_info.desired = desired; } - void set_actual_rate(float actual) { _pid_info.actual = actual; } + void set_target_rate(float target) { _pid_info.target = target; } + void set_actual_rate(float actual) { _pid_info.actual = actual; } - const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; } + // integrator setting functions + void set_integrator(float target, float measurement, float i); + void set_integrator(float error, float i); + void set_integrator(float i) { _integrator = constrain_float(i, -_kimax, _kimax); } + + const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; } // parameter var table - static const struct AP_Param::GroupInfo var_info[]; + static const struct AP_Param::GroupInfo var_info[]; protected: // 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 _ff; + AP_Float _kp; + AP_Float _ki; + AP_Float _kd; + AP_Float _kff; + AP_Float _kimax; + AP_Float _filt_T_hz; // PID target filter frequency in Hz + AP_Float _filt_E_hz; // PID error filter frequency in Hz + AP_Float _filt_D_hz; // PID derivative 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 + 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 _dt; // timestep in seconds + float _integrator; // integrator value + float _target; // target value to enable filtering + float _error; // error value to enable filtering + float _derivative; // derivative value to enable filtering - AP_Logger::PID_Info _pid_info; + AP_Logger::PID_Info _pid_info; }; diff --git a/libraries/AC_PID/examples/AC_PID_test/AC_PID_test.cpp b/libraries/AC_PID/examples/AC_PID_test/AC_PID_test.cpp index 25fec3e14a..0574fdcad9 100644 --- a/libraries/AC_PID/examples/AC_PID_test/AC_PID_test.cpp +++ b/libraries/AC_PID/examples/AC_PID_test/AC_PID_test.cpp @@ -70,8 +70,8 @@ void setup() void loop() { // setup (unfortunately must be done here as we cannot create a global AC_PID object) - AC_PID pid(TEST_P, TEST_I, TEST_D, TEST_IMAX * 100, TEST_FILTER, TEST_DT); - AC_HELI_PID heli_pid(TEST_P, TEST_I, TEST_D, TEST_IMAX * 100, TEST_FILTER, TEST_DT, TEST_INITIAL_FF); + AC_PID pid(TEST_P, TEST_I, TEST_D, 0.0f, TEST_IMAX * 100.0f, 0.0f, 0.0f, TEST_FILTER, TEST_DT); + AC_HELI_PID heli_pid(TEST_P, TEST_I, TEST_D, TEST_INITIAL_FF, TEST_IMAX * 100, 0.0f, 0.0f, TEST_FILTER, TEST_DT); // display PID gains hal.console->printf("P %f I %f D %f imax %f\n", (double)pid.kP(), (double)pid.kI(), (double)pid.kD(), (double)pid.imax()); @@ -91,7 +91,7 @@ void loop() rc().read_input(); // poll the radio for new values const uint16_t radio_in = c->get_radio_in(); const int16_t error = radio_in - radio_trim; - pid.set_input_filter_all(error); + pid.update_error(error); const float control_P = pid.get_p(); const float control_I = pid.get_i(); const float control_D = pid.get_d();