From e84a05d5ba4ffad7dacb7165a2637d38d4e71141 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 5 Nov 2021 20:52:42 +0000 Subject: [PATCH] AC_PID: add slew_rate modifier --- libraries/AC_PID/AC_PID.cpp | 6 ++++++ libraries/AC_PID/AC_PID.h | 1 + 2 files changed, 7 insertions(+) diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp index 7f555745cd..9940485352 100644 --- a/libraries/AC_PID/AC_PID.cpp +++ b/libraries/AC_PID/AC_PID.cpp @@ -121,6 +121,12 @@ void AC_PID::filt_D_hz(float hz) _filt_D_hz.set(fabsf(hz)); } +// slew_limit - set slew limit +void AC_PID::slew_limit(float smax) +{ + _slew_rate_max.set(fabsf(smax)); +} + // 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 diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h index faaaabee5e..5b8d8c217a 100644 --- a/libraries/AC_PID/AC_PID.h +++ b/libraries/AC_PID/AC_PID.h @@ -101,6 +101,7 @@ public: void filt_T_hz(const float v); void filt_E_hz(const float v); void filt_D_hz(const float v); + void slew_limit(const float v); // set the desired and actual rates (for logging purposes) void set_target_rate(float target) { _pid_info.target = target; }