diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp index 961bcfed09..4a6478c1fb 100644 --- a/libraries/AC_PID/AC_PID.cpp +++ b/libraries/AC_PID/AC_PID.cpp @@ -93,6 +93,10 @@ AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_ _flags._reset_filter = true; memset(&_pid_info, 0, sizeof(_pid_info)); + + // slew limit scaler allows for plane to use degrees/sec slew + // limit + _slew_limit_scale = 1; } // set_dt - set time step in seconds @@ -156,7 +160,7 @@ float AC_PID::update_all(float target, float measurement, bool limit) float D_out = (_derivative * _kd); // calculate slew limit modifier for P+D - _pid_info.Dmod = _slew_limiter.modifier(_pid_info.P + _pid_info.D, _dt); + _pid_info.Dmod = _slew_limiter.modifier((_pid_info.P + _pid_info.D) * _slew_limit_scale, _dt); P_out *= _pid_info.Dmod; D_out *= _pid_info.Dmod; @@ -208,7 +212,7 @@ float AC_PID::update_error(float error, bool limit) float D_out = (_derivative * _kd); // calculate slew limit modifier for P+D - _pid_info.Dmod = _slew_limiter.modifier(_pid_info.P + _pid_info.D, _dt); + _pid_info.Dmod = _slew_limiter.modifier((_pid_info.P + _pid_info.D) * _slew_limit_scale, _dt); P_out *= _pid_info.Dmod; D_out *= _pid_info.Dmod; diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h index e556ce544d..2f0e0c892f 100644 --- a/libraries/AC_PID/AC_PID.h +++ b/libraries/AC_PID/AC_PID.h @@ -107,6 +107,9 @@ public: void set_integrator(float error, float i); void set_integrator(float i); + // set slew limiter scale factor + void set_slew_limit_scale(int8_t scale) { _slew_limit_scale = scale; } + const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; } // parameter var table @@ -144,6 +147,7 @@ protected: float _target; // target value to enable filtering float _error; // error value to enable filtering float _derivative; // derivative value to enable filtering + int8_t _slew_limit_scale; uint16_t _reset_counter; // loop counter for reset decay uint64_t _reset_last_update; //time in microseconds of last update to reset_I