From 31580d82eea900ca627a9f1d8d9b0bca666d133f Mon Sep 17 00:00:00 2001
From: bnsgeyer <bnsgeyer@aol.com>
Date: Tue, 1 Dec 2020 10:02:54 -0500
Subject: [PATCH] AC_PID: add support to smoothly reset the integrator

---
 libraries/AC_PID/AC_PID.cpp | 17 +++++++++++++++++
 libraries/AC_PID/AC_PID.h   |  6 ++++++
 2 files changed, 23 insertions(+)

diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp
index 8ea4a5fb09..088dc78974 100644
--- a/libraries/AC_PID/AC_PID.cpp
+++ b/libraries/AC_PID/AC_PID.cpp
@@ -249,6 +249,23 @@ 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;
+}
+
 void AC_PID::load_gains()
 {
     _kp.load();
diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h
index 631eb1c28f..2bf1dea4d5 100644
--- a/libraries/AC_PID/AC_PID.h
+++ b/libraries/AC_PID/AC_PID.h
@@ -12,6 +12,7 @@
 #define AC_PID_TFILT_HZ_DEFAULT  0.0f   // default input filter frequency
 #define AC_PID_EFILT_HZ_DEFAULT  0.0f   // default input filter frequency
 #define AC_PID_DFILT_HZ_DEFAULT  20.0f   // default input filter frequency
+#define AC_PID_RESET_TC          0.16f   // Time constant for integrator reset decay to zero
 
 /// @class	AC_PID
 /// @brief	Copter PID control class
@@ -56,6 +57,9 @@ 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;
@@ -131,6 +135,8 @@ protected:
     float _target;            // target value to enable filtering
     float _error;             // error value to enable filtering
     float _derivative;        // derivative value to enable filtering
+    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;
 };