antiwindup update Foxboro type simplefoc#59

Author: askuric

src/common/lowpass_filter.cpp

@@ -13,12 +13,15 @@ float LowPassFilter::operator() (float x)
     unsigned long timestamp = _micros();
     float dt = (timestamp - timestamp_prev)*1e-6f;
 
-    if (dt < 0.0f || dt > 0.5f)
-        dt = 1e-3f;
+    if (dt < 0.0f ) dt = 1e-3f;
+    else if(dt > 0.3f) {
+        y_prev = x;
+        timestamp_prev = timestamp;
+        return x;
+    }
 
     float alpha = Tf/(Tf + dt);
     float y = alpha*y_prev + (1.0f - alpha)*x;
-
     y_prev = y;
     timestamp_prev = timestamp;
     return y;

src/common/pid.cpp

@@ -28,9 +28,11 @@ float PIDController::operator() (float error){
     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.5f*(error + error_prev);
+    // method uses the antiwindup Foxboro method : https://core.ac.uk/download/pdf/289952713.pdf
+    float integral = integral_prev + I*Ts*0.5f*(error + error_prev) + integral_antiwindup_prev*I;
     // antiwindup - limit the output
-    integral = _constrain(integral, -limit, limit);
+    float integral_constrained = _constrain(integral, -limit, limit);
+    integral_antiwindup_prev = integral - integral_constrained;
     // Discrete derivation
     // u_dk = D(ek - ek_1)/Ts
     float derivative = D*(error - error_prev)/Ts;

src/common/pid.h

@@ -30,11 +30,13 @@ class PIDController
     float output_ramp; //!< Maximum speed of change of the output value
     float limit; //!< Maximum output value
 
-protected:
+    float output_prev;  //!< last pid output value
     float integral_prev; //!< last integral component value
+
+protected:
+    float integral_antiwindup_prev; //!< last integral antiwindup component value
     float error_prev; //!< last tracking error value
     unsigned long timestamp_prev; //!< Last execution timestamp
-    float output_prev;  //!< last pid output value
 };
 
 #endif // PID_H