Index: firmware/App/Services/PIControllers.c
===================================================================
diff -u -r832ca83805dc132cb753e79bdca81197331e03e6 -r9864b14f76782f1e68bf266dcd843451748715a0
--- firmware/App/Services/PIControllers.c	(.../PIControllers.c)	(revision 832ca83805dc132cb753e79bdca81197331e03e6)
+++ firmware/App/Services/PIControllers.c	(.../PIControllers.c)	(revision 9864b14f76782f1e68bf266dcd843451748715a0)
@@ -15,8 +15,11 @@
  *
  **************************************************************************/
 
-#include "math.h"
+#ifndef _VECTORCAST_
+    #include "math.h"
+#endif
 
+#include "SystemCommMessages.h"
 #include "PIControllers.h"
 
 // ********** private definitions **********
@@ -144,12 +147,29 @@
         controlSignalBeforeWindup = ( controller->Kp * controller->errorSignal ) + ( controller->Ki * controller->errorSum );
         controller->controlSignal = RANGE( controlSignalBeforeWindup, controller->uMin, controller->uMax );
         // handle anti-windup for i term
-        windupError = controller->controlSignal - controlSignalBeforeWindup;
+        windupError = controlSignalBeforeWindup - controller->controlSignal;
         if ( fabs( windupError ) > NEARLY_ZERO )
         {
             controller->errorSum -= ( windupError / controller->Ki );
         }
         result = controller->controlSignal;
+//#ifdef DEBUG_ENABLED
+//    {
+//        // TODO - temporary debug code - remove later
+//        char debugFlowStr[ 100 ];
+//        S32  nums = (S32)(measuredSignal);
+//        S32  decs = (S32)(fabs(measuredSignal-(S32)(measuredSignal))*100.0);
+//        S32  nume = (S32)controller->errorSignal;
+//        S32  dece = (S32)(fabs(controller->errorSignal-(S32)controller->errorSignal)*100.0);
+//        S32  numes = (S32)controller->errorSum;
+//        S32  deces = (S32)((controller->errorSum-(S32)(controller->errorSum))*100.0);
+//        S32  nump = (S32)controller->controlSignal;
+//        S32  decp = (S32)((controller->controlSignal-(S32)controller->controlSignal)*10000.0);
+//
+//        sprintf( debugFlowStr, "%6d.%02d  %6d.%02d  %10d.%02d  %3d.%04d\n", nums, decs, nume, dece, numes, deces, nump, decp );
+//        sendDebugData( (U08*)debugFlowStr, strlen(debugFlowStr) );
+//    }
+//#endif
     }
 
     return result;