diff --git a/libraries/AC_PID/AC_HELI_PID.cpp b/libraries/AC_PID/AC_HELI_PID.cpp index 9236c01568..f514a955d2 100644 --- a/libraries/AC_PID/AC_HELI_PID.cpp +++ b/libraries/AC_PID/AC_HELI_PID.cpp @@ -73,8 +73,8 @@ const AP_Param::GroupInfo AC_HELI_PID::var_info[] = { }; /// Constructor for PID -AC_HELI_PID::AC_HELI_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) : - AC_PID(initial_p, initial_i, initial_d, initial_ff, initial_imax, initial_filt_T_hz, initial_filt_E_hz, initial_filt_D_hz, dt) +AC_HELI_PID::AC_HELI_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) : + AC_PID(initial_p, initial_i, initial_d, initial_ff, initial_imax, initial_filt_T_hz, initial_filt_E_hz, initial_filt_D_hz) { _last_requested_rate = 0; } @@ -84,7 +84,7 @@ AC_HELI_PID::AC_HELI_PID(float initial_p, float initial_i, float initial_d, floa void AC_HELI_PID::update_leaky_i(float leak_rate) { - if (!is_zero(_ki) && !is_zero(_dt)){ + if (!is_zero(_ki)){ // integrator does not leak down below Leak Min if (_integrator > _leak_min){ diff --git a/libraries/AC_PID/AC_HELI_PID.h b/libraries/AC_PID/AC_HELI_PID.h index 8a6b7274a4..0ed7737f18 100644 --- a/libraries/AC_PID/AC_HELI_PID.h +++ b/libraries/AC_PID/AC_HELI_PID.h @@ -17,7 +17,7 @@ class AC_HELI_PID : public AC_PID { public: /// Constructor for PID - AC_HELI_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); + AC_HELI_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); CLASS_NO_COPY(AC_HELI_PID); diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp index a38b811676..bd628108c5 100644 --- a/libraries/AC_PID/AC_PID.cpp +++ b/libraries/AC_PID/AC_PID.cpp @@ -69,8 +69,7 @@ const AP_Param::GroupInfo AC_PID::var_info[] = { // Constructor 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, float initial_srmax, float initial_srtau): - _dt(dt) + float initial_srmax, float initial_srtau) { // load parameter values from eeprom AP_Param::setup_object_defaults(this, var_info); @@ -96,13 +95,6 @@ AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_ _slew_limit_scale = 1; } -// set_dt - set time step in seconds -void AC_PID::set_dt(float dt) -{ - // set dt and calculate the input filter alpha - _dt = dt; -} - // filt_T_hz - set target filter hz void AC_PID::filt_T_hz(float hz) { @@ -131,7 +123,7 @@ void AC_PID::slew_limit(float smax) // 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) +float AC_PID::update_all(float target, float measurement, float dt, bool limit) { // don't process inf or NaN if (!isfinite(target) || !isfinite(measurement)) { @@ -146,24 +138,24 @@ float AC_PID::update_all(float target, float measurement, bool limit) _derivative = 0.0f; } else { float error_last = _error; - _target += get_filt_T_alpha() * (target - _target); - _error += get_filt_E_alpha() * ((_target - measurement) - _error); + _target += get_filt_T_alpha(dt) * (target - _target); + _error += get_filt_E_alpha(dt) * ((_target - measurement) - _error); // calculate and filter derivative - if (_dt > 0.0f) { - float derivative = (_error - error_last) / _dt; - _derivative += get_filt_D_alpha() * (derivative - _derivative); + if (is_positive(dt)) { + float derivative = (_error - error_last) / dt; + _derivative += get_filt_D_alpha(dt) * (derivative - _derivative); } } // update I term - update_i(limit); + update_i(dt, limit); float P_out = (_error * _kp); float D_out = (_derivative * _kd); // calculate slew limit modifier for P+D - _pid_info.Dmod = _slew_limiter.modifier((_pid_info.P + _pid_info.D) * _slew_limit_scale, _dt); + _pid_info.Dmod = _slew_limiter.modifier((_pid_info.P + _pid_info.D) * _slew_limit_scale, dt); _pid_info.slew_rate = _slew_limiter.get_slew_rate(); P_out *= _pid_info.Dmod; @@ -184,7 +176,7 @@ float AC_PID::update_all(float target, float measurement, bool limit) // 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) +float AC_PID::update_error(float error, float dt, bool limit) { // don't process inf or NaN if (!isfinite(error)) { @@ -200,23 +192,23 @@ float AC_PID::update_error(float error, bool limit) _derivative = 0.0f; } else { float error_last = _error; - _error += get_filt_E_alpha() * (error - _error); + _error += get_filt_E_alpha(dt) * (error - _error); // calculate and filter derivative - if (_dt > 0.0f) { - float derivative = (_error - error_last) / _dt; - _derivative += get_filt_D_alpha() * (derivative - _derivative); + if (is_positive(dt)) { + float derivative = (_error - error_last) / dt; + _derivative += get_filt_D_alpha(dt) * (derivative - _derivative); } } // update I term - update_i(limit); + update_i(dt, limit); float P_out = (_error * _kp); float D_out = (_derivative * _kd); // calculate slew limit modifier for P+D - _pid_info.Dmod = _slew_limiter.modifier((_pid_info.P + _pid_info.D) * _slew_limit_scale, _dt); + _pid_info.Dmod = _slew_limiter.modifier((_pid_info.P + _pid_info.D) * _slew_limit_scale, dt); _pid_info.slew_rate = _slew_limiter.get_slew_rate(); P_out *= _pid_info.Dmod; @@ -233,12 +225,12 @@ float AC_PID::update_error(float error, bool limit) // 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) +void AC_PID::update_i(float dt, bool limit) { - if (!is_zero(_ki) && is_positive(_dt)) { + 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 += ((float)_error * _ki) * dt; _integrator = constrain_float(_integrator, -_kimax, _kimax); } } else { @@ -302,7 +294,7 @@ void AC_PID::save_gains() } /// Overload the function call operator to permit easy initialisation -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) +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) { _kp.set(p_val); _ki.set(i_val); @@ -312,31 +304,24 @@ void AC_PID::operator()(float p_val, float i_val, float d_val, float ff_val, flo _filt_T_hz.set(input_filt_T_hz); _filt_E_hz.set(input_filt_E_hz); _filt_D_hz.set(input_filt_D_hz); - _dt = dt; } // get_filt_T_alpha - get the target filter alpha -float AC_PID::get_filt_T_alpha() const +float AC_PID::get_filt_T_alpha(float dt) const { - return get_filt_alpha(_filt_T_hz); + return calc_lowpass_alpha_dt(dt, _filt_T_hz); } // get_filt_E_alpha - get the error filter alpha -float AC_PID::get_filt_E_alpha() const +float AC_PID::get_filt_E_alpha(float dt) const { - return get_filt_alpha(_filt_E_hz); + return calc_lowpass_alpha_dt(dt, _filt_E_hz); } // get_filt_D_alpha - get the derivative filter alpha -float AC_PID::get_filt_D_alpha() const +float AC_PID::get_filt_D_alpha(float dt) 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 -{ - return calc_lowpass_alpha_dt(_dt, filt_hz); + return calc_lowpass_alpha_dt(dt, _filt_D_hz); } void AC_PID::set_integrator(float target, float measurement, float integrator) @@ -356,9 +341,11 @@ void AC_PID::set_integrator(float integrator) _pid_info.I = _integrator; } -void AC_PID::relax_integrator(float integrator, float time_constant) +void AC_PID::relax_integrator(float integrator, float dt, float time_constant) { integrator = constrain_float(integrator, -_kimax, _kimax); - _integrator = _integrator + (integrator - _integrator) * (_dt / (_dt + time_constant)); + if (is_positive(dt)) { + _integrator = _integrator + (integrator - _integrator) * (dt / (dt + time_constant)); + } _pid_info.I = _integrator; } diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h index cd291a02fc..8e4f735c4c 100644 --- a/libraries/AC_PID/AC_PID.h +++ b/libraries/AC_PID/AC_PID.h @@ -23,18 +23,15 @@ public: // Constructor for 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, float initial_srmax=0, float initial_srtau=1.0); + float initial_srmax=0, float initial_srtau=1.0); CLASS_NO_COPY(AC_PID); - // set_dt - set time step in seconds - void set_dt(float dt); - // 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); + float update_all(float target, float measurement, float dt, bool limit = false); // update_error - set error input to PID controller and calculate outputs // target is set to zero and error is set and filtered @@ -42,11 +39,11 @@ public: // 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); + float update_error(float error, float dt, 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); + void update_i(float dt, bool limit); // get_pid - get results from pid controller float get_pid() const; @@ -71,7 +68,7 @@ public: void save_gains(); /// operator function call for easy initialisation - 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); + 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); // get accessors AP_Float &kP() { return _kp; } @@ -85,10 +82,9 @@ public: AP_Float &slew_limit() { return _slew_rate_max; } 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; + float get_filt_T_alpha(float dt) const; + float get_filt_E_alpha(float dt) const; + float get_filt_D_alpha(float dt) const; // set accessors void kP(const float v) { _kp.set(v); } @@ -109,7 +105,7 @@ public: void set_integrator(float target, float measurement, float i); void set_integrator(float error, float i); void set_integrator(float i); - void relax_integrator(float integrator, float time_constant); + void relax_integrator(float integrator, float dt, float time_constant); // set slew limiter scale factor void set_slew_limit_scale(int8_t scale) { _slew_limit_scale = scale; } @@ -149,7 +145,6 @@ protected: } _flags; // internal variables - float _dt; // timestep in seconds float _integrator; // integrator value float _target; // target value to enable filtering float _error; // error value to enable filtering diff --git a/libraries/AC_PID/AC_PID_2D.cpp b/libraries/AC_PID/AC_PID_2D.cpp index 16ea099ff6..99d188bec7 100644 --- a/libraries/AC_PID/AC_PID_2D.cpp +++ b/libraries/AC_PID/AC_PID_2D.cpp @@ -50,8 +50,7 @@ const AP_Param::GroupInfo AC_PID_2D::var_info[] = { }; // Constructor -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) +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) { // load parameter values from eeprom AP_Param::setup_object_defaults(this, var_info); @@ -72,7 +71,7 @@ AC_PID_2D::AC_PID_2D(float initial_kP, float initial_kI, float initial_kD, float // target and error are filtered // the derivative is then calculated and filtered // the integral is then updated if it does not increase in the direction of the limit vector -Vector2f AC_PID_2D::update_all(const Vector2f &target, const Vector2f &measurement, const Vector2f &limit) +Vector2f AC_PID_2D::update_all(const Vector2f &target, const Vector2f &measurement, float dt, const Vector2f &limit) { // don't process inf or NaN if (target.is_nan() || target.is_inf() || @@ -89,17 +88,17 @@ Vector2f AC_PID_2D::update_all(const Vector2f &target, const Vector2f &measureme _derivative.zero(); } else { Vector2f error_last{_error}; - _error += ((_target - measurement) - _error) * get_filt_E_alpha(); + _error += ((_target - measurement) - _error) * get_filt_E_alpha(dt); // calculate and filter derivative - if (_dt > 0.0f) { - const Vector2f derivative{(_error - error_last) / _dt}; - _derivative += (derivative - _derivative) * get_filt_D_alpha(); + if (is_positive(dt)) { + const Vector2f derivative{(_error - error_last) / dt}; + _derivative += (derivative - _derivative) * get_filt_D_alpha(dt); } } // update I term - update_i(limit); + update_i(dt, limit); _pid_info_x.target = _target.x; _pid_info_x.actual = measurement.x; @@ -120,19 +119,19 @@ Vector2f AC_PID_2D::update_all(const Vector2f &target, const Vector2f &measureme return _error * _kp + _integrator + _derivative * _kd + _target * _kff; } -Vector2f AC_PID_2D::update_all(const Vector3f &target, const Vector3f &measurement, const Vector3f &limit) +Vector2f AC_PID_2D::update_all(const Vector3f &target, const Vector3f &measurement, float dt, const Vector3f &limit) { - return update_all(Vector2f{target.x, target.y}, Vector2f{measurement.x, measurement.y}, Vector2f{limit.x, limit.y}); + return update_all(Vector2f{target.x, target.y}, Vector2f{measurement.x, measurement.y}, dt, Vector2f{limit.x, limit.y}); } // update_i - update the integral // If the limit is set the integral is only allowed to reduce in the direction of the limit -void AC_PID_2D::update_i(const Vector2f &limit) +void AC_PID_2D::update_i(float dt, const Vector2f &limit) { _pid_info_x.limit = false; _pid_info_y.limit = false; - Vector2f delta_integrator = (_error * _ki) * _dt; + Vector2f delta_integrator = (_error * _ki) * dt; float integrator_length = _integrator.length(); _integrator += delta_integrator; // do not let integrator increase in length if delta_integrator is in the direction of limit @@ -186,15 +185,15 @@ void AC_PID_2D::save_gains() } // get the target filter alpha -float AC_PID_2D::get_filt_E_alpha() const +float AC_PID_2D::get_filt_E_alpha(float dt) const { - return calc_lowpass_alpha_dt(_dt, _filt_E_hz); + return calc_lowpass_alpha_dt(dt, _filt_E_hz); } // get the derivative filter alpha -float AC_PID_2D::get_filt_D_alpha() const +float AC_PID_2D::get_filt_D_alpha(float dt) const { - return calc_lowpass_alpha_dt(_dt, _filt_D_hz); + return calc_lowpass_alpha_dt(dt, _filt_D_hz); } void AC_PID_2D::set_integrator(const Vector2f& target, const Vector2f& measurement, const Vector2f& i) diff --git a/libraries/AC_PID/AC_PID_2D.h b/libraries/AC_PID/AC_PID_2D.h index 031f340e93..549152a990 100644 --- a/libraries/AC_PID/AC_PID_2D.h +++ b/libraries/AC_PID/AC_PID_2D.h @@ -15,23 +15,20 @@ class AC_PID_2D { public: // Constructor for PID - 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); + 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); CLASS_NO_COPY(AC_PID_2D); - // set time step in seconds - void set_dt(float dt) { _dt = dt; } - // 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 if it does not increase in the direction of the limit vector - Vector2f update_all(const Vector2f &target, const Vector2f &measurement, const Vector2f &limit); - Vector2f update_all(const Vector3f &target, const Vector3f &measurement, const Vector3f &limit); + Vector2f update_all(const Vector2f &target, const Vector2f &measurement, float dt, const Vector2f &limit); + Vector2f update_all(const Vector3f &target, const Vector3f &measurement, float dt, const Vector3f &limit); // update the integral // if the limit flag is set the integral is only allowed to shrink - void update_i(const Vector2f &limit); + void update_i(float dt, const Vector2f &limit); // get results from pid controller Vector2f get_p() const; @@ -57,8 +54,8 @@ public: 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; + float get_filt_E_alpha(float dt) const; + float get_filt_D_alpha(float dt) const; // set accessors void kP(float v) { _kp.set(v); } @@ -93,7 +90,6 @@ protected: AP_Float _filt_D_hz; // PID derivative filter frequency in Hz // internal variables - 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 diff --git a/libraries/AC_PID/AC_PID_Basic.cpp b/libraries/AC_PID/AC_PID_Basic.cpp index be859852a7..ce192cf140 100644 --- a/libraries/AC_PID/AC_PID_Basic.cpp +++ b/libraries/AC_PID/AC_PID_Basic.cpp @@ -52,8 +52,7 @@ const AP_Param::GroupInfo AC_PID_Basic::var_info[] = { }; // 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) +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) { // load parameter values from eeprom AP_Param::setup_object_defaults(this, var_info); @@ -70,16 +69,16 @@ AC_PID_Basic::AC_PID_Basic(float initial_p, float initial_i, float initial_d, fl _reset_filter = true; } -float AC_PID_Basic::update_all(float target, float measurement, bool limit) +float AC_PID_Basic::update_all(float target, float measurement, float dt, bool limit) { - return update_all(target, measurement, (limit && is_negative(_integrator)), (limit && is_positive(_integrator))); + return update_all(target, measurement, dt, (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) +float AC_PID_Basic::update_all(float target, float measurement, float dt, bool limit_neg, bool limit_pos) { // don't process inf or NaN if (!isfinite(target) || isnan(target) || @@ -97,17 +96,17 @@ float AC_PID_Basic::update_all(float target, float measurement, bool limit_neg, _derivative = 0.0f; } else { float error_last = _error; - _error += get_filt_E_alpha() * ((_target - measurement) - _error); + _error += get_filt_E_alpha(dt) * ((_target - measurement) - _error); // calculate and filter derivative - if (is_positive(_dt)) { - float derivative = (_error - error_last) / _dt; - _derivative += get_filt_D_alpha() * (derivative - _derivative); + if (is_positive(dt)) { + float derivative = (_error - error_last) / dt; + _derivative += get_filt_D_alpha(dt) * (derivative - _derivative); } } // update I term - update_i(limit_neg, limit_pos); + update_i(dt, limit_neg, limit_pos); const float P_out = _error * _kp; const float D_out = _derivative * _kd; @@ -126,12 +125,12 @@ float AC_PID_Basic::update_all(float target, float measurement, bool limit_neg, // 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) +void AC_PID_Basic::update_i(float dt, 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 += ((float)_error * _ki) * dt; _integrator = constrain_float(_integrator, -_kimax, _kimax); } } else { @@ -158,15 +157,15 @@ void AC_PID_Basic::save_gains() } // get_filt_T_alpha - get the target filter alpha -float AC_PID_Basic::get_filt_E_alpha() const +float AC_PID_Basic::get_filt_E_alpha(float dt) const { - return calc_lowpass_alpha_dt(_dt, _filt_E_hz); + 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 +float AC_PID_Basic::get_filt_D_alpha(float dt) const { - return calc_lowpass_alpha_dt(_dt, _filt_D_hz); + return calc_lowpass_alpha_dt(dt, _filt_D_hz); } void AC_PID_Basic::set_integrator(float target, float measurement, float i) diff --git a/libraries/AC_PID/AC_PID_Basic.h b/libraries/AC_PID/AC_PID_Basic.h index 02f564e7bd..25d24ba485 100644 --- a/libraries/AC_PID/AC_PID_Basic.h +++ b/libraries/AC_PID/AC_PID_Basic.h @@ -13,21 +13,18 @@ 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; } + 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); // 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; + float update_all(float target, float measurement, float dt, bool limit = false) WARN_IF_UNUSED; + float update_all(float target, float measurement, float dt, 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); + void update_i(float dt, bool limit_neg, bool limit_pos); // get results from pid controller float get_p() const WARN_IF_UNUSED { return _error * _kp; } @@ -53,8 +50,8 @@ public: 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; + float get_filt_E_alpha(float dt) const WARN_IF_UNUSED; + float get_filt_D_alpha(float dt) const WARN_IF_UNUSED; // set accessors void kP(float v) { _kp.set(v); } @@ -87,7 +84,6 @@ protected: 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 diff --git a/libraries/AC_PID/AC_P_1D.cpp b/libraries/AC_PID/AC_P_1D.cpp index 7827b1de2f..69670839d5 100644 --- a/libraries/AC_PID/AC_P_1D.cpp +++ b/libraries/AC_PID/AC_P_1D.cpp @@ -13,8 +13,7 @@ const AP_Param::GroupInfo AC_P_1D::var_info[] = { }; // Constructor -AC_P_1D::AC_P_1D(float initial_p, float dt) : - _dt(dt) +AC_P_1D::AC_P_1D(float initial_p) { // load parameter values from eeprom AP_Param::setup_object_defaults(this, var_info); @@ -38,7 +37,7 @@ float AC_P_1D::update_all(float &target, float measurement) } // MIN(_Dxy_max, _D2xy_max / _kxy_P) limits the max accel to the point where max jerk is exceeded - return sqrt_controller(_error, _kp, _D1_max, _dt); + return sqrt_controller(_error, _kp, _D1_max, 0.0); } // set_limits - sets the maximum error to limit output and first and second derivative of output diff --git a/libraries/AC_PID/AC_P_1D.h b/libraries/AC_PID/AC_P_1D.h index 5285ebce79..1cdb56e799 100644 --- a/libraries/AC_PID/AC_P_1D.h +++ b/libraries/AC_PID/AC_P_1D.h @@ -12,13 +12,10 @@ class AC_P_1D { public: // constructor - AC_P_1D(float initial_p, float dt); + AC_P_1D(float initial_p); CLASS_NO_COPY(AC_P_1D); - // 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 float update_all(float &target, float measurement) WARN_IF_UNUSED; @@ -56,7 +53,6 @@ private: AP_Float _kp; // internal variables - float _dt; // time step in seconds float _error; // time step in seconds float _error_min; // error limit in negative direction float _error_max; // error limit in positive direction diff --git a/libraries/AC_PID/AC_P_2D.cpp b/libraries/AC_PID/AC_P_2D.cpp index a1e46cf8a4..4cc2df4f63 100644 --- a/libraries/AC_PID/AC_P_2D.cpp +++ b/libraries/AC_PID/AC_P_2D.cpp @@ -13,8 +13,7 @@ const AP_Param::GroupInfo AC_P_2D::var_info[] = { }; // Constructor -AC_P_2D::AC_P_2D(float initial_p, float dt) : - _dt(dt) +AC_P_2D::AC_P_2D(float initial_p) { // load parameter values from eeprom AP_Param::setup_object_defaults(this, var_info); @@ -36,7 +35,7 @@ Vector2f AC_P_2D::update_all(postype_t &target_x, postype_t &target_y, const Vec } // MIN(_Dmax, _D2max / _kp) limits the max accel to the point where max jerk is exceeded - return sqrt_controller(_error, _kp, _D1_max, _dt); + return sqrt_controller(_error, _kp, _D1_max, 0.0); } // set_limits - sets the maximum error to limit output and first and second derivative of output diff --git a/libraries/AC_PID/AC_P_2D.h b/libraries/AC_PID/AC_P_2D.h index 40cf379331..d0dd792172 100644 --- a/libraries/AC_PID/AC_P_2D.h +++ b/libraries/AC_PID/AC_P_2D.h @@ -12,13 +12,10 @@ class AC_P_2D { public: // constructor - AC_P_2D(float initial_p, float dt); + AC_P_2D(float initial_p); CLASS_NO_COPY(AC_P_2D); - // 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(postype_t &target_x, postype_t &target_y, const Vector2f &measurement) WARN_IF_UNUSED; @@ -58,7 +55,6 @@ private: AP_Float _kp; // internal variables - float _dt; // time step in seconds Vector2f _error; // time step in seconds float _error_max; // error limit in positive direction float _D1_max; // maximum first derivative of output 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 0574fdcad9..bc641acd80 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, 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); + AC_PID pid(TEST_P, TEST_I, TEST_D, 0.0f, TEST_IMAX * 100.0f, 0.0f, 0.0f, TEST_FILTER); + AC_HELI_PID heli_pid(TEST_P, TEST_I, TEST_D, TEST_INITIAL_FF, TEST_IMAX * 100, 0.0f, 0.0f, TEST_FILTER); // 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.update_error(error); + pid.update_error(error, TEST_DT); const float control_P = pid.get_p(); const float control_I = pid.get_i(); const float control_D = pid.get_d();