Merge pull request #1 from ubi-agni/antiwindup-fix-neg-gain

Fixed i_error saturation when i_gain is negative
ros-controls · Jun 22, 2015 · 5633b61 · 5633b61
2 parents 8b63dee + 903b53e
commit 5633b61
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/src/pid.cpp b/src/pid.cpp
@@ -363,7 +363,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