AC_PID: add reset and I term set flags to PIDInfo

This commit is contained in:
Iampete1 2023-12-11 18:44:30 +00:00 committed by Randy Mackay
parent 9146458d4a
commit d53b73468b
3 changed files with 11 additions and 0 deletions

View File

@ -198,6 +198,7 @@ float AC_PID::update_all(float target, float measurement, float dt, bool limit,
} }
// reset input filter to value received // reset input filter to value received
_pid_info.reset = _flags._reset_filter;
if (_flags._reset_filter) { if (_flags._reset_filter) {
_flags._reset_filter = false; _flags._reset_filter = false;
_target = target; _target = target;
@ -319,6 +320,10 @@ void AC_PID::update_i(float dt, bool limit)
} }
_pid_info.I = _integrator; _pid_info.I = _integrator;
_pid_info.limit = limit; _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 float AC_PID::get_p() const
@ -343,6 +348,7 @@ float AC_PID::get_ff() const
void AC_PID::reset_I() void AC_PID::reset_I()
{ {
_flags._I_set = true;
_integrator = 0.0; _integrator = 0.0;
} }
@ -404,6 +410,7 @@ float AC_PID::get_filt_D_alpha(float dt) const
void AC_PID::set_integrator(float integrator) void AC_PID::set_integrator(float integrator)
{ {
_flags._I_set = true;
_integrator = constrain_float(integrator, -_kimax, _kimax); _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); integrator = constrain_float(integrator, -_kimax, _kimax);
if (is_positive(dt)) { if (is_positive(dt)) {
_flags._I_set = true;
_integrator = _integrator + (integrator - _integrator) * (dt / (dt + time_constant)); _integrator = _integrator + (integrator - _integrator) * (dt / (dt + time_constant));
} }
} }

View File

@ -184,6 +184,7 @@ protected:
// flags // flags
struct ac_pid_flags { struct ac_pid_flags {
bool _reset_filter :1; // true when input filter should be reset during next call to set_input 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; } _flags;
// internal variables // internal variables

View File

@ -19,4 +19,6 @@ struct AP_PIDInfo {
float slew_rate; float slew_rate;
bool limit; bool limit;
bool PD_limit; bool PD_limit;
bool reset;
bool I_term_set;
}; };