Index: firmware/App/Controllers/Heaters.c
===================================================================
diff -u -re88c2cfc5d6b693d8c85cbb5bc81836b04ad3b2b -r1b4447ba6fdc3d90a9013571b5710a52295c43d3
--- firmware/App/Controllers/Heaters.c	(.../Heaters.c)	(revision e88c2cfc5d6b693d8c85cbb5bc81836b04ad3b2b)
+++ firmware/App/Controllers/Heaters.c	(.../Heaters.c)	(revision 1b4447ba6fdc3d90a9013571b5710a52295c43d3)
@@ -48,7 +48,7 @@
 
 #define HEATERS_MAX_DUTY_CYCLE                                     1.00F                                                           ///< Heaters max duty cycle (100%).
 #define HEATERS_MIN_DUTY_CYCLE                                     0.00F                                                           ///< Heaters minimum duty cycle (0.00%).
-#define HEATERS_MIN_HEAT_DISINFECT_DUTY_CYCLE                      0.6F  // TODO remove                                                          ///< Heaters minimum duty cycle during heat disinfect.
+#define HEATERS_DISINFECT_TEMPERATURE_DRIFT_C                      1.0F                                                            ///< Heaters disinfect temperature drift in C.
 #define HEATERS_MIN_EST_GAIN                                       0.2F                                                            ///< Heaters minimum estimation gain.
 #define HEATERS_MAX_EST_GAIN                                       5.0F                                                            ///< Heaters maximum estimation gain.
 #define HEATERS_NEUTRAL_EST_GAIN                                   1.0F                                                            ///< Heaters neutral estimation gain.
@@ -539,29 +539,20 @@
  *************************************************************************/
 static HEATERS_STATE_T handleHeaterStateControlToDisinfectTarget( DG_HEATERS_T heater )
 {
-    HEATERS_STATE_T state       = HEATER_EXEC_STATE_CONTROL_TO_DISINFECT_TARGET;
-    F32 heatDisinfectSensorTemp = getTemperatureValue( TEMPSENSORS_HEAT_DISINFECT );
+    HEATERS_STATE_T state  = HEATER_EXEC_STATE_CONTROL_TO_DISINFECT_TARGET;
+    F32 currentTemperature = getTemperatureValue( TEMPSENSORS_HEAT_DISINFECT );
+    F32 targetTemperature  = heatersStatus[ DG_PRIMARY_HEATER ].targetTemp;
 
-    if ( ++heaterControlCounter > HEATER_CONTROL_CHECK_INTERVAL_COUNT )
+    if ( ( currentTemperature < targetTemperature ) || ( currentTemperature - targetTemperature > HEATERS_DISINFECT_TEMPERATURE_DRIFT_C ) )
     {
-        F32 tempDutyCycle     = 0.0F;
-        F32 targetTemperature = heatersStatus[ DG_TRIMMER_HEATER ].targetTemp;
-        F32 targetFlowLPM     = getTargetROPumpFlowRateLPM();
-        F32 dutyCycle         = calculateTrimmerHeaterDutyCycle( targetTemperature, heatDisinfectSensorTemp, targetFlowLPM, FALSE );
-        heaterControlCounter  = 0;
-        // Trimmer heater duty cycle function checks the minimum to -100%. So if the duty cycle was calculated to be negative it is set to 0 here
-        tempDutyCycle         = MAX( dutyCycle, HEATERS_MIN_DUTY_CYCLE );
-        heatersStatus[ DG_TRIMMER_HEATER ].targetFlow = getTargetROPumpFlowRateLPM();
-
-        setHeaterDutyCycle( DG_TRIMMER_HEATER, tempDutyCycle );
-
-        targetTemperature = heatersStatus[ DG_PRIMARY_HEATER ].targetTemp;
-        targetFlowLPM     = getTargetROPumpFlowRateLPM();
-        dutyCycle         = calculatePrimaryHeaterDutyCycle( targetTemperature, heatDisinfectSensorTemp, targetFlowLPM, FALSE );
-        heatersStatus[ DG_PRIMARY_HEATER ].targetFlow = getTargetROPumpFlowRateLPM();
-
-        setHeaterDutyCycle( DG_PRIMARY_HEATER, dutyCycle );
+        setHeaterDutyCycle( DG_TRIMMER_HEATER, HEATERS_MAX_DUTY_CYCLE );
+        setHeaterDutyCycle( DG_PRIMARY_HEATER, HEATERS_MAX_DUTY_CYCLE );
     }
+    else
+    {
+        setHeaterDutyCycle( DG_TRIMMER_HEATER, HEATERS_MIN_DUTY_CYCLE );
+        setHeaterDutyCycle( DG_PRIMARY_HEATER, HEATERS_MIN_DUTY_CYCLE );
+    }
 
     return state;
 }