From 51da9f378b001a5721d791c3e7916abc39293cba Mon Sep 17 00:00:00 2001
From: Leonard Hall <leonardthall@gmail.com>
Date: Sat, 8 Jan 2022 21:40:48 +1030
Subject: [PATCH] AC_PID: replace reset_I_smoothly with relax_integrator

---
 libraries/AC_PID/AC_PID.cpp | 38 ++++++++++++++-----------------------
 libraries/AC_PID/AC_PID.h   |  6 +-----
 2 files changed, 15 insertions(+), 29 deletions(-)

diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp
index 9940485352..f3f38d0957 100644
--- a/libraries/AC_PID/AC_PID.cpp
+++ b/libraries/AC_PID/AC_PID.cpp
@@ -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;
 }
diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h
index 5b8d8c217a..c3a968dda9 100644
--- a/libraries/AC_PID/AC_PID.h
+++ b/libraries/AC_PID/AC_PID.h
@@ -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;
 };