From 90c5c221fddf7248db732967ff54c7680f33583b Mon Sep 17 00:00:00 2001
From: Alex White <alexander.white@technipfmc.com>
Date: Wed, 19 Apr 2023 16:15:10 -0700
Subject: [PATCH 1/5] Added adaptive PID functionality to PID controller

Added save_iterm_ to PID gains
Added save_iterm() and clear_saved_iterm() methods to update and clear save integral term
Cleared saved integral term in constructor
Called save_iterm() in reset() method
Added saved integral term to PID output
---
 include/control_toolbox/pid.hpp     | 44 ++++++++++++++++++---
 include/control_toolbox/pid_ros.hpp | 13 +++++-
 src/pid.cpp                         | 61 ++++++++++++++++++++++++-----
 src/pid_ros.cpp                     | 26 ++++++++----
 test/pid_parameters_tests.cpp       | 25 +++++++++---
 test/pid_publisher_tests.cpp        |  4 +-
 test/pid_tests.cpp                  | 18 ++++++---
 7 files changed, 153 insertions(+), 38 deletions(-)

diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp
index 13f8553a..3ccb7a69 100644
--- a/include/control_toolbox/pid.hpp
+++ b/include/control_toolbox/pid.hpp
@@ -113,9 +113,10 @@ class CONTROL_TOOLBOX_PUBLIC Pid
    */
   struct Gains
   {
-    // Optional constructor for passing in values without antiwindup
+    // Optional constructor for passing in values without antiwindup/save i-term
     Gains(double p, double i, double d, double i_max, double i_min)
-    : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(false)
+    : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(false),
+      save_iterm_(false)
     {
     }
     // Optional constructor for passing in values
@@ -123,8 +124,15 @@ class CONTROL_TOOLBOX_PUBLIC Pid
     : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(antiwindup)
     {
     }
+    // Optional constructor for passing in values
+    Gains(double p, double i, double d, double i_max, double i_min, bool antiwindup,
+      bool save_iterm) : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min),
+      antiwindup_(antiwindup), save_iterm_(save_iterm)
+    {
+    }
     // Default constructor
-    Gains() : p_gain_(0.0), i_gain_(0.0), d_gain_(0.0), i_max_(0.0), i_min_(0.0), antiwindup_(false)
+    Gains() : p_gain_(0.0), i_gain_(0.0), d_gain_(0.0), i_max_(0.0), i_min_(0.0),
+      antiwindup_(false), save_iterm_(false)
     {
     }
     double p_gain_;   /**< Proportional gain. */
@@ -133,6 +141,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid
     double i_max_;    /**< Maximum allowable integral term. */
     double i_min_;    /**< Minimum allowable integral term. */
     bool antiwindup_; /**< Antiwindup. */
+    bool save_iterm_; /**< Save integral term. */
   };
 
   /*!
@@ -151,7 +160,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid
    */
   Pid(
     double p = 0.0, double i = 0.0, double d = 0.0, double i_max = 0.0, double i_min = -0.0,
-    bool antiwindup = false);
+    bool antiwindup = false, bool save_iterm = false);
 
   /**
    * \brief Copy constructor required for preventing mutexes from being copied
@@ -176,13 +185,24 @@ class CONTROL_TOOLBOX_PUBLIC Pid
    *
    * \note New gains are not applied if i_min_ > i_max_
    */
-  void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);
+  void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup = false,
+    bool save_iterm = false);
 
   /*!
    * \brief Reset the state of this PID controller
    */
   void reset();
 
+  /*!
+   * \brief Save the integrator output of this controller
+   */
+  void save_iterm();
+
+  /*!
+   * \brief Clear the saved the integrator output of this controller
+   */
+  void clear_saved_iterm();
+
   /*!
    * \brief Get PID gains for the controller.
    * \param p The proportional gain.
@@ -194,6 +214,9 @@ class CONTROL_TOOLBOX_PUBLIC Pid
   void getGains(double & p, double & i, double & d, double & i_max, double & i_min);
   void getGains(
     double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup);
+  void getGains(
+    double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup,
+    bool & save_iterm);
 
   /*!
    * \brief Get PID gains for the controller.
@@ -211,7 +234,8 @@ class CONTROL_TOOLBOX_PUBLIC Pid
    *
    * \note New gains are not applied if i_min > i_max
    */
-  void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);
+  void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false,
+    bool save_iterm = false);
 
   /*!
    * \brief Set PID gains for the controller.
@@ -288,14 +312,22 @@ class CONTROL_TOOLBOX_PUBLIC Pid
     return *this;
   }
 
+  /*!
+   * \brief Return saved integral term
+   */
+  double getSavedITerm() {return i_term_saved_; }
+
 protected:
   // Store the PID gains in a realtime buffer to allow dynamic reconfigure to update it without
   // blocking the realtime update loop
   realtime_tools::RealtimeBuffer<Gains> gains_buffer_;
+  double min_i_term_ = 0.01;  // Minimum value for saving integral term
 
   double p_error_last_; /**< _Save position state for derivative state calculation. */
   double p_error_;      /**< Position error. */
   double i_error_;      /**< Integral of position error. */
+  double i_term_saved_; /**< Retained integral output */
+  double i_term_last_;  /**< Last integrator term output */
   double d_error_;      /**< Derivative of position error. */
   double cmd_;          /**< Command to send. */
   double error_dot_;    /**< Derivative error */
diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp
index cf55eb3c..2b9391d6 100644
--- a/include/control_toolbox/pid_ros.hpp
+++ b/include/control_toolbox/pid_ros.hpp
@@ -98,10 +98,12 @@ class CONTROL_TOOLBOX_PUBLIC PidROS
    * \param i_max The max integral windup.
    * \param i_min The min integral windup.
    * \param antiwindup antiwindup.
+   * \param save_iterm save integrator output between resets.
    *
    * \note New gains are not applied if i_min_ > i_max_
    */
-  void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup);
+  void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup,
+    bool save_iterm);
 
   /*!
    * \brief Initialize the PID controller based on already set parameters
@@ -114,6 +116,11 @@ class CONTROL_TOOLBOX_PUBLIC PidROS
    */
   void reset();
 
+  /*!
+   * \brief Save the integrator output of this controller
+   */
+  void save_iterm();
+
   /*!
    * \brief Set the PID error and compute the PID command with nonuniform time
    * step size. The derivative error is computed from the change in the error
@@ -153,10 +160,12 @@ class CONTROL_TOOLBOX_PUBLIC PidROS
    * \param i_max The max integral windup.
    * \param i_min The min integral windup.
    * \param antiwindup antiwindup.
+   * \param save_iterm save integrator output between resets.
    *
    * \note New gains are not applied if i_min > i_max
    */
-  void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);
+  void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false,
+    bool save_iterm = false);
 
   /*!
    * \brief Set PID gains for the controller.
diff --git a/src/pid.cpp b/src/pid.cpp
index 282aafb2..28e26e54 100644
--- a/src/pid.cpp
+++ b/src/pid.cpp
@@ -46,13 +46,15 @@
 
 namespace control_toolbox
 {
-Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup)
+Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup, bool save_iterm)
 : gains_buffer_()
 {
   if (i_min > i_max) {
     throw std::invalid_argument("received i_min > i_max");
   }
-  setGains(p, i, d, i_max, i_min, antiwindup);
+  // Initialize saved i-term values
+  clear_saved_iterm();
+  setGains(p, i, d, i_max, i_min, antiwindup, save_iterm);
 
   reset();
 }
@@ -62,21 +64,27 @@ Pid::Pid(const Pid & source)
   // Copy the realtime buffer to the new PID class
   gains_buffer_ = source.gains_buffer_;
 
+  // Initialize saved i-term values
+  clear_saved_iterm();
+
   // Reset the state of this PID controller
   reset();
 }
 
 Pid::~Pid() {}
 
-void Pid::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup)
+void Pid::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup,
+  bool save_iterm)
 {
-  setGains(p, i, d, i_max, i_min, antiwindup);
+  setGains(p, i, d, i_max, i_min, antiwindup, save_iterm);
 
   reset();
 }
 
 void Pid::reset()
 {
+  save_iterm();
+
   p_error_last_ = 0.0;
   p_error_ = 0.0;
   i_error_ = 0.0;
@@ -84,14 +92,39 @@ void Pid::reset()
   cmd_ = 0.0;
 }
 
+void Pid::save_iterm()
+{
+  // If last integral term is less than min_i_term_, just return - don't keep very small terms
+  if (std::fabs(i_term_last_) < min_i_term_) return;
+
+  // Get the gain parameters from the realtime buffer
+  Gains gains = *gains_buffer_.readFromRT();
+
+  if (gains.save_iterm_) {
+    i_term_saved_ += i_term_last_;
+    // Limit saved integrator output to max integrator output
+    i_term_saved_ = std::clamp(i_term_saved_, gains.i_min_, gains.i_max_);
+  } else {
+    i_term_saved_ = 0.0;
+  }
+  i_term_last_ = 0.0;
+}
+
+void Pid::clear_saved_iterm()
+{
+  i_term_saved_ = 0.0;
+  i_term_last_ = 0.0;
+}
+
 void Pid::getGains(double & p, double & i, double & d, double & i_max, double & i_min)
 {
-  bool antiwindup;
-  getGains(p, i, d, i_max, i_min, antiwindup);
+  bool antiwindup, save_iterm;
+  getGains(p, i, d, i_max, i_min, antiwindup, save_iterm);
 }
 
 void Pid::getGains(
-  double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup)
+  double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup,
+  bool &save_iterm)
 {
   Gains gains = *gains_buffer_.readFromRT();
 
@@ -101,13 +134,15 @@ void Pid::getGains(
   i_max = gains.i_max_;
   i_min = gains.i_min_;
   antiwindup = gains.antiwindup_;
+  save_iterm = gains.save_iterm_;
 }
 
 Pid::Gains Pid::getGains() { return *gains_buffer_.readFromRT(); }
 
-void Pid::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup)
+void Pid::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup,
+  bool save_iterm)
 {
-  Gains gains(p, i, d, i_max, i_min, antiwindup);
+  Gains gains(p, i, d, i_max, i_min, antiwindup, save_iterm);
 
   setGains(gains);
 }
@@ -171,12 +206,18 @@ double Pid::computeCommand(double error, double error_dot, uint64_t dt)
     // Limit i_term so that the limit is meaningful in the output
     i_term = std::clamp(i_term, gains.i_min_, gains.i_max_);
   }
+  i_term_last_ = i_term;
+
+  if (!gains.save_iterm_) {
+    // reset saved integrator term if disabled
+    i_term_saved_ = 0.0;
+  }
 
   // Calculate derivative contribution to command
   d_term = gains.d_gain_ * d_error_;
 
   // Compute the command
-  cmd_ = p_term + i_term + d_term;
+  cmd_ = p_term + i_term + i_term_saved_ + d_term;
 
   return cmd_;
 }
diff --git a/src/pid_ros.cpp b/src/pid_ros.cpp
index 58d0f609..1b84c96d 100644
--- a/src/pid_ros.cpp
+++ b/src/pid_ros.cpp
@@ -169,6 +169,7 @@ bool PidROS::initPid()
   double p, i, d, i_min, i_max;
   p = i = d = i_min = i_max = std::numeric_limits<double>::quiet_NaN();
   bool antiwindup = false;
+  bool save_iterm = false;
   bool all_params_available = true;
   all_params_available &= getDoubleParam(param_prefix_ + "p", p);
   all_params_available &= getDoubleParam(param_prefix_ + "i", i);
@@ -177,12 +178,13 @@ bool PidROS::initPid()
   all_params_available &= getDoubleParam(param_prefix_ + "i_clamp_min", i_min);
 
   getBooleanParam(param_prefix_ + "antiwindup", antiwindup);
+  getBooleanParam(param_prefix_ + "save_iterm", save_iterm);
 
   if (all_params_available) {
     setParameterEventCallback();
   }
 
-  pid_.initPid(p, i, d, i_max, i_min, antiwindup);
+  pid_.initPid(p, i, d, i_max, i_min, antiwindup, save_iterm);
 
   return all_params_available;
 }
@@ -194,12 +196,13 @@ void PidROS::declareParam(const std::string & param_name, rclcpp::ParameterValue
   }
 }
 
-void PidROS::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup)
+void PidROS::initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup,
+  bool save_iterm)
 {
   if (i_min > i_max) {
     RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains");
   } else {
-    pid_.initPid(p, i, d, i_max, i_min, antiwindup);
+    pid_.initPid(p, i, d, i_max, i_min, antiwindup, save_iterm);
 
     declareParam(param_prefix_ + "p", rclcpp::ParameterValue(p));
     declareParam(param_prefix_ + "i", rclcpp::ParameterValue(i));
@@ -207,6 +210,7 @@ void PidROS::initPid(double p, double i, double d, double i_max, double i_min, b
     declareParam(param_prefix_ + "i_clamp_max", rclcpp::ParameterValue(i_max));
     declareParam(param_prefix_ + "i_clamp_min", rclcpp::ParameterValue(i_min));
     declareParam(param_prefix_ + "antiwindup", rclcpp::ParameterValue(antiwindup));
+    declareParam(param_prefix_ + "save_iterm", rclcpp::ParameterValue(save_iterm));
 
     setParameterEventCallback();
   }
@@ -214,6 +218,8 @@ void PidROS::initPid(double p, double i, double d, double i_max, double i_min, b
 
 void PidROS::reset() { pid_.reset(); }
 
+void PidROS::save_iterm() { pid_.save_iterm(); }
+
 std::shared_ptr<rclcpp::Publisher<control_msgs::msg::PidState>> PidROS::getPidStatePublisher()
 {
   return state_pub_;
@@ -237,7 +243,8 @@ double PidROS::computeCommand(double error, double error_dot, rclcpp::Duration d
 
 Pid::Gains PidROS::getGains() { return pid_.getGains(); }
 
-void PidROS::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup)
+void PidROS::setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup,
+  bool save_iterm)
 {
   if (i_min > i_max) {
     RCLCPP_ERROR(node_logging_->get_logger(), "received i_min > i_max, skip new gains");
@@ -247,11 +254,11 @@ void PidROS::setGains(double p, double i, double d, double i_max, double i_min,
       rclcpp::Parameter(param_prefix_ + "d", d),
       rclcpp::Parameter(param_prefix_ + "i_clamp_max", i_max),
       rclcpp::Parameter(param_prefix_ + "i_clamp_min", i_min),
-      rclcpp::Parameter(param_prefix_ + "antiwindup", antiwindup)});
+      rclcpp::Parameter(param_prefix_ + "antiwindup", antiwindup),
+     rclcpp::Parameter(param_prefix_ + "save_iterm", save_iterm)});
 
-    pid_.setGains(p, i, d, i_max, i_min, antiwindup);
+      pid_.setGains(p, i, d, i_max, i_min, antiwindup, save_iterm);
   }
-}
 
 void PidROS::publishPIDState(double cmd, double error, rclcpp::Duration dt)
 {
@@ -309,6 +316,8 @@ void PidROS::printValues()
                                                     << "  I_Min:        " << gains.i_min_ << "\n"
                                                     << "  Antiwindup:   " << gains.antiwindup_
                                                     << "\n"
+                                                    << "  Save I-Term:  " << gains.save_iterm_
+                                                    << "\n"
                                                     << "  P_Error:      " << p_error_ << "\n"
                                                     << "  I_Error:      " << i_error_ << "\n"
                                                     << "  D_Error:      " << d_error_ << "\n"
@@ -355,6 +364,9 @@ void PidROS::setParameterEventCallback()
         } else if (param_name == param_prefix_ + "antiwindup") {
           gains.antiwindup_ = parameter.get_value<bool>();
           changed = true;
+        } else if (param_name == param_prefix_ + "save_iterm") {
+          gains.save_iterm_ = parameter.get_value<bool>();
+          changed = true;
         }
       } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
         RCLCPP_INFO_STREAM(node_logging_->get_logger(), "Please use the right type: " << e.what());
diff --git a/test/pid_parameters_tests.cpp b/test/pid_parameters_tests.cpp
index 9a26d659..7a9d4a81 100644
--- a/test/pid_parameters_tests.cpp
+++ b/test/pid_parameters_tests.cpp
@@ -57,8 +57,9 @@ 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;
 
-  ASSERT_NO_THROW(pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP));
+  ASSERT_NO_THROW(pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_ITERM));
 
   rclcpp::Parameter param;
 
@@ -81,6 +82,9 @@ void check_set_parameters(
   ASSERT_TRUE(node->get_parameter(prefix + "antiwindup", param));
   ASSERT_EQ(param.get_value<bool>(), ANTIWINDUP);
 
+  ASSERT_TRUE(node->get_parameter(prefix + "save_iterm", param));
+  ASSERT_EQ(param.get_value<bool>(), SAVE_ITERM);
+
   // check gains were set
   control_toolbox::Pid::Gains gains = pid.getGains();
   ASSERT_EQ(gains.p_gain_, P);
@@ -89,6 +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_);
 }
 
 TEST(PidParametersTest, InitPidTest)
@@ -215,8 +220,9 @@ TEST(PidParametersTest, SetParametersTest)
   const double I_MAX = 10.0;
   const double I_MIN = -10.0;
   const bool ANTIWINDUP = true;
+  const bool SAVE_ITERM = false;
 
-  pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP);
+  pid.initPid(P, I, D, I_MAX, I_MIN, ANTIWINDUP, SAVE_ITERM);
 
   rcl_interfaces::msg::SetParametersResult set_result;
 
@@ -237,6 +243,8 @@ TEST(PidParametersTest, SetParametersTest)
   ASSERT_TRUE(set_result.successful);
   ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("antiwindup", ANTIWINDUP)));
   ASSERT_TRUE(set_result.successful);
+  ASSERT_NO_THROW(set_result = node->set_parameter(rclcpp::Parameter("save_iterm", SAVE_ITERM)));
+  ASSERT_TRUE(set_result.successful);
 
   // process callbacks
   rclcpp::spin_some(node->get_node_base_interface());
@@ -249,6 +257,7 @@ TEST(PidParametersTest, SetParametersTest)
   ASSERT_EQ(gains.i_max_, I_MAX);
   ASSERT_EQ(gains.i_min_, I_MIN);
   ASSERT_EQ(gains.antiwindup_, ANTIWINDUP);
+  ASSERT_EQ(gains.save_iterm_, SAVE_ITERM);
 }
 
 TEST(PidParametersTest, SetBadParametersTest)
@@ -313,9 +322,10 @@ TEST(PidParametersTest, GetParametersTest)
   const double I_MAX = 10.0;
   const double I_MIN = -10.0;
   const bool ANTIWINDUP = true;
+  const bool SAVE_ITERM = false;
 
-  pid.initPid(0.0, 0.0, 0.0, 0.0, 0.0, false);
-  pid.setGains(P, I, D, I_MAX, I_MIN, ANTIWINDUP);
+  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);
 
   rclcpp::Parameter param;
 
@@ -336,6 +346,9 @@ TEST(PidParametersTest, GetParametersTest)
 
   ASSERT_TRUE(node->get_parameter("antiwindup", param));
   ASSERT_EQ(param.get_value<bool>(), ANTIWINDUP);
+
+  ASSERT_TRUE(node->get_parameter("save_iterm", param));
+  ASSERT_EQ(param.get_value<bool>(), SAVE_ITERM);
 }
 
 TEST(PidParametersTest, GetParametersFromParams)
@@ -380,8 +393,8 @@ TEST(PidParametersTest, MultiplePidInstances)
   const double I_MAX = 10.0;
   const double I_MIN = -10.0;
 
-  ASSERT_NO_THROW(pid_1.initPid(P, I, D, I_MAX, I_MIN, false));
-  ASSERT_NO_THROW(pid_2.initPid(P, I, D, I_MAX, I_MIN, true));
+  ASSERT_NO_THROW(pid_1.initPid(P, I, D, I_MAX, I_MIN, false, false));
+  ASSERT_NO_THROW(pid_2.initPid(P, I, D, I_MAX, I_MIN, true, false));
 
   rclcpp::Parameter param_1, param_2;
   ASSERT_TRUE(node->get_parameter("PID_1.p", param_1));
diff --git a/test/pid_publisher_tests.cpp b/test/pid_publisher_tests.cpp
index 1c3de5f1..48abc05b 100644
--- a/test/pid_publisher_tests.cpp
+++ b/test/pid_publisher_tests.cpp
@@ -39,7 +39,7 @@ TEST(PidPublisherTest, PublishTest)
 
   control_toolbox::PidROS pid_ros = control_toolbox::PidROS(node);
 
-  pid_ros.initPid(1.0, 1.0, 1.0, 5.0, -5.0, false);
+  pid_ros.initPid(1.0, 1.0, 1.0, 5.0, -5.0, false, false);
 
   bool callback_called = false;
   control_msgs::msg::PidState::SharedPtr last_state_msg;
@@ -77,7 +77,7 @@ TEST(PidPublisherTest, PublishTestLifecycle)
       pid_ros.getPidStatePublisher());
   // state_pub_lifecycle_->on_activate();
 
-  pid_ros.initPid(1.0, 1.0, 1.0, 5.0, -5.0, false);
+  pid_ros.initPid(1.0, 1.0, 1.0, 5.0, -5.0, false, false);
 
   bool callback_called = false;
   control_msgs::msg::PidState::SharedPtr last_state_msg;
diff --git a/test/pid_tests.cpp b/test/pid_tests.cpp
index b8ef5635..50e3cf55 100644
--- a/test/pid_tests.cpp
+++ b/test/pid_tests.cpp
@@ -182,15 +182,17 @@ TEST(ParameterTest, gainSettingCopyPIDTest)
   double i_max = std::rand() % 100;
   double i_min = -1 * std::rand() % 100;
   bool antiwindup = false;
+  bool save_iterm = false;
 
   // Initialize the default way
   Pid pid1(p_gain, i_gain, d_gain, i_max, i_min, antiwindup);
 
   // Test return values  -------------------------------------------------
   double p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return;
-  bool antiwindup_return;
+  bool antiwindup_return, save_iterm_return;
   pid1.getGains(
-    p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return);
+    p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return,
+    save_iterm_return);
 
   EXPECT_EQ(p_gain, p_gain_return);
   EXPECT_EQ(i_gain, i_gain_return);
@@ -198,6 +200,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest)
   EXPECT_EQ(i_max, i_max_return);
   EXPECT_EQ(i_min, i_min_return);
   EXPECT_EQ(antiwindup, antiwindup_return);
+  EXPECT_EQ(save_iterm, save_iterm_return);
 
   // Test return values using struct -------------------------------------------------
 
@@ -207,7 +210,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest)
   d_gain = std::rand() % 100;
   i_max = std::rand() % 100;
   i_min = -1 * std::rand() % 100;
-  pid1.setGains(p_gain, i_gain, d_gain, i_max, i_min, antiwindup);
+  pid1.setGains(p_gain, i_gain, d_gain, i_max, i_min, antiwindup, save_iterm);
 
   Pid::Gains g1 = pid1.getGains();
   EXPECT_EQ(p_gain, g1.p_gain_);
@@ -216,6 +219,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest)
   EXPECT_EQ(i_max, g1.i_max_);
   EXPECT_EQ(i_min, g1.i_min_);
   EXPECT_EQ(antiwindup, g1.antiwindup_);
+  EXPECT_EQ(save_iterm, g1.save_iterm_);
 
   // \todo test initParam() -------------------------------------------------
 
@@ -229,7 +233,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest)
   Pid pid2(pid1);
 
   pid2.getGains(
-    p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return);
+    p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return,
+    save_iterm_return);
 
   EXPECT_EQ(p_gain, p_gain_return);
   EXPECT_EQ(i_gain, i_gain_return);
@@ -237,6 +242,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest)
   EXPECT_EQ(i_max, i_max_return);
   EXPECT_EQ(i_min, i_min_return);
   EXPECT_EQ(antiwindup, antiwindup_return);
+  EXPECT_EQ(save_iterm, save_iterm_return);
 
   // Test that errors are zero
   double pe2, ie2, de2;
@@ -250,7 +256,8 @@ TEST(ParameterTest, gainSettingCopyPIDTest)
   pid3 = pid1;
 
   pid3.getGains(
-    p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return);
+    p_gain_return, i_gain_return, d_gain_return, i_max_return, i_min_return, antiwindup_return,
+    save_iterm_return);
 
   EXPECT_EQ(p_gain, p_gain_return);
   EXPECT_EQ(i_gain, i_gain_return);
@@ -258,6 +265,7 @@ TEST(ParameterTest, gainSettingCopyPIDTest)
   EXPECT_EQ(i_max, i_max_return);
   EXPECT_EQ(i_min, i_min_return);
   EXPECT_EQ(antiwindup, antiwindup_return);
+  EXPECT_EQ(save_iterm, save_iterm_return);
 
   // Test that errors are zero
   double pe3, ie3, de3;

From 3aaab3806c225a3e1281c6af7f3a764b51eca53b Mon Sep 17 00:00:00 2001
From: Alex White <alexander.white@technipfmc.com>
Date: Thu, 20 Apr 2023 08:02:01 -0700
Subject: [PATCH 2/5] Updates from PR comments

Simplified saving of integral term
---
 include/control_toolbox/pid.hpp     | 18 +++------------
 include/control_toolbox/pid_ros.hpp |  7 +-----
 src/pid.cpp                         | 35 +++++++----------------------
 src/pid_ros.cpp                     |  2 --
 4 files changed, 12 insertions(+), 50 deletions(-)

diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp
index 3ccb7a69..5226a957 100644
--- a/include/control_toolbox/pid.hpp
+++ b/include/control_toolbox/pid.hpp
@@ -121,7 +121,8 @@ class CONTROL_TOOLBOX_PUBLIC Pid
     }
     // Optional constructor for passing in values
     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)
+    : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(antiwindup),
+      save_iterm_(false)
     {
     }
     // Optional constructor for passing in values
@@ -194,12 +195,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid
   void reset();
 
   /*!
-   * \brief Save the integrator output of this controller
-   */
-  void save_iterm();
-
-  /*!
-   * \brief Clear the saved the integrator output of this controller
+   * \brief Clear the saved integrator output of this controller
    */
   void clear_saved_iterm();
 
@@ -312,22 +308,14 @@ class CONTROL_TOOLBOX_PUBLIC Pid
     return *this;
   }
 
-  /*!
-   * \brief Return saved integral term
-   */
-  double getSavedITerm() {return i_term_saved_; }
-
 protected:
   // Store the PID gains in a realtime buffer to allow dynamic reconfigure to update it without
   // blocking the realtime update loop
   realtime_tools::RealtimeBuffer<Gains> gains_buffer_;
-  double min_i_term_ = 0.01;  // Minimum value for saving integral term
 
   double p_error_last_; /**< _Save position state for derivative state calculation. */
   double p_error_;      /**< Position error. */
   double i_error_;      /**< Integral of position error. */
-  double i_term_saved_; /**< Retained integral output */
-  double i_term_last_;  /**< Last integrator term output */
   double d_error_;      /**< Derivative of position error. */
   double cmd_;          /**< Command to send. */
   double error_dot_;    /**< Derivative error */
diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp
index 2b9391d6..1333dce5 100644
--- a/include/control_toolbox/pid_ros.hpp
+++ b/include/control_toolbox/pid_ros.hpp
@@ -103,7 +103,7 @@ class CONTROL_TOOLBOX_PUBLIC PidROS
    * \note New gains are not applied if i_min_ > i_max_
    */
   void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup,
-    bool save_iterm);
+    bool save_iterm = false);
 
   /*!
    * \brief Initialize the PID controller based on already set parameters
@@ -116,11 +116,6 @@ class CONTROL_TOOLBOX_PUBLIC PidROS
    */
   void reset();
 
-  /*!
-   * \brief Save the integrator output of this controller
-   */
-  void save_iterm();
-
   /*!
    * \brief Set the PID error and compute the PID command with nonuniform time
    * step size. The derivative error is computed from the change in the error
diff --git a/src/pid.cpp b/src/pid.cpp
index 28e26e54..b654a77b 100644
--- a/src/pid.cpp
+++ b/src/pid.cpp
@@ -52,9 +52,10 @@ Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwind
   if (i_min > i_max) {
     throw std::invalid_argument("received i_min > i_max");
   }
+  setGains(p, i, d, i_max, i_min, antiwindup, save_iterm);
+
   // Initialize saved i-term values
   clear_saved_iterm();
-  setGains(p, i, d, i_max, i_min, antiwindup, save_iterm);
 
   reset();
 }
@@ -83,37 +84,23 @@ void Pid::initPid(double p, double i, double d, double i_max, double i_min, bool
 
 void Pid::reset()
 {
-  save_iterm();
-
   p_error_last_ = 0.0;
   p_error_ = 0.0;
-  i_error_ = 0.0;
   d_error_ = 0.0;
   cmd_ = 0.0;
-}
 
-void Pid::save_iterm()
-{
-  // If last integral term is less than min_i_term_, just return - don't keep very small terms
-  if (std::fabs(i_term_last_) < min_i_term_) return;
+  // If last integral error is already zero, just return
+  if (std::fabs(i_error_) < std::numeric_limits<double>::epsilon()) return;
 
   // Get the gain parameters from the realtime buffer
   Gains gains = *gains_buffer_.readFromRT();
-
-  if (gains.save_iterm_) {
-    i_term_saved_ += i_term_last_;
-    // Limit saved integrator output to max integrator output
-    i_term_saved_ = std::clamp(i_term_saved_, gains.i_min_, gains.i_max_);
-  } else {
-    i_term_saved_ = 0.0;
-  }
-  i_term_last_ = 0.0;
+  // Check to see if we should reset integral error here
+  if (!gains.save_iterm_) i_error_ = 0.0;
 }
 
 void Pid::clear_saved_iterm()
 {
-  i_term_saved_ = 0.0;
-  i_term_last_ = 0.0;
+  i_error_ = 0.0;
 }
 
 void Pid::getGains(double & p, double & i, double & d, double & i_max, double & i_min)
@@ -206,18 +193,12 @@ double Pid::computeCommand(double error, double error_dot, uint64_t dt)
     // Limit i_term so that the limit is meaningful in the output
     i_term = std::clamp(i_term, gains.i_min_, gains.i_max_);
   }
-  i_term_last_ = i_term;
-
-  if (!gains.save_iterm_) {
-    // reset saved integrator term if disabled
-    i_term_saved_ = 0.0;
-  }
 
   // Calculate derivative contribution to command
   d_term = gains.d_gain_ * d_error_;
 
   // Compute the command
-  cmd_ = p_term + i_term + i_term_saved_ + d_term;
+  cmd_ = p_term + i_term + d_term;
 
   return cmd_;
 }
diff --git a/src/pid_ros.cpp b/src/pid_ros.cpp
index 1b84c96d..23572150 100644
--- a/src/pid_ros.cpp
+++ b/src/pid_ros.cpp
@@ -218,8 +218,6 @@ void PidROS::initPid(double p, double i, double d, double i_max, double i_min, b
 
 void PidROS::reset() { pid_.reset(); }
 
-void PidROS::save_iterm() { pid_.save_iterm(); }
-
 std::shared_ptr<rclcpp::Publisher<control_msgs::msg::PidState>> PidROS::getPidStatePublisher()
 {
   return state_pub_;

From 2fa429a45875eaa4dca7de8a4e1e1e95e544937b Mon Sep 17 00:00:00 2001
From: Whalex451 <101365645+Whalex451@users.noreply.github.com>
Date: Thu, 20 Apr 2023 11:19:12 -0700
Subject: [PATCH 3/5] Apply suggestions from code review

Co-authored-by: Dr. Denis <denis@stoglrobotics.de>
---
 include/control_toolbox/pid.hpp | 2 +-
 src/pid.cpp                     | 7 +++++--
 2 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/include/control_toolbox/pid.hpp b/include/control_toolbox/pid.hpp
index 5226a957..b858d278 100644
--- a/include/control_toolbox/pid.hpp
+++ b/include/control_toolbox/pid.hpp
@@ -113,7 +113,7 @@ class CONTROL_TOOLBOX_PUBLIC Pid
    */
   struct Gains
   {
-    // Optional constructor for passing in values without antiwindup/save i-term
+    // Optional constructor for passing in values without antiwindup and save i-term
     Gains(double p, double i, double d, double i_max, double i_min)
     : p_gain_(p), i_gain_(i), d_gain_(d), i_max_(i_max), i_min_(i_min), antiwindup_(false),
       save_iterm_(false)
diff --git a/src/pid.cpp b/src/pid.cpp
index b654a77b..63e61ad0 100644
--- a/src/pid.cpp
+++ b/src/pid.cpp
@@ -90,7 +90,10 @@ 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()) return;
+  if (std::fabs(i_error_) < std::numeric_limits<double>::epsilon()) 
+  {
+    return;
+  }
 
   // Get the gain parameters from the realtime buffer
   Gains gains = *gains_buffer_.readFromRT();
@@ -111,7 +114,7 @@ void Pid::getGains(double & p, double & i, double & d, double & i_max, double &
 
 void Pid::getGains(
   double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup,
-  bool &save_iterm)
+  bool & save_iterm)
 {
   Gains gains = *gains_buffer_.readFromRT();
 

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 4/5] 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);

From e67dd314bedef6e674cc1bd1427f7ad411ea3b1b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= <denis@stoglrobotics.de>
Date: Mon, 12 Feb 2024 11:22:50 +0100
Subject: [PATCH 5/5] Fixup for compilation.

---
 src/pid_ros.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/pid_ros.cpp b/src/pid_ros.cpp
index 23572150..1c69d374 100644
--- a/src/pid_ros.cpp
+++ b/src/pid_ros.cpp
@@ -257,6 +257,7 @@ void PidROS::setGains(double p, double i, double d, double i_max, double i_min,
 
       pid_.setGains(p, i, d, i_max, i_min, antiwindup, save_iterm);
   }
+}
 
 void PidROS::publishPIDState(double cmd, double error, rclcpp::Duration dt)
 {