Index: firmware/App/Services/PIControllers.c
===================================================================
diff -u -r25770b4b27e75e730d89ac046f7db405b1bfe39f -r9ce06772b2f651c57144327e6cbf886e2bc22dee
--- firmware/App/Services/PIControllers.c	(.../PIControllers.c)	(revision 25770b4b27e75e730d89ac046f7db405b1bfe39f)
+++ firmware/App/Services/PIControllers.c	(.../PIControllers.c)	(revision 9ce06772b2f651c57144327e6cbf886e2bc22dee)
@@ -15,7 +15,7 @@
 *
 ***************************************************************************/
 
-#include "math.h"
+#include <math.h>
 
 #include "SystemCommMessages.h"
 #include "PIControllers.h"
@@ -30,9 +30,9 @@
 /// Enumeration of PI controller direction.
 typedef enum controller_Directions
 {
-    CONTROLLER_BIDIRECTIONAL = 0,          ///< Controller runs bidirectional so it only covers positive and negative values
-    CONTROLLER_UNIDIRECTIONAL,             ///< Controller run unidirectional so it only covers positive values
-    NUM_OF_CONTROLLELR_DIRECTIONS          ///< Number of PI controllers directions
+    CONTROLLER_BIDIRECTIONAL = 0,           ///< Controller runs bidirectional so it covers positive and negative control signals
+    CONTROLLER_UNIDIRECTIONAL,              ///< Controller runs unidirectional so it only covers positive control signals
+    NUM_OF_CONTROLLELR_DIRECTIONS           ///< Number of PI controllers directions
 } PI_CONTROLLER_DIRECTIONS_T;
 
 /// Record for PI controller.
@@ -43,12 +43,13 @@
     F32 uMax;                               ///< Maximum control signal.
     F32 uMin;                               ///< Minimum control signal.
     // -- PI's signals --
-    F32 referenceSignal;                    ///< reference signal.
-    F32 measuredSignal;                     ///< measured signal.
-    F32 errorSignal;                        ///< reference - measured signal.
-    F32 errorSumBeforeWindUp;               ///< error signal before windup correction.
-    F32 errorSum;                           ///< error integral after windup correction.
-    F32 controlSignal;                      ///< actual control signal.
+    F32 referenceSignal;                    ///< Reference signal.
+    F32 measuredSignal;                     ///< Measured signal.
+    F32 errorSignal;                        ///< Reference - measured signal.
+    F32 errorSumBeforeWindUp;               ///< Error signal before windup correction.
+    F32 errorSum;                           ///< Error integral after windup correction.
+    F32 controlSignal;                      ///< Actual control signal.
+    F32 maxErrorSumStep;                    ///< Maximum change in I (error sum) for a single control interval.
     PI_CONTROLLER_DIRECTIONS_T direction;   ///< PI controller control direction.
 } PI_CONTROLLER_T;
 
@@ -83,14 +84,14 @@
 void initializePIController( PI_CONTROLLER_ID_T controllerID, F32 initialControlSignal,
                              F32 kP, F32 kI, F32 controlMin, F32 controlMax )
 {
-    PI_CONTROLLER_T *controller;
-
     if ( controllerID < NUM_OF_PI_CONTROLLERS_IDS )
     {
+        PI_CONTROLLER_T *controller;
+
         SET_CONTROLLER( controller, controllerID );
 
         controller->Kp = kP;
-        if ( fabs( kI ) > MIN_KI )      // ensure kI is not zero
+        if ( fabs( kI ) > MIN_KI )      // Ensure kI is not zero
         {
             controller->Ki = kI;
         }
@@ -174,18 +175,39 @@
             controller->errorSignal = referenceSignal - measuredSignal;
         }
 
-        controller->errorSum += controller->errorSignal;
-        // anti-windup
+        // Limit error sum step size
+        if ( fabs( controller->errorSignal ) > controller->maxErrorSumStep )
+        {
+            if ( controller->errorSignal < 0.0 )
+            {
+                controller->errorSum += ( controller->maxErrorSumStep * -1.0 );
+            }
+            else
+            {
+                controller->errorSum += controller->maxErrorSumStep;
+            }
+        }
+        else
+        {
+            controller->errorSum += controller->errorSignal;
+        }
+
+        // Calculate control signal from i term
         controller->errorSumBeforeWindUp = controller->errorSum;
-        // calculate control signal
-        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
+
+        // Handle anti-windup for i term
         windupError = controlSignalBeforeWindup - controller->controlSignal;
         if ( fabs( windupError ) > NEARLY_ZERO )
         {
             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;
     }
     else
@@ -251,10 +273,10 @@
             default:
                 SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_SIGNAL, (U32)signalID )
                 break;
-        } // end of switch
+        } // End of switch
     }
     else
-    {   // invalid controller given
+    {   // Invalid controller given
         SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID )
     }