Index: firmware/App/Services/PIControllers.c
===================================================================
diff -u -r766708fceb0bdf1af8c7897df29d4f5036bfd3db -r1c1a8d0528f19da5d94bdf75f687bbb1d1de6f0e
--- firmware/App/Services/PIControllers.c	(.../PIControllers.c)	(revision 766708fceb0bdf1af8c7897df29d4f5036bfd3db)
+++ firmware/App/Services/PIControllers.c	(.../PIControllers.c)	(revision 1c1a8d0528f19da5d94bdf75f687bbb1d1de6f0e)
@@ -195,9 +195,9 @@
             controller->errorSum += controller->errorSignal;
         }
 
-        // Calculate control signal
+        // Calculate control signal from i term
         controller->errorSumBeforeWindUp = controller->errorSum;
-        controlSignalBeforeWindup = ( controller->Kp * controller->errorSignal ) + ( controller->Ki * controller->errorSum );
+        controlSignalBeforeWindup = ( controller->Ki * controller->errorSum );
         controller->controlSignal = RANGE( controlSignalBeforeWindup, controller->uMin, controller->uMax );
 
         // Handle anti-windup for i term
@@ -206,6 +206,10 @@
         {
             controller->errorSum -= ( windupError / controller->Ki );
         }
+
+        // Add p term to control signal and re-apply range limits
+        controller->controlSignal += ( controller->Kp * controller->errorSignal );
+        controller->controlSignal = RANGE( controller->controlSignal, controller->uMin, controller->uMax );
 
         result = controller->controlSignal;
     }