From 6159a75017121f375f56c0f9bc3d3b9a0c198a94 Mon Sep 17 00:00:00 2001
From: Brian Carcich <briantcarcich@gmail.com>
Date: Sun, 21 Mar 2021 18:55:33 -0400
Subject: [PATCH] Update approach to anti-wind-up in PID.c

Current approach (re-written to be more concise, although possibly less clear):

  I = I + deltaT * Ki * mean_error;
  I = (I>limImax) ? limImax : ((I<limImin) ? limiImin : I);

  /* Calculate out from P, I, and D, then clamp */
  out = P + I + D;
  out = (out>limMax) ? limMax : ((out<limMin) ? limiMin : out);

Example where this fails, or at least performs poorly:

- Assume

  P = 90%; D = 0%; I = 20%; limImax=50%; limMax = 100%;

  /* I < limImax, so no anti-windup is in effect; however ... */

  out = P + I + D;                    /* => 110%, i.e. out is over-saturated */
  out - (out>limMax) ? limMax : ...;  /* => 100%, out is clamped, but I is not */

New approach:

  I = I + deltaT * Ki * mean_error;   /* Same */
  /* Remove static limiting of I */

  /* Same */
  out = P + I + D;
  out = (out>limMax) ? limMax : ((out<limMin) ? limiMin : out);

  /* New - solve for I from [out=P+I+D] above */
  I = out - (P + D);   /* [I] will not change unless out is being clamped
---
 PID.c | 15 +++++----------
 1 file changed, 5 insertions(+), 10 deletions(-)

diff --git a/PID.c b/PID.c
index 877c32d..02e0f9c 100644
--- a/PID.c
+++ b/PID.c
@@ -32,16 +32,7 @@ float PIDController_Update(PIDController *pid, float setpoint, float measurement
 	*/
     pid->integrator = pid->integrator + 0.5f * pid->Ki * pid->T * (error + pid->prevError);
 
-	/* Anti-wind-up via integrator clamping */
-    if (pid->integrator > pid->limMaxInt) {
-
-        pid->integrator = pid->limMaxInt;
-
-    } else if (pid->integrator < pid->limMinInt) {
-
-        pid->integrator = pid->limMinInt;
-
-    }
+    /* Anti-windup moved below */
 
 
 	/*
@@ -60,10 +51,14 @@ float PIDController_Update(PIDController *pid, float setpoint, float measurement
 
     if (pid->out > pid->limMax) {
 
+        /* Anti-wind-up for over-saturated output */
+        pid->integrator += pid->limMax - pid->out;
         pid->out = pid->limMax;
 
     } else if (pid->out < pid->limMin) {
 
+        /* Anti-wind-up for under-saturated output */
+        pid->integrator += pid->limMin - pid->out;
         pid->out = pid->limMin;
 
     }