Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_PID: replace reset_I_smoothly with relax_integrator

This commit is contained in:
Leonard Hall 2022-01-08 21:40:48 +10:30 committed by Randy Mackay
parent 545a20473e
commit 51da9f378b
2 changed files with 15 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AC_PID/AC_PID.cpp
View File

@ -271,24 +271,7 @@ float AC_PID::get_ff()
void AC_PID::reset_I()
{
_integrator = 0;
}
void AC_PID::reset_I_smoothly()
{
float reset_time = AC_PID_RESET_TC * 3.0f;
uint64_t now = AP_HAL::micros64();
if ((now - _reset_last_update) > 5e5 ) {
_reset_counter = 0;
}
if ((float)_reset_counter < (reset_time/_dt)) {
_integrator = _integrator - (_dt / (_dt + AC_PID_RESET_TC)) * _integrator;
_reset_counter++;
} else {
_integrator = 0;
}
_reset_last_update = now;
_integrator = 0.0;
}
void AC_PID::load_gains()
@ -355,19 +338,26 @@ float AC_PID::get_filt_alpha(float filt_hz) const
return calc_lowpass_alpha_dt(_dt, filt_hz);
}
void AC_PID::set_integrator(float target, float measurement, float i)
void AC_PID::set_integrator(float target, float measurement, float integrator)
{
set_integrator(target - measurement, i);
set_integrator(target - measurement, integrator);
}
void AC_PID::set_integrator(float error, float i)
void AC_PID::set_integrator(float error, float integrator)
{
_integrator = constrain_float(i - error * _kp, -_kimax, _kimax);
_integrator = constrain_float(integrator - error * _kp, -_kimax, _kimax);
_pid_info.I = _integrator;
}
void AC_PID::set_integrator(float i)
void AC_PID::set_integrator(float integrator)
{
_integrator = constrain_float(i, -_kimax, _kimax);
_integrator = constrain_float(integrator, -_kimax, _kimax);
_pid_info.I = _integrator;
}
void AC_PID::relax_integrator(float integrator, float time_constant)
{
integrator = constrain_float(integrator, -_kimax, _kimax);
_integrator = _integrator + (integrator - _integrator) * (_dt / (_dt + time_constant));
_pid_info.I = _integrator;
}

6
libraries/AC_PID/AC_PID.h
View File

@ -58,9 +58,6 @@ public:
// reset_I - reset the integrator
void reset_I();
// reset_I - reset the integrator smoothly to zero within 0.5 seconds
void reset_I_smoothly();
// reset_filter - input filter will be reset to the next value provided to set_input()
void reset_filter() {
_flags._reset_filter = true;
@ -111,6 +108,7 @@ public:
void set_integrator(float target, float measurement, float i);
void set_integrator(float error, float i);
void set_integrator(float i);
void relax_integrator(float integrator, float time_constant);
// set slew limiter scale factor
void set_slew_limit_scale(int8_t scale) { _slew_limit_scale = scale; }
@ -156,8 +154,6 @@ protected:
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
AP_Logger::PID_Info _pid_info;
};