Index: firmware/App/HDCommon.h
===================================================================
diff -u -r778f343622819491cf644dab36f3194709ce1e95 -r1c1a8d0528f19da5d94bdf75f687bbb1d1de6f0e
--- firmware/App/HDCommon.h	(.../HDCommon.h)	(revision 778f343622819491cf644dab36f3194709ce1e95)
+++ firmware/App/HDCommon.h	(.../HDCommon.h)	(revision 1c1a8d0528f19da5d94bdf75f687bbb1d1de6f0e)
@@ -44,14 +44,14 @@
     #define DISABLE_ACCELS                  1           // Disable accelerometer POST and monitoring
     #define DISABLE_CRC_ERROR               1           // Do not error on bad CRC for CAN messages
     #define DISABLE_ACK_ERRORS              1           // Do not error on failure of other node(s) to ACK a message
-    #define DISABLE_MOTOR_CURRENT_CHECKS    1           // Do not error on HD pump current checks
-    #define DISABLE_PUMP_FLOW_CHECKS        1           // Do not error on HD pump flow checks
-    #define DISABLE_PUMP_SPEED_CHECKS       1           // Do not error on HD pump speed checks
-    #define DISABLE_PUMP_DIRECTION_CHECKS   1           // Do not error on HD pump direction checks
-    #define DISABLE_SALINE_BOLUS_CHECKS     1           // Do not error on HD saline bolus checks
-    #define DISABLE_PRESSURE_CHECKS         1           // Do not error on HD pressure checks
-    #define DISABLE_UF_ALARMS               1           // Do not error on HD ultrafiltration checks
-    #define DISABLE_VALVE_ALARMS            1           // Do not error on HD valve position
+//    #define DISABLE_MOTOR_CURRENT_CHECKS    1           // Do not error on HD pump current checks
+//    #define DISABLE_PUMP_FLOW_CHECKS        1           // Do not error on HD pump flow checks
+//    #define DISABLE_PUMP_SPEED_CHECKS       1           // Do not error on HD pump speed checks
+//    #define DISABLE_PUMP_DIRECTION_CHECKS   1           // Do not error on HD pump direction checks
+//    #define DISABLE_SALINE_BOLUS_CHECKS     1           // Do not error on HD saline bolus checks
+//    #define DISABLE_PRESSURE_CHECKS         1           // Do not error on HD pressure checks
+//    #define DISABLE_UF_ALARMS               1           // Do not error on HD ultrafiltration checks
+//    #define DISABLE_VALVE_ALARMS            1           // Do not error on HD valve position
 //    #define RUN_PUMPS_OPEN_LOOP             1           // BP and DPi pumps will be run open loop (no flow sensor feedback)
 //    #define RAW_FLOW_SENSOR_DATA            1           // Test build will not filter flow sensor data
 //    #define READ_FPGA_ASYNC_DATA            1           // Test build reads non-priority register page every other time
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;
     }