Index: firmware/App/Controllers/Heaters.c
===================================================================
diff -u -rf43eb0d0f168ea14b846d0c24f0ad0cb30784d3f -r3d413ac026e89a1324c4b0e99516735ec382fed8
--- firmware/App/Controllers/Heaters.c	(.../Heaters.c)	(revision f43eb0d0f168ea14b846d0c24f0ad0cb30784d3f)
+++ firmware/App/Controllers/Heaters.c	(.../Heaters.c)	(revision 3d413ac026e89a1324c4b0e99516735ec382fed8)
@@ -66,7 +66,7 @@
 #define TRIMMER_HEATER_CONTROL_CHECK_INTERVAL_COUNT                ( ( 30 * MS_PER_SECOND ) / TASK_GENERAL_INTERVAL )              ///< Trimmer heater control interval count.
 
 #define DELTA_TEMPERATURE_TIME_COSNTANT_C                          8.6F                                                            ///< Delta temperature calculated from time constant.
-#define PRIMARY_HEATER_DUTY_CYCLE_PER_TEMPERATURE_C                0.03F                                                           ///< Primary heaters duty cycle per temperature in C.
+#define PRIMARY_HEATER_DUTY_CYCLE_PER_TEMPERATURE_C                0.015F                                                          ///< Primary heaters duty cycle per temperature in C.
 #define DATA_PUBLISH_COUNTER_START_COUNT                           70                                                              ///< Data publish counter start count.
 
 static const F32 WATER_SPECIFIC_HEAT_DIVIDED_BY_MINUTES = 4184 / SEC_PER_MIN;                                                      ///< Water specific heat in J/KgC / 60.
@@ -360,20 +360,61 @@
 
 /*********************************************************************//**
  * @brief
- * The resetHeatersEfficiency function resets the heaters efficiency upon
+ * The resetHeatersEstimationGain function resets the heaters estimation gain upon
  * the start of a treatment.
  * @details Inputs: none
  * @details Outputs: heaterStatus
  * @return none
  *************************************************************************/
-void resetHeatersEfficiency( void )
+void resetHeatersEstimationGain( void )
 {
     heatersStatus[ DG_PRIMARY_HEATER ].heaterEstGain = HEATERS_NEUTRAL_EST_GAIN;
     heatersStatus[ DG_TRIMMER_HEATER ].heaterEstGain = HEATERS_NEUTRAL_EST_GAIN;
 }
 
 /*********************************************************************//**
  * @brief
+ * The calculateHeaterEstimationGain function calculates the heater estimation
+ * gain.
+ * @details Inputs: none
+ * @details Outputs: heatersStatus
+ * @return none
+ *************************************************************************/
+void calculateHeaterEstimationGain( DG_HEATERS_T heater )
+{
+#ifndef _RELEASE_
+    if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_HEATERS_EFFICIENCY ) != SW_CONFIG_ENABLE_VALUE )
+#endif
+    {
+        F32 heaterEstGain            = heatersStatus[ heater ].heaterEstGain;
+        F32 heaterDutyCycle          = heatersStatus[ heater ].dutycycle;
+        F32 lastFillTemperature      = getAvgFillTemperature();
+        F32 primaryTargetTemperature = heatersStatus[ heater ].targetTemp;
+        BOOL isTempUnderTarget       = ( lastFillTemperature < primaryTargetTemperature ? TRUE : FALSE );
+
+        if ( TRUE == isTempUnderTarget )
+        {
+            if ( heaterDutyCycle < HEATERS_MAX_DUTY_CYCLE )
+            {
+                heaterEstGain += ( primaryTargetTemperature - lastFillTemperature ) * PRIMARY_HEATER_DUTY_CYCLE_PER_TEMPERATURE_C;
+            }
+        }
+        else
+        {
+            if ( heaterDutyCycle > HEATERS_MIN_DUTY_CYCLE )
+            {
+                heaterEstGain -= ( lastFillTemperature - primaryTargetTemperature ) * PRIMARY_HEATER_DUTY_CYCLE_PER_TEMPERATURE_C;
+            }
+        }
+
+        heaterEstGain = MAX( heaterEstGain, HEATERS_MIN_EST_GAIN );
+        heaterEstGain = MIN( heaterEstGain, HEATERS_MAX_EST_GAIN );
+        heatersStatus[ heater ].heaterEstGain = heaterEstGain;
+    }
+}
+
+/*********************************************************************//**
+ * @brief
  * The handleHeaterStateOff function handles the heater not running state.
  * @details Inputs: heaterStatus
  * @details Outputs: heaterStatus
@@ -424,10 +465,9 @@
     else if ( ( DG_MODE_GENE == opMode ) || ( DG_MODE_DRAI == opMode ) )
     {
         targetTemperature += DELTA_TEMPERATURE_TIME_COSNTANT_C;
-        // Use target flow rate during Idle and drain
-        targetFlow = getTargetROPumpFlowRateLPM();
-        dutyCycle  = calculatePrimaryHeaterDutyCycle( targetTemperature, inletTemperature, targetFlow, FALSE );
-        state      = HEATER_EXEC_STATE_PRIMARY_CONTROL_TO_TARGET;
+        targetFlow         = getTargetROPumpFlowRateLPM();
+        dutyCycle          = calculatePrimaryHeaterDutyCycle( targetTemperature, inletTemperature, targetFlow, FALSE );
+        state              = HEATER_EXEC_STATE_PRIMARY_CONTROL_TO_TARGET;
     }
     else if ( ( DG_MODE_HEAT == opMode ) || ( DG_MODE_CHEM == opMode ) )
     {
@@ -437,6 +477,11 @@
         dutyCycle  = calculatePrimaryHeaterDutyCycle( targetTemperature, inletTemperature, targetFlow, FALSE );
         state      = HEATER_EXEC_STATE_CONTROL_TO_DISINFECT_TARGET;
     }
+    else
+    {
+        // No other modes are using the heaters
+        // TODO software fault
+    }
 
     // Update the calculated target temperature
     heatersStatus[ DG_PRIMARY_HEATER ].calculatedTemperature = targetTemperature;
@@ -631,49 +676,13 @@
  *************************************************************************/
 static F32 calculatePrimaryHeaterDutyCycle( F32 targetTemperature, F32 currentTemperature, F32 flow, BOOL checkEfficiency )
 {
-    // Get the primary heater's efficiency and the last fill temperature from the ModeFill
-    F32 heaterEstGain = heatersStatus[ DG_PRIMARY_HEATER ].heaterEstGain;
     // Duty cycle = ( 69.73 * flow rate * deltaT / primary heater maximum power ) ^ 1/2
     // Multiply the duty cycle to the heater efficiency
-    F32 dutyCycle        = sqrt( ( WATER_SPECIFIC_HEAT_DIVIDED_BY_MINUTES * fabs( targetTemperature - currentTemperature ) * flow ) /
-                                   PRIMARY_HEATERS_MAXIMUM_POWER_WATTS );
+    F32 dutyCycle = sqrt( ( WATER_SPECIFIC_HEAT_DIVIDED_BY_MINUTES * fabs( targetTemperature - currentTemperature ) * flow ) / PRIMARY_HEATERS_MAXIMUM_POWER_WATTS );
+    dutyCycle    *= heatersStatus[ DG_PRIMARY_HEATER ].heaterEstGain;
+    dutyCycle     = MIN( dutyCycle, HEATERS_MAX_DUTY_CYCLE );
+    dutyCycle     = MAX( dutyCycle, HEATERS_MIN_DUTY_CYCLE );
 
-#ifndef _RELEASE_
-    if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_HEATERS_EFFICIENCY ) != SW_CONFIG_ENABLE_VALUE )
-#endif
-    {
-        if ( ( TRUE == checkEfficiency ) && ( FALSE == isThisTheFirstFill() ) )
-        {
-            F32 heaterDutyCycle          = heatersStatus[ DG_PRIMARY_HEATER ].dutycycle;
-            F32 lastFillTemperature      = getLastFillTemperature();
-            F32 primaryTargetTemperature = heatersStatus[ DG_PRIMARY_HEATER ].targetTemp;
-            BOOL isTempUnderTarget       = ( lastFillTemperature < primaryTargetTemperature ? TRUE : FALSE );
-
-            if ( TRUE == isTempUnderTarget )
-            {
-                if ( heaterDutyCycle < HEATERS_MAX_DUTY_CYCLE )
-                {
-                    heaterEstGain += ( primaryTargetTemperature - lastFillTemperature ) * PRIMARY_HEATER_DUTY_CYCLE_PER_TEMPERATURE_C;
-                }
-            }
-            else
-            {
-                if ( heaterDutyCycle > HEATERS_MIN_DUTY_CYCLE )
-                {
-                    heaterEstGain -= ( lastFillTemperature - primaryTargetTemperature ) * PRIMARY_HEATER_DUTY_CYCLE_PER_TEMPERATURE_C;
-                }
-            }
-
-            heaterEstGain = MAX( heaterEstGain, HEATERS_MIN_EST_GAIN );
-            heaterEstGain = MIN( heaterEstGain, HEATERS_MAX_EST_GAIN );
-            heatersStatus[ DG_PRIMARY_HEATER ].heaterEstGain = heaterEstGain;
-        }
-    }
-
-    dutyCycle *= heatersStatus[ DG_PRIMARY_HEATER ].heaterEstGain;
-    dutyCycle  = MIN( dutyCycle, HEATERS_MAX_DUTY_CYCLE );
-    dutyCycle  = MAX( dutyCycle, HEATERS_MIN_DUTY_CYCLE );
-
     return dutyCycle;
 }
 
@@ -808,9 +817,9 @@
         data.primaryEfficiency      = heatersStatus[ DG_PRIMARY_HEATER ].heaterEstGain * 100;
         data.primaryCalcTargetTemp  = heatersStatus[ DG_PRIMARY_HEATER ].calculatedTemperature;
         data.trimmerCalcCurrentTemp = heatersStatus[ DG_TRIMMER_HEATER ].calculatedTemperature;
-        broadcastData( MSG_ID_DG_HEATERS_DATA, COMM_BUFFER_OUT_CAN_DG_BROADCAST, (U08*)&data, sizeof( HEATERS_DATA_T ) );
-
         dataPublicationTimerCounter = 0;
+
+        broadcastData( MSG_ID_DG_HEATERS_DATA, COMM_BUFFER_OUT_CAN_DG_BROADCAST, (U08*)&data, sizeof( HEATERS_DATA_T ) );
     }
 }