diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp index 6864072a30..166be7b7b3 100644 --- a/libraries/AC_PID/AC_PID.cpp +++ b/libraries/AC_PID/AC_PID.cpp @@ -198,6 +198,7 @@ float AC_PID::update_all(float target, float measurement, float dt, bool limit, } // reset input filter to value received + _pid_info.reset = _flags._reset_filter; if (_flags._reset_filter) { _flags._reset_filter = false; _target = target; @@ -319,6 +320,10 @@ void AC_PID::update_i(float dt, bool limit) } _pid_info.I = _integrator; _pid_info.limit = limit; + + // Set I set flag for logging and clear + _pid_info.I_term_set = _flags._I_set; + _flags._I_set = false; } float AC_PID::get_p() const @@ -343,6 +348,7 @@ float AC_PID::get_ff() const void AC_PID::reset_I() { + _flags._I_set = true; _integrator = 0.0; } @@ -404,6 +410,7 @@ float AC_PID::get_filt_D_alpha(float dt) const void AC_PID::set_integrator(float integrator) { + _flags._I_set = true; _integrator = constrain_float(integrator, -_kimax, _kimax); } @@ -411,6 +418,7 @@ void AC_PID::relax_integrator(float integrator, float dt, float time_constant) { integrator = constrain_float(integrator, -_kimax, _kimax); if (is_positive(dt)) { + _flags._I_set = true; _integrator = _integrator + (integrator - _integrator) * (dt / (dt + time_constant)); } } diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h index dce0082143..fbbebcac7d 100644 --- a/libraries/AC_PID/AC_PID.h +++ b/libraries/AC_PID/AC_PID.h @@ -184,6 +184,7 @@ protected: // flags struct ac_pid_flags { bool _reset_filter :1; // true when input filter should be reset during next call to set_input + bool _I_set :1; // true if if the I terms has been set externally including zeroing } _flags; // internal variables diff --git a/libraries/AC_PID/AP_PIDInfo.h b/libraries/AC_PID/AP_PIDInfo.h index 70afb0974e..992a287ec8 100644 --- a/libraries/AC_PID/AP_PIDInfo.h +++ b/libraries/AC_PID/AP_PIDInfo.h @@ -19,4 +19,6 @@ struct AP_PIDInfo { float slew_rate; bool limit; bool PD_limit; + bool reset; + bool I_term_set; };