Index: firmware/App/Controllers/Heaters.c
===================================================================
diff -u -r0c14c0f5a3a3c6e47a0e4ac5fcb8e8b23e766905 -r4053bb88fd83acda93e7b6a27480658e82814609
--- firmware/App/Controllers/Heaters.c	(.../Heaters.c)	(revision 0c14c0f5a3a3c6e47a0e4ac5fcb8e8b23e766905)
+++ firmware/App/Controllers/Heaters.c	(.../Heaters.c)	(revision 4053bb88fd83acda93e7b6a27480658e82814609)
@@ -8,7 +8,7 @@
 * @file    Heaters.c
 *
 * @author  (last)      Vinayakam Mani
-* @date    (last)      11-Feb-2026
+* @date    (last)      03-Mar-2026
 *
 * @author  (original)  Vinayakam Mani
 * @date    (original)  11-Oct-2024
@@ -18,6 +18,7 @@
 #include <math.h> // Used for mathematical operations
 
 #include "Conductivity.h"
+#include "ConductivityTeensy.h"
 #include "FpgaDD.h"
 #include "Heaters.h"
 #include "Level.h"
@@ -55,6 +56,9 @@
 #define ADJ_DELTA_TEMP_STEP                                        2.0F                                                            ///< Adjust target temperature based on D28 feedback per cycle.
 #define MAX_ADJ_DELTA_TEMP_C                                       7.0F                                                            ///< Maximum adjusted delta temperature to add/remove from calculated target temperature
 #define D5_HEAT_TX_INIT_FEED_FORWARD                               0.0F                                                            ///< Initial Feed forward term for heater control
+//#define D5_HEAT_TX_P_COEFFICIENT                                   0.050F                                                          ///< P Term for AC primary heater control during treatment mode.
+//#define D5_HEAT_TX_I_COEFFICIENT                                   0.015F                                                          ///< I Term for AC primary heater control during treatment mode.
+
 #define D5_HEAT_TX_P_COEFFICIENT                                   0.035F                                                          ///< P Term for AC primary heater control during treatment mode.
 #define D5_HEAT_TX_I_COEFFICIENT                                   0.004F                                                          ///< I Term for AC primary heater control during treatment mode.
 
@@ -77,6 +81,7 @@
 #define D5_HEATER_PWM_ADJ_SLOPE_FACTOR                             0.00005F                                                        ///< AC heater close loop PWM adjustment slope factor
 #define D5_HEATER_PWM_INTERCEPT_FACTOR                             0.0055F                                                         ///< AC heater close loop PWM adjustment intercept factor
 
+//#define D5_HEAT_CONTROL_INTERVAL_MS                                30000                                                           /// Primary heater control interval in milli seconds
 #define D5_HEAT_CONTROL_INTERVAL_MS                                3000                                                            /// Primary heater control interval in milli seconds
 #define D5_HEAT_CONTROL_INTERVAL_COUNT                             ( D5_HEAT_CONTROL_INTERVAL_MS / TASK_GENERAL_INTERVAL )         ///< Primary heater control interval count.
 #define D45_HEAT_CONTROL_INTERVAL_MS                               ( 1 * MS_PER_SECOND )                                           ///< Trimmer heater control interval in milli seconds
@@ -424,11 +429,17 @@
     if ( ++primaryTargetTempAdjCounter >= d5OuterLoopControlInterval )
     {
         F32 targetTempfromTD            = getTDTargetDialysateTemperature();
-#ifdef __TEENSY_CONDUCTIVITY_DRIVER__
-        F32 measuredTempAtDialyzer      = getTeensyConductivityTemperatureValue( D27_COND );
-#else
-        F32 measuredTempAtDialyzer      = getConductivityTemperatureValue( D27_COND );
-#endif
+        F32 measuredTempAtDialyzer      = 0.0F;
+
+        if ( getTestConfigStatus( TEST_CONFIG_DD_FP_ENABLE_BETA_2_0_HW ) != TRUE )
+        {
+            measuredTempAtDialyzer      = getTeensyConductivityTemperatureValue( D27_COND );
+        }
+        else
+        {
+            measuredTempAtDialyzer      = getConductivityTemperature( D27_COND );
+        }
+
         F32 calcTargetTemp              = getHeaterTargetTemperature( D5_HEAT );
         F32 dialysateFlowrate           = getTDDialysateFlowrate();
         F32 deltaTempC                  = targetTempfromTD - measuredTempAtDialyzer;
@@ -923,6 +934,9 @@
         F32 control = getHeaterControl( heater );
         F32 period  = getHeaterPWMPeriod ( heater );
 
+        //Make sure PWM control is enabled all time
+        setFPGAD5HeaterPWMEnableControl( TRUE );
+
         if ( D5_HEAT == heater )
         {
             //Convert duty cycle into LowState and multiply by period