Fixed i_error saturation when i_gain is negative

ros-controls · Aug 12, 2015 · 310bd6f · 310bd6f
1 parent e4cd9f8
commit 310bd6f
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/src/pid.cpp b/src/pid.cpp
@@ -310,7 +310,7 @@ double Pid::computeCommand(double error, double error_dot, ros::Duration dt)
   if(antiwindup_)
   {
     // Prevent i_error_ from climbing higher than permitted by i_max_/i_min_
-    i_error_ = std::max(gains.i_min_ / gains.i_gain_, std::min(i_error_, gains.i_max_ / gains.i_gain_));
+    i_error_ = std::max(gains.i_min_ / std::fabs(gains.i_gain_), std::min(i_error_, gains.i_max_ / std::fabs(gains.i_gain_)));
   }
 
   // Calculate integral contribution to command