Index: firmware/App/Controllers/Heaters.c
===================================================================
diff -u -rbe027f8c9bee75f07267f7a9ab5635bbb02df86e -rd838fbaa8507b883fa112e69137e86fc9fb2cb6f
--- firmware/App/Controllers/Heaters.c	(.../Heaters.c)	(revision be027f8c9bee75f07267f7a9ab5635bbb02df86e)
+++ firmware/App/Controllers/Heaters.c	(.../Heaters.c)	(revision d838fbaa8507b883fa112e69137e86fc9fb2cb6f)
@@ -274,8 +274,10 @@
  *************************************************************************/
 void stopPrimaryHeater ( void )
 {
-    setMainPrimaryHeaterPWM ( HEATERS_MIN_DUTY_CYCLE );
-    setSmallPrimaryHeaterPWM ( HEATERS_MIN_DUTY_CYCLE );
+    mainPrimaryHeaterDutyCycle = HEATERS_MIN_DUTY_CYCLE;
+    smallPrimaryHeaterDutyCycle = HEATERS_MIN_DUTY_CYCLE;
+    setMainPrimaryHeaterPWM ( mainPrimaryHeaterDutyCycle );
+    setSmallPrimaryHeaterPWM ( smallPrimaryHeaterDutyCycle );
     isPrimaryHeaterOn = FALSE;
 }
 
@@ -290,7 +292,8 @@
  *************************************************************************/
 void stopTrimmerHeater ( void )
 {
-    setTrimmerHeaterPWM ( HEATERS_MIN_DUTY_CYCLE );
+    trimmerHeaterDutyCycle = HEATERS_MIN_DUTY_CYCLE;
+    setTrimmerHeaterPWM ( trimmerHeaterDutyCycle );
     isTrimmerHeaterOn = FALSE;
 }
 
@@ -556,9 +559,12 @@
         resetHeaterState ( PRIMARY_HEATER );
         setMainPrimaryHeaterPWM ( mainPrimaryHeaterDutyCycle );
         isPrimaryHeaterOn = TRUE;
+        hasStartPrimaryHeaterRequested = FALSE;
         state = PRIMARY_HEATERS_EXEC_STATE_CONTROL_TO_TARGET;
     }
 
+    publishHeatersData();
+
     return state;
 }
 
@@ -602,6 +608,7 @@
         }
         primaryHeaterTimerCounter = 0;
     }
+
     publishHeatersData();
 
     // TODO remove this code
@@ -667,10 +674,13 @@
     {
         resetHeaterState ( TRIMMER_HEATER );
         isTrimmerHeaterOn = TRUE;
+        hasStartTrimmerHeaterRequested = FALSE;
         setTrimmerHeaterPWM ( trimmerHeaterDutyCycle );
         state = TRIMMER_HEATER_EXEC_STATE_CONTROL_TO_TARGET;
     }
 
+    publishHeatersData();
+
     return state;
 }
 
@@ -697,6 +707,7 @@
         setTrimmerHeaterPWM ( trimmerHeaterDutyCycle );
         trimmerHeaterTimerCounter = 0;
     }
+
     publishHeatersData();
 
     // TODO remove this code
@@ -838,43 +849,11 @@
 #ifdef DEBUG_ENABLED
     {
        char debugTempStr[ 256 ];
-       // TODO: clean up.
-       // To the reviewers: please ignore this section
-       /*sprintf( debugTempStr, "MPDC: %2d, SPDC: %2d, THDC: %2d, TPi: %6.2f, TPo: %6.2f, TD1: %6.2f, "
-                              "TD2: %6.2f, TRo: %6.2f, TDi: %6.2f, TPh: %6.2f, TTh: %6.2f, PTarg: %6.2f, TTarg: %6.2f, ROFlow: %6.2f, PCJ: %6.2f, TCJ: %6.2f\r\n",
-                              (U32)(mainPrimaryHeaterDutyCycle*100), (U32)(smallPrimaryHeaterDutyCycle*100), (U32)(trimmerHeaterDutyCycle*100),
-                              getTemperatureValue ( TEMPSENSORS_INLET_PRIMARY_HEATER_TEMP_SENSOR ),
-                              getTemperatureValue ( TEMPSENSORS_OUTLET_PRIMARY_HEATER_TEMP_SENSOR ),
-                              getTemperatureValue ( TEMPSENSORS_CONDUCTIVITY_SENSOR_1_TEMP_SENSOR ),
-                              getTemperatureValue ( TEMPSENSORS_CONDUCTIVITY_SENSOR_2_TEMP_SENSOR ),
-                              getTemperatureValue ( TEMPSENSORS_OUTLET_REDUNDANCY_TEMP_SENSOR ),
-                              getTemperatureValue ( TEMPSENSORS_INLET_DIALYSATE_TEMP_SENSOR ),
-                              getTemperatureValue ( TEMPSENSORS_PRIMARY_HEATER_THERMO_COUPLE_TEMP_SENSOR ),
-                              getTemperatureValue ( TEMPSENSORS_TRIMMER_HEATER_THERMO_COUPLE_TEMP_SESNOR ),
-                              primaryHeaterTargetTemperature, trimmerHeaterTargetTemperature,
-                              (F32)(10909.0909/getFPGAROPumpFlowRate()),
-                              getTemperatureValue ( TEMPSENSORS_PRIMARY_HEATER_COLD_JUNCTION_TEMP_SENSOR ),
-                              getTemperatureValue ( TEMPSENSORS_TRIMMER_HEATER_COLD_JUNCTION_TEMP_SENSOR ))
-                              //(F32)(convertedPriJC*0.0625),
-                              //(F32)(convertedTriJC*0.0625)); //TODO Remove flow reader later
-       sprintf( debugTempStr, "MPDC: %2d, SPDC: %2d, THDC: %2d, TPi: %6.2f, TPo: %6.2f, TRo: %6.2f, TPh: %6.2f, TTh: %6.2f, PCJ: %6.2f, TCJ: %6.2f,"
-                              "TPI: %6.2f, TTI: %6.2f\r\n",
-                              (U32)(mainPrimaryHeaterDutyCycle*100), (U32)(smallPrimaryHeaterDutyCycle*100), (U32)(trimmerHeaterDutyCycle*100),
-                              getTemperatureValue ( TEMPSENSORS_INLET_PRIMARY_HEATER_TEMP_SENSOR ),
-                              getTemperatureValue ( TEMPSENSORS_OUTLET_PRIMARY_HEATER_TEMP_SENSOR ),
-                              getTemperatureValue ( TEMPSENSORS_OUTLET_REDUNDANCY_TEMP_SENSOR ),
-                              getTemperatureValue ( TEMPSENSORS_PRIMARY_HEATER_INTERNAL_TEMP ),
-                              getTemperatureValue ( TEMPSENSORS_TRIMMER_HEATER_INTERNAL_TEMP),
-                              getTemperatureValue ( TEMPSENSORS_PRIMARY_HEATER_COLD_JUNCTION_TEMP_SENSOR ),
-                              getTemperatureValue ( TEMPSENSORS_TRIMMER_HEATER_COLD_JUNCTION_TEMP_SENSOR ),
-                              getTemperatureValue ( TEMPSENSORS_PRIMARY_HEATER_THERMO_COUPLE_TEMP_SENSOR),
-                              getTemperatureValue ( TEMPSENSORS_TRIMMER_HEATER_THERMO_COUPLE_TEMP_SENSOR));*/
        sprintf( debugTempStr, "TPo: %6.2f, PriCJ: %6.2f, PriTC: %6.2f, TpriInt: %6.2f, TRo: %6.2f, TriCJ: %6.2f, TriTC: %6.2f, TtriInt: %6.2f\r\n",
                               getTemperatureValue ( TEMPSENSORS_OUTLET_PRIMARY_HEATER_TEMP_SENSOR ),
                               getTemperatureValue ( TEMPSENSORS_PRIMARY_HEATER_COLD_JUNCTION_TEMP_SENSOR ),
                               getTemperatureValue ( TEMPSENSORS_PRIMARY_HEATER_THERMO_COUPLE_TEMP_SENSOR ),
                               getTemperatureValue ( TEMPSENSORS_PRIMARY_HEATER_INTERNAL_TEMP ),
-
                               getTemperatureValue ( TEMPSENSORS_OUTLET_REDUNDANCY_TEMP_SENSOR ),
                               getTemperatureValue ( TEMPSENSORS_TRIMMER_HEATER_COLD_JUNCTION_TEMP_SENSOR ),
                               getTemperatureValue ( TEMPSENSORS_TRIMMER_HEATER_THERMO_COUPLE_TEMP_SENSOR ),
Index: firmware/App/Controllers/TemperatureSensors.c
===================================================================
diff -u -r69c1c495f29d102b2bfac2045b7611e63a8bad08 -rd838fbaa8507b883fa112e69137e86fc9fb2cb6f
--- firmware/App/Controllers/TemperatureSensors.c	(.../TemperatureSensors.c)	(revision 69c1c495f29d102b2bfac2045b7611e63a8bad08)
+++ firmware/App/Controllers/TemperatureSensors.c	(.../TemperatureSensors.c)	(revision d838fbaa8507b883fa112e69137e86fc9fb2cb6f)
@@ -802,7 +802,7 @@
         }
 
         broadcastTemperatureSensorsData ( (U08*)&tempValuesForPublication,
-                                          sizeof(tempValuesForPublication) );
+                                          NUM_OF_TEMPERATURE_SENSORS * sizeof(F32) );
 
         dataPublicationTimerCounter = 0;
     }
Index: firmware/App/Services/SystemComm.c
===================================================================
diff -u -r3cc737c39b4440e18abab95ef2f8b6aef13acba9 -rd838fbaa8507b883fa112e69137e86fc9fb2cb6f
--- firmware/App/Services/SystemComm.c	(.../SystemComm.c)	(revision 3cc737c39b4440e18abab95ef2f8b6aef13acba9)
+++ firmware/App/Services/SystemComm.c	(.../SystemComm.c)	(revision d838fbaa8507b883fa112e69137e86fc9fb2cb6f)
@@ -1093,6 +1093,10 @@
                 handleTestTemperatureSensorsOverrideRequest ( message );
                 break;
 
+            case MSG_ID_TEMPERATURE_SENSORS_PUBLISH_INTERVAL_OVERRIDE:
+                handleTestTemperatureSensorsDataPublishOverrideRequest ( message );
+                break;
+
             case MSG_ID_HEATERS_PUBLISH_INTERVAL_ORVERRIDE:
                 handleTestHeatersDataPublishOverrideRequest ( message );
                 break;
Index: firmware/App/Services/SystemCommMessages.c
===================================================================
diff -u -r9e114bbc273088bff2739e8ea54d3af66b6cd141 -rd838fbaa8507b883fa112e69137e86fc9fb2cb6f
--- firmware/App/Services/SystemCommMessages.c	(.../SystemCommMessages.c)	(revision 9e114bbc273088bff2739e8ea54d3af66b6cd141)
+++ firmware/App/Services/SystemCommMessages.c	(.../SystemCommMessages.c)	(revision d838fbaa8507b883fa112e69137e86fc9fb2cb6f)
@@ -948,18 +948,25 @@
  *************************************************************************/
 BOOL handleStartStopPrimaryHeater( MESSAGE_T * message )
 {
-    BOOL result;
-    U32 heaterStatus = message->payload[0];
+    BOOL result = FALSE;
 
-    if ( heaterStatus == 1 )
+    if ( message->hdr.payloadLen == sizeof(U32) )
     {
-        result = startPrimaryHeater();
+        BOOL startingHeater;
+
+        memcpy( &startingHeater, message->payload, sizeof(U32) );
+
+        if ( TRUE == startingHeater )
+        {
+            result = startPrimaryHeater();
+        }
+        else
+        {
+            stopPrimaryHeater();
+            result = TRUE;
+        }
     }
-    else
-    {
-        stopPrimaryHeater();
-        result = TRUE;
-    }
+    sendAckResponseMsg( (MSG_ID_T)message->hdr.msgID, COMM_BUFFER_OUT_CAN_DG_2_HD, result );
 
     return result;
 }
Index: firmware/App/Services/SystemCommMessages.h
===================================================================
diff -u -r3cc737c39b4440e18abab95ef2f8b6aef13acba9 -rd838fbaa8507b883fa112e69137e86fc9fb2cb6f
--- firmware/App/Services/SystemCommMessages.h	(.../SystemCommMessages.h)	(revision 3cc737c39b4440e18abab95ef2f8b6aef13acba9)
+++ firmware/App/Services/SystemCommMessages.h	(.../SystemCommMessages.h)	(revision d838fbaa8507b883fa112e69137e86fc9fb2cb6f)
@@ -116,6 +116,9 @@
 // MSG_ID_TEMPERATURE_SENSORS_OVERRIDE
 void handleTestTemperatureSensorsOverrideRequest ( MESSAGE_T *message );
 
+// MSG_ID_TEMPERATURE_SENSORS_DATA_PUBLISH_OVERRIDE
+void handleTestTemperatureSensorsDataPublishOverrideRequest ( MESSAGE_T *message );
+
 // MSG_ID_PUBLSIH_HEATERS_DATA_OVERRIDE
 void handleTestHeatersDataPublishOverrideRequest ( MESSAGE_T *message );