From 9d8bf5004eb8c8da20d6dfc4dea2f8616ec75a77 Mon Sep 17 00:00:00 2001
From: esaldiran <emresaldiran@gmail.com>
Date: Mon, 18 Jul 2022 17:49:16 +0100
Subject: [PATCH] AC_PID: update pid_info with integrator reset for logging
 purposes

---
 libraries/AC_PID/AC_PID.cpp       | 1 +
 libraries/AC_PID/AC_PID_2D.cpp    | 7 +++++++
 libraries/AC_PID/AC_PID_2D.h      | 2 +-
 libraries/AC_PID/AC_PID_Basic.cpp | 7 +++++++
 libraries/AC_PID/AC_PID_Basic.h   | 2 +-
 5 files changed, 17 insertions(+), 2 deletions(-)

diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp
index f3f38d0957..e63025877d 100644
--- a/libraries/AC_PID/AC_PID.cpp
+++ b/libraries/AC_PID/AC_PID.cpp
@@ -272,6 +272,7 @@ float AC_PID::get_ff()
 void AC_PID::reset_I()
 {
     _integrator = 0.0;
+    _pid_info.I = 0.0;
 }
 
 void AC_PID::load_gains()
diff --git a/libraries/AC_PID/AC_PID_2D.cpp b/libraries/AC_PID/AC_PID_2D.cpp
index 4e5a91d91c..559faa34f7 100644
--- a/libraries/AC_PID/AC_PID_2D.cpp
+++ b/libraries/AC_PID/AC_PID_2D.cpp
@@ -166,6 +166,13 @@ Vector2f AC_PID_2D::get_ff()
     return _target * _kff;
 }
 
+void AC_PID_2D::reset_I()
+{
+    _integrator.zero(); 
+    _pid_info_x.I = 0.0;
+    _pid_info_y.I = 0.0;
+}
+
 // save_gains - save gains to eeprom
 void AC_PID_2D::save_gains()
 {
diff --git a/libraries/AC_PID/AC_PID_2D.h b/libraries/AC_PID/AC_PID_2D.h
index 6e18340048..031f340e93 100644
--- a/libraries/AC_PID/AC_PID_2D.h
+++ b/libraries/AC_PID/AC_PID_2D.h
@@ -41,7 +41,7 @@ public:
     const Vector2f& get_error() const { return _error; }
 
     // reset the integrator
-    void reset_I() { _integrator.zero(); };
+    void reset_I();
 
     // reset_filter - input and D term filter will be reset to the next value provided to set_input()
     void reset_filter() { _reset_filter = true; }
diff --git a/libraries/AC_PID/AC_PID_Basic.cpp b/libraries/AC_PID/AC_PID_Basic.cpp
index 8fade0595c..b285a31541 100644
--- a/libraries/AC_PID/AC_PID_Basic.cpp
+++ b/libraries/AC_PID/AC_PID_Basic.cpp
@@ -139,6 +139,12 @@ void AC_PID_Basic::update_i(bool limit_neg, bool limit_pos)
     }
 }
 
+void AC_PID_Basic::reset_I()
+{
+    _integrator = 0.0; 
+    _pid_info.I = 0.0;
+}
+
 // save_gains - save gains to eeprom
 void AC_PID_Basic::save_gains()
 {
@@ -176,4 +182,5 @@ void AC_PID_Basic::set_integrator(float error, float i)
 void AC_PID_Basic::set_integrator(float i)
 {
     _integrator = constrain_float(i, -_kimax, _kimax);
+    _pid_info.I = _integrator;
 }
diff --git a/libraries/AC_PID/AC_PID_Basic.h b/libraries/AC_PID/AC_PID_Basic.h
index 8ab1f6f100..02f564e7bd 100644
--- a/libraries/AC_PID/AC_PID_Basic.h
+++ b/libraries/AC_PID/AC_PID_Basic.h
@@ -37,7 +37,7 @@ public:
     float get_error() const WARN_IF_UNUSED { return _error; }
 
     // reset the integrator
-    void reset_I() { _integrator = 0.0f; }
+    void reset_I();
 
     // input and D term filter will be reset to the next value provided to set_input()
     void reset_filter() { _reset_filter = true; }