diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
index 7c7050d9c7..515e507295 100644
--- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
+++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
@@ -171,8 +171,6 @@ void AP_IOMCU_FW::init()
         has_heater = true;
     }
 
-    //Set Heater PWM Polarity, 0 for Active Low and 1 for Active High
-    heater_pwm_polarity = !palReadLine(HAL_GPIO_PIN_HEATER);
     //Set Heater pin mode
     if (heater_pwm_polarity) {
         palSetLineMode(HAL_GPIO_PIN_HEATER, PAL_MODE_OUTPUT_PUSHPULL);
diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.h b/libraries/AP_IOMCU/iofirmware/iofirmware.h
index febd730330..68888746d4 100644
--- a/libraries/AP_IOMCU/iofirmware/iofirmware.h
+++ b/libraries/AP_IOMCU/iofirmware/iofirmware.h
@@ -126,7 +126,7 @@ private:
     bool update_default_rate;
     bool update_rcout_freq;
     bool has_heater;
-    bool heater_pwm_polarity;
+    const bool heater_pwm_polarity = IOMCU_IMU_HEATER_POLARITY;
     uint32_t last_blue_led_ms;
     uint32_t safety_update_ms;
     uint32_t safety_button_counter;
diff --git a/libraries/AP_IOMCU/iofirmware/wscript b/libraries/AP_IOMCU/iofirmware/wscript
index aaf201e467..7e051d72cb 100644
--- a/libraries/AP_IOMCU/iofirmware/wscript
+++ b/libraries/AP_IOMCU/iofirmware/wscript
@@ -18,8 +18,17 @@ def build(bld):
             'libraries/AP_HAL_ChibiOS/Storage.cpp'
         ]
     )
+
     bld.ap_program(
-        program_name='iofirmware',
+        program_name='iofirmware_lowpolh',
         use='iofirmware_libs',
         program_groups=['bin','iofirmware'],
+        defines=['IOMCU_IMU_HEATER_POLARITY=0']
+    )
+
+    bld.ap_program(
+        program_name='iofirmware_highpolh',
+        use='iofirmware_libs',
+        program_groups=['bin','iofirmware'],
+        defines=['IOMCU_IMU_HEATER_POLARITY=1']
     )