[Control] Update pid_IC_controller and test

modules/control/common/pid_IC_controller.cc

@@ -17,6 +17,7 @@
 #include "modules/control/common/pid_IC_controller.h"
 
 #include <cmath>
+#include <iostream>
 
 #include "modules/common/log.h"
 
@@ -41,12 +42,20 @@ double PIDICController::Control(const double error, const double dt) {
     integral_ = 0;
   } else {
     double u = error * kp_ + integral_ + error * dt * ki_ + diff * kd_;
-    if (((error * u) > 0) &&
-        ((u > saturation_high_) || (u < saturation_low_))) {
-      // Do not update integral_
+    if (((error * u) > 0) && ((u > integrator_saturation_high_) ||
+                              (u < integrator_saturation_low_))) {
     } else {
+      // Only update integral then
       integral_ += error * dt * ki_;
     }
+
+    if (integral_ > integrator_saturation_high_) {
+      integrator_saturation_status_ = 1;
+    } else if (integral_ < integrator_saturation_low_) {
+      integrator_saturation_status_ = -1;
+    } else {
+      integrator_saturation_status_ = 0;
+    }
   }
 
   previous_error_ = error;

modules/control/common/pid_IC_controller.h

@@ -49,18 +49,6 @@ class PIDICController : public PIDController {
    */
   virtual double Control(const double error, const double dt);
 
-  /**
-   * @brief get saturation status
-   * @return saturation status
-   */
-  int SaturationStatus() const;
-
-  /**
-   * @brief get status that if integrator is hold
-   * @return if integrator is hold return true
-   */
-  bool integrator_hold() const;
-
  private:
 };
 

modules/control/common/pid_IC_controller_test.cc

@@ -69,12 +69,10 @@ TEST_F(PidICControllerTest, SpeedPidController) {
   pid_IC_controller.Reset();
   EXPECT_NEAR(pid_IC_controller.Control(-0.1, dt), -0.1505, 1e-6);
   pid_IC_controller.Reset();
-  EXPECT_NEAR(pid_IC_controller.Control(500.0, dt), 750.3, 1e-6);
-  pid_IC_controller.Reset();
-  EXPECT_EQ(pid_IC_controller.SaturationStatus(), 1);
-  pid_IC_controller.Reset();
-  EXPECT_NEAR(pid_IC_controller.Control(-500.0, dt), -750.3, 1e-6);
-  EXPECT_EQ(pid_IC_controller.SaturationStatus(), -1);
+  dt = 2;
+  EXPECT_NEAR(pid_IC_controller.Control(0.1, dt), 0.25, 1e-6);
+  EXPECT_EQ(pid_IC_controller.IntegratorSaturationStatus(), 0);
+  EXPECT_NEAR(pid_IC_controller.Control(0.1, dt), 0.25, 1e-6);
 }
 }  // namespace control
 }  // namespace apollo