Index: firmware/App/Controllers/Heaters.c
===================================================================
diff -u -rbb23492d17539eb0d1ec14c4f2eb25e2756a6ae1 -r6b13bb16465fb2eee853288fe0d109223d978cdd
--- firmware/App/Controllers/Heaters.c	(.../Heaters.c)	(revision bb23492d17539eb0d1ec14c4f2eb25e2756a6ae1)
+++ firmware/App/Controllers/Heaters.c	(.../Heaters.c)	(revision 6b13bb16465fb2eee853288fe0d109223d978cdd)
@@ -8,7 +8,7 @@
 * @file    Heaters.c
 *
 * @author  (last)      Dara Navaei
-* @date    (last)      23-May-2022
+* @date    (last)      04-Aug-2022
 *
 * @author  (original)  Dara Navaei
 * @date    (original)  23-Apr-2020
@@ -646,8 +646,10 @@
         F32 targetTemperature          = heatersStatus[ DG_TRIMMER_HEATER ].targetTemp;
         F32 targetFlowLPM              = heatersStatus[ DG_TRIMMER_HEATER ].targetFlow;
         F32 dutyCycle                  = calculateTrimmerHeaterDutyCycle( targetTemperature, outletRedundantTemperature, targetFlowLPM, TRUE );
+
         trimmerHeaterControlCounter    = 0;
-        setHeaterDutyCycle( DG_TRIMMER_HEATER, ( heatersStatus[ DG_TRIMMER_HEATER ].dutycycle + dutyCycle ) );
+
+        setHeaterDutyCycle( DG_TRIMMER_HEATER, dutyCycle );
     }
 
     return state;
@@ -697,6 +699,39 @@
  *************************************************************************/
 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 heaterEfficiency = heatersStatus[ DG_PRIMARY_HEATER ].heaterEfficiency;
+
+#ifndef _RELEASE_
+    if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_HEATERS_EFFICIENCY ) != SW_CONFIG_ENABLE_VALUE )
+#endif
+    {
+        if ( TRUE == checkEfficiency )
+        {
+            F32 lastFillTemperature      = getLastFillTemperature();
+            F32 primaryTargetTemperature = heatersStatus[ DG_PRIMARY_HEATER ].targetTemp;
+
+            // If the last fill temperature > target temperature, it means the primary heater overshot the duty cycle
+            // so with its efficiency is toned down for the next fill cycle
+            // If the heater under-shoots the duty cycle, the efficiency increases the duty cycle for the next fill cycle
+            if ( lastFillTemperature - primaryTargetTemperature < 0.0F )
+            {
+                heaterEfficiency -= ( lastFillTemperature - primaryTargetTemperature ) * PRIMARY_HEATER_DUTY_CYCLE_PER_TEMPERATURE_C;
+            }
+            else
+            {
+                heaterEfficiency += ( primaryTargetTemperature - lastFillTemperature ) * PRIMARY_HEATER_DUTY_CYCLE_PER_TEMPERATURE_C;
+            }
+
+            // If the efficiency is less than 0, set it to 0 but if the efficiency is greater than 100, it is not enforced
+            // to be 100 since the duty cycle range check will cap the duty cycle to 100 anyways.
+            heaterEfficiency = MAX( heaterEfficiency, 0.0 );
+
+            // Update the heaters efficiency
+            heatersStatus[ DG_PRIMARY_HEATER ].heaterEfficiency = heaterEfficiency;
+        }
+    }
+
     // 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 );
@@ -826,6 +861,7 @@
     if ( ++dataPublicationTimerCounter >= getU32OverrideValue( &heatersDataPublishInterval ) )
     {
         HEATERS_DATA_T data;
+
         data.mainPrimayHeaterDC     = heatersStatus[ DG_PRIMARY_HEATER ].dutycycle * 100.0;
         // The duty cycle of the primary heater is divided into 2 parts and is applied to main
         // and small primary heaters. So they are always the same.