From 0d196f28034d09f4f009bf3679e3eeda845c51ba Mon Sep 17 00:00:00 2001
From: Alex White <alexander.white@technipfmc.com>
Date: Thu, 20 Apr 2023 12:00:58 -0700
Subject: [PATCH] Applied more suggestions from code review

---
 include/control_toolbox/pid.hpp | 12 +++++++++++-
 src/pid.cpp                     |  2 +-
 test/pid_parameters_tests.cpp   |  6 +++---
 3 files changed, 15 insertions(+), 5 deletions(-)

diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp
index b858d278..cb0531b4 100644
--- a/include/control_toolbox/pid.hpp
+++ b/include/control_toolbox/pid.hpp
@@ -57,6 +57,14 @@ namespace control_toolbox
   be subclassed to provide more specific controls
   based on a particular control loop.
 
+  This class also allows for retention of integral
+  term on reset.  This is useful for control loops
+  that are enabled/disabled with a constant steady-state
+  external disturbance.  Once the integrator cancels
+  out the external disturbance, disabling/resetting/
+  re-enabling closed-loop control does not require
+  the integrator to wind up again.
+
   In particular, this class implements the standard
   pid equation:
 
@@ -83,6 +91,8 @@ namespace control_toolbox
 
   \param i_clamp Min/max bounds for the integral windup, the clamp is applied to the \f$i_{term}\f$
 
+  \param save_iterm boolean indicating if integral term is retained on reset()
+
   \section Usage
 
   To use the Pid class, you should first call some version of init()
@@ -119,7 +129,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid
       save_iterm_(false)
     {
     }
-    // Optional constructor for passing in values
+    // Optional constructor for passing in values without save i-term
     Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup)
     : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(antiwindup),
       save_iterm_(false)
diff --git a/src/pid.cpp b/src/pid.cpp
index 63e61ad0..cea3f442 100644
--- a/src/pid.cpp
+++ b/src/pid.cpp
@@ -90,7 +90,7 @@ void Pid::reset()
   cmd_ = 0.0;
 
   // If last integral error is already zero, just return
-  if (std::fabs(i_error_) < std::numeric_limits<double>::epsilon()) 
+  if (std::fabs(i_error_) < std::numeric_limits<double>::epsilon())
   {
     return;
   }
diff --git a/test/pid_parameters_tests.cpp b/test/pid_parameters_tests.cpp
index 7a9d4a81..9bb3eeef 100644
--- a/test/pid_parameters_tests.cpp
+++ b/test/pid_parameters_tests.cpp
@@ -57,7 +57,7 @@ void check_set_parameters(
   const double I_MAX = 10.0;
   const double I_MIN = -10.0;
   const bool ANTIWINDUP = true;
-  const bool SAVE_ITERM = false;
+  const bool SAVE_ITERM = true;
 
   ASSERT_NO_THROW(pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_ITERM));
 
@@ -93,7 +93,7 @@ void check_set_parameters(
   ASSERT_EQ(gains.i_max_, I_MAX);
   ASSERT_EQ(gains.i_min_, I_MIN);
   ASSERT_TRUE(gains.antiwindup_);
-  ASSERT_FALSE(gains.save_iterm_);
+  ASSERT_TRUE(gains.save_iterm_);
 }
 
 TEST(PidParametersTest, InitPidTest)
@@ -322,7 +322,7 @@ TEST(PidParametersTest, GetParametersTest)
   const double I_MAX = 10.0;
   const double I_MIN = -10.0;
   const bool ANTIWINDUP = true;
-  const bool SAVE_ITERM = false;
+  const bool SAVE_ITERM = true;
 
   pid.initPid(0.0, 0.0, 0.0, 0.0, 0.0, false, false);
   pid.setGains(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_ITERM);