Index: firmware/App/Controllers/Heaters.c
===================================================================
diff -u -r26c64288f4baab3707eb9555636b61c0e747f965 -r428b61f33828c36245ce3c3d8ed86a00797791f3
--- firmware/App/Controllers/Heaters.c	(.../Heaters.c)	(revision 26c64288f4baab3707eb9555636b61c0e747f965)
+++ firmware/App/Controllers/Heaters.c	(.../Heaters.c)	(revision 428b61f33828c36245ce3c3d8ed86a00797791f3)
@@ -97,8 +97,8 @@
 static U32                              primaryHeaterTimerCounter;      ///< Primary heater timer counter
 static U32                              trimmerHeaterTimerCounter;      ///< Trimmer heater timer counter
 static U32                              dataPublicationTimerCounter;    ///< Data publication timer counter
-static BOOL                             isPrimaryHeaterOn;              ///< Flag to show if the primary heater is on   //TODO add to AE
-static BOOL                             isTrimmerHeaterOn;              ///< Flag to show if the trimmer heater is on   //TODO add to AE
+static BOOL                             isPrimaryHeaterOn;              ///< Flag to show if the primary heater is on
+static BOOL                             isTrimmerHeaterOn;              ///< Flag to show if the trimmer heater is on
 
 // private functions prototypes
 
@@ -139,16 +139,17 @@
  *************************************************************************/
 void initHeaters ( void )
 {
-    heatersSelfTestResult   = SELF_TEST_STATUS_IN_PROGRESS;
-    heatersSelfTestState    = HEATERS_SELF_TEST_START;
-    primaryHeatersExecState = PRIMARY_HEATERS_EXEC_STATE_OFF;
-    trimmerHeaterExecState  = TRIMMER_HEATER_EXEC_STATE_OFF;
-
+    heatersSelfTestResult          = SELF_TEST_STATUS_IN_PROGRESS;
+    heatersSelfTestState           = HEATERS_SELF_TEST_START;
+    primaryHeatersExecState        = PRIMARY_HEATERS_EXEC_STATE_OFF;
+    trimmerHeaterExecState         = TRIMMER_HEATER_EXEC_STATE_OFF;
     primaryHeaterTargetTemperature = 0.0;
     trimmerHeaterTargetTemperature = 0.0;
     primaryHeaterTimerCounter      = 0;
     trimmerHeaterTimerCounter      = 5;
     dataPublicationTimerCounter    = 0;
+    isPrimaryHeaterOn              = FALSE;
+    isTrimmerHeaterOn              = FALSE;
 
     // Initialize the PI controller for the primary heaters
     initializePIController( PI_CONTROLLER_ID_PRIMARY_HEATER, HEATERS_MIN_DUTY_CYCLE,
@@ -197,8 +198,9 @@
  * The startPrimaryHeater function starts the primary heaters. It resets
  * the primary heaters state and sets the main primary heater duty cycle
  * @details
- * Inputs : mainPrimaryHeaterDutyCycle, primaryHeaterTargetTemperature
- * Outputs : primaryHeatersExecState
+ * Inputs : mainPrimaryHeaterDutyCycle, primaryHeaterTargetTemperature,
+ * isPrimaryHeaterOn
+ * Outputs : isPrimaryHeaterOn
  * @param none
  * @return status
  *************************************************************************/
@@ -210,7 +212,7 @@
     {
         resetHeaterState ( PRIMARY_HEATER );
         setMainPrimaryHeaterPWM ( mainPrimaryHeaterDutyCycle );
-        primaryHeatersExecState = PRIMARY_HEATERS_EXEC_STATE_CONTROL_TO_TARGET;
+        isPrimaryHeaterOn = TRUE;
         status = TRUE;
     }
 
@@ -222,8 +224,9 @@
  * The startTrimmerHeater function starts the trimmer heater. It resets the
  * trimmer heater's state and sets the duty cycle of the trimmer heater
  * @details
- * Inputs : trimmerHeaterDutyCycle, trimmerHeaterTargetTemperature
- * Outputs : trimmerHeaterExecState
+ * Inputs : trimmerHeaterDutyCycle, trimmerHeaterTargetTemperature,
+ * isTrimmerHeaterOn
+ * Outputs : isTrimmerHeaterOn
  * @param none
  * @return status
  *************************************************************************/
@@ -235,7 +238,7 @@
     {
         resetHeaterState ( TRIMMER_HEATER );
         setTrimmerHeaterPWM ( trimmerHeaterDutyCycle );
-        trimmerHeaterExecState = TRIMMER_HEATER_EXEC_STATE_CONTROL_TO_TARGET;
+        isTrimmerHeaterOn = TRUE;
         status = TRUE;
     }
 
@@ -246,31 +249,31 @@
  * @brief
  * The stopPrimaryHeater function stops the primary heaters
  * @details
- * Inputs : none
- * Outputs : primaryHeatersExecState
+ * Inputs : isPrimaryHeaterOn
+ * Outputs : isPrimaryHeaterOn
  * @param none
  * @return none
  *************************************************************************/
 void stopPrimaryHeater ( void )
 {
     setMainPrimaryHeaterPWM ( HEATERS_MIN_DUTY_CYCLE );
     setSmallPrimaryHeaterPWM ( HEATERS_MIN_DUTY_CYCLE );
-    primaryHeatersExecState = PRIMARY_HEATERS_EXEC_STATE_OFF;
+    isPrimaryHeaterOn = FALSE;
 }
 
 /*********************************************************************//**
  * @brief
  * The stopTrimmerHeater function stops the trimmer heater
  * @details
- * Inputs : none
- * Outputs : trimmerHeaterExecState
+ * Inputs : isTrimmerHeaterOn
+ * Outputs : isTrimmerHeaterOn
  * @param none
  * @return none
  *************************************************************************/
 void stopTrimmerHeater ( void )
 {
     setTrimmerHeaterPWM ( HEATERS_MIN_DUTY_CYCLE );
-    trimmerHeaterExecState = TRIMMER_HEATER_EXEC_STATE_OFF;
+    isTrimmerHeaterOn = FALSE;
 }
 
 /*********************************************************************//**
@@ -387,31 +390,33 @@
  * The handlePrimaryHeaterStateOff function handles the primary heaters at
  * off state
  * @details
- * Inputs : none
+ * Inputs : isPrimaryHeaterOn
  * Outputs : state (PRIMARY_HEATERS_EXEC_STATES_T)
  * @param none
  * @return state (PRIMARY_HEATERS_EXEC_STATES_T)
  *************************************************************************/
 static PRIMARY_HEATERS_EXEC_STATES_T handlePrimaryHeaterStateOff ( void )
 {
-    PRIMARY_HEATERS_EXEC_STATES_T state = PRIMARY_HEATERS_EXEC_STATE_CONTROL_TO_TARGET;
+    PRIMARY_HEATERS_EXEC_STATES_T state = PRIMARY_HEATERS_EXEC_STATE_OFF;
 
     // TODO for testing only. remove
-    state = PRIMARY_HEATERS_EXEC_STATE_OFF;
     if ( TOGGLEPRIMAYHEATER() )
     {
         setPrimaryHeaterTargetTemperature ( 37 );
         //if ( getTemperatureValue ( TEMPSENSORS_INLET_PRIMARY_HEATER_TEMP_SENSOR ) > 0 )
         {
             startPrimaryHeater();
-            state = PRIMARY_HEATERS_EXEC_STATE_CONTROL_TO_TARGET;
             temporaryStartROPump();
             temporaryStartFan();
         }
-        //state = PRIMARY_HEATERS_EXEC_STATE_CONTROL_TO_TARGET;
     }
     // TODO remove this code for testing
 
+    if ( isPrimaryHeaterOn )
+    {
+        state = PRIMARY_HEATERS_EXEC_STATE_CONTROL_TO_TARGET;
+    }
+
     return state;
 }
 
@@ -421,7 +426,7 @@
  * heaters at control state when the heaters are active
  * @details
  * Inputs : primaryHeaterTimerCounter, mainPrimaryHeaterDutyCycle,
- * smallPrimaryHeaterDutyCycle
+ * smallPrimaryHeaterDutyCycle, isPrimaryHeaterOn
  * Outputs : primaryHeaterTimerCounter, mainPrimaryHeaterDutyCycle,
  * smallPrimaryHeaterDutyCycle
  * @param none
@@ -434,10 +439,14 @@
     if ( ++primaryHeaterTimerCounter >= CONTROLLER_CHECK_INTERVAL_COUNT )
     {
         F32 outletTemp = getTemperatureValue ( TEMPSENSORS_OUTLET_PRIMARY_HEATER_TEMP_SENSOR );
-        mainPrimaryHeaterDutyCycle = runPIController ( PI_CONTROLLER_ID_PRIMARY_HEATER, primaryHeaterTargetTemperature, outletTemp );
+        mainPrimaryHeaterDutyCycle = runPIController ( PI_CONTROLLER_ID_PRIMARY_HEATER,
+                                                       primaryHeaterTargetTemperature, outletTemp );
 
         if ( mainPrimaryHeaterDutyCycle >= MAIN_PRIMARY_HEATER_MAX_DUTY_CYCLE )
         {
+            // The duty cycle from the PI controller was greater than max duty cycle of the main primary
+            // heater. So subtract the remaining from the max main primary heater duty cycle and set
+            // the rest to the small primary heater
             smallPrimaryHeaterDutyCycle = mainPrimaryHeaterDutyCycle - MAIN_PRIMARY_HEATER_MAX_DUTY_CYCLE;
             mainPrimaryHeaterDutyCycle = MAIN_PRIMARY_HEATER_MAX_DUTY_CYCLE;
             setMainPrimaryHeaterPWM ( mainPrimaryHeaterDutyCycle );
@@ -459,10 +468,14 @@
         stopPrimaryHeater();
         temporaryStopROPump();
         temporaryStopFan();
-        state = PRIMARY_HEATERS_EXEC_STATE_OFF;
     }
     // TODO Remove this code
 
+    if ( ! isPrimaryHeaterOn )
+    {
+        state = PRIMARY_HEATERS_EXEC_STATE_OFF;
+    }
+
     return state;
 }
 
@@ -471,7 +484,7 @@
  * The handleTrimmerHeaterStateOff function handles the trimmer heater at
  * off state
  * @details
- * Inputs : none
+ * Inputs : isTrimmerHeaterOn
  * Outputs : state (TRIMMER_HEATER_EXEC_STATES_T)
  * @param none
  * @return state (TRIMMER_HEATER_EXEC_STATES_T)
@@ -488,14 +501,18 @@
         //if ( getTemperatureValue ( TEMPSENSORS_OUTLET_REDUNDANCY_TEMP_SENSOR ) > 0 )
         {
             startTrimmerHeater();
-            state = TRIMMER_HEATER_EXEC_STATE_CONTROL_TO_TARGET;
             //temporaryStartROPump();
             //temporaryStartFan();
         }
         //state = TRIMMER_HEATER_EXEC_STATE_CONTROL_TO_TARGET;
     }
     // TODO remove this code for testing
 
+    if ( isTrimmerHeaterOn )
+    {
+        state = TRIMMER_HEATER_EXEC_STATE_CONTROL_TO_TARGET;
+    }
+
     return state;
 }
 
@@ -505,7 +522,8 @@
  * heater at control state when the heater is active
  * @details
  * Inputs : trimmerHeaterTimerCounter, trimmerHeaterDutyCycle
- * Outputs : trimmerHeaterTimerCounter, trimmerHeaterDutyCycle
+ * Outputs : trimmerHeaterTimerCounter, trimmerHeaterDutyCycle,
+ * isTrimmerHeaterOn
  * @param none
  * @return state (TRIMMER_HEATER_EXEC_STATES_T)
  *************************************************************************/
@@ -529,10 +547,14 @@
         stopTrimmerHeater();
         temporaryStopROPump();
         temporaryStopFan();
-        state = TRIMMER_HEATER_EXEC_STATE_OFF;
     }
     // TODO Remove this code
 
+    if ( ! isTrimmerHeaterOn )
+    {
+        state = TRIMMER_HEATER_EXEC_STATE_OFF;
+    }
+
     return state;
 }