From 95b375dc87346c4b739608214492380b8a22d552 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 4 Apr 2021 19:49:31 +1000 Subject: [PATCH] AC_PID: log SRate in pid info and expose slew rate --- libraries/AC_PID/AC_PID.cpp | 2 ++ libraries/AC_PID/AC_PID.h | 5 +++++ 2 files changed, 7 insertions(+) diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp index 4a6478c1fb..7887ac2e64 100644 --- a/libraries/AC_PID/AC_PID.cpp +++ b/libraries/AC_PID/AC_PID.cpp @@ -161,6 +161,7 @@ float AC_PID::update_all(float target, float measurement, bool limit) // 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.slew_rate = _slew_limiter.get_slew_rate(); P_out *= _pid_info.Dmod; D_out *= _pid_info.Dmod; @@ -213,6 +214,7 @@ float AC_PID::update_error(float error, bool limit) // 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.slew_rate = _slew_limiter.get_slew_rate(); 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 2f0e0c892f..3476643fdd 100644 --- a/libraries/AC_PID/AC_PID.h +++ b/libraries/AC_PID/AC_PID.h @@ -82,6 +82,8 @@ public: 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; } + 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; @@ -110,6 +112,9 @@ public: // set slew limiter scale factor void set_slew_limit_scale(int8_t scale) { _slew_limit_scale = scale; } + // return current slew rate of slew limiter. Will return 0 if SMAX is zero + float get_slew_rate(void) const { return _slew_limiter.get_slew_rate(); } + const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; } // parameter var table