diff --git a/src/common/pid.cpp b/src/common/pid.cpp
index a910556f..79717be7 100644
--- a/src/common/pid.cpp
+++ b/src/common/pid.cpp
@@ -27,19 +27,15 @@ float PIDController::operator() (float error){
     // u_p  = P *e(k)
     float proportional = P * error;
     // Tustin transform of the integral part
-    // u_ik = u_ik_1  + I*Ts/2*(ek + ek_1)
-    float integral = integral_prev + I*Ts*0.5*(error + error_prev);
-    // antiwindup - limit the output
-    integral = _constrain(integral, -limit, limit);
+    // u_ik = u_ik_1  + I*Ts*e(k)
+    float integral = integral_prev + I*Ts*error;
     // Discrete derivation
     // u_dk = D(ek - ek_1)/Ts
     float derivative = D*(error - error_prev)/Ts;
 
     // sum all the components
     float output = proportional + integral + derivative;
-    // antiwindup - limit the output variable
-    output = _constrain(output, -limit, limit);
-
+    
     // limit the acceleration by ramping the output
     float output_rate = (output - output_prev)/Ts;
     if (output_rate > output_ramp)
@@ -47,6 +43,15 @@ float PIDController::operator() (float error){
     else if (output_rate < -output_ramp)
         output = output_prev - output_ramp*Ts;
         
+    // limit abs value of output and antiwindup for integrator
+    if (output > limit) {
+        output = limit;
+        integral = output - proportional;
+    } else if (output < -limit) {
+        output = -limit;
+        integral = output - proportional;
+    }
+    
     // saving for the next pass
     integral_prev = integral;
     output_prev = output;