diff --git a/libraries/AP_HAL_ChibiOS/RCInput.cpp b/libraries/AP_HAL_ChibiOS/RCInput.cpp
index 150e313476..640de28e00 100644
--- a/libraries/AP_HAL_ChibiOS/RCInput.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCInput.cpp
@@ -20,6 +20,7 @@
 #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
 
 #if HAL_WITH_IO_MCU
+#include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_IOMCU/AP_IOMCU.h>
 extern AP_IOMCU iomcu;
 #endif
@@ -185,7 +186,8 @@ void ChibiRCInput::_timer_tick(void)
 
 #if HAL_WITH_IO_MCU
     chMtxLock(&rcin_mutex);
-    if (iomcu.check_rcinput(last_iomcu_us, _num_channels, _rc_values, RC_INPUT_MAX_CHANNELS)) {
+    if (AP_BoardConfig::io_enabled() &&
+        iomcu.check_rcinput(last_iomcu_us, _num_channels, _rc_values, RC_INPUT_MAX_CHANNELS)) {
         _rcin_timestamp_last_signal = last_iomcu_us;
     }
     chMtxUnlock(&rcin_mutex);
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
index 82b4a2d7ba..c33caecbc5 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
@@ -16,6 +16,7 @@
  */
 #include "RCOutput.h"
 #include <AP_Math/AP_Math.h>
+#include <AP_BoardConfig/AP_BoardConfig.h>
 
 using namespace ChibiOS;
 
@@ -64,10 +65,11 @@ void ChibiRCOutput::init()
         pwmStart(pwm_group_list[i].pwm_drv, &pwm_group_list[i].pwm_cfg);
     }
 #if HAL_WITH_IO_MCU
-    iomcu.init();
-
-    // with IOMCU the local channels start at 8
-    chan_offset = 8;
+    if (AP_BoardConfig::io_enabled()) {
+        iomcu.init();
+        // with IOMCU the local channels start at 8
+        chan_offset = 8;
+    }
 #endif
 }
 
@@ -83,7 +85,9 @@ void ChibiRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
     }
 
 #if HAL_WITH_IO_MCU
-    iomcu.set_freq(chmask, freq_hz);
+    if (AP_BoardConfig::io_enabled()) {
+        iomcu.set_freq(chmask, freq_hz);
+    }
 #endif
 
     chmask >>= chan_offset;
@@ -169,7 +173,9 @@ void ChibiRCOutput::write(uint8_t chan, uint16_t period_us)
     
 #if HAL_WITH_IO_MCU
     // handle IO MCU channels
-    iomcu.write_channel(chan, period_us);
+    if (AP_BoardConfig::io_enabled()) {
+        iomcu.write_channel(chan, period_us);
+    }
 #endif
     if (chan < chan_offset) {
         return;
@@ -273,10 +279,11 @@ void ChibiRCOutput::set_output_mode(enum output_mode mode)
 bool ChibiRCOutput::force_safety_on(void)
 {
 #if HAL_WITH_IO_MCU
-    return iomcu.force_safety_on();
-#else
-    return false;
+    if (AP_BoardConfig::io_enabled()) {
+        return iomcu.force_safety_on();
+    }
 #endif
+    return false;
 }
 
 /*
@@ -285,7 +292,9 @@ bool ChibiRCOutput::force_safety_on(void)
 void ChibiRCOutput::force_safety_off(void)
 {
 #if HAL_WITH_IO_MCU
-    iomcu.force_safety_off();
+    if (AP_BoardConfig::io_enabled()) {
+        iomcu.force_safety_off();
+    }
 #endif
 }
 
@@ -296,7 +305,9 @@ void ChibiRCOutput::cork(void)
 {
     corked = true;
 #if HAL_WITH_IO_MCU
-    iomcu.cork();
+    if (AP_BoardConfig::io_enabled()) {
+        iomcu.cork();
+    }
 #endif
 }
 
@@ -308,7 +319,9 @@ void ChibiRCOutput::push(void)
     corked = false;
     push_local();
 #if HAL_WITH_IO_MCU
-    iomcu.push();
+    if (AP_BoardConfig::io_enabled()) {
+        iomcu.push();
+    }
 #endif
 }
 
@@ -318,8 +331,9 @@ void ChibiRCOutput::push(void)
 bool ChibiRCOutput::enable_px4io_sbus_out(uint16_t rate_hz)
 {
 #if HAL_WITH_IO_MCU
-    return iomcu.enable_sbus_out(rate_hz);
-#else
-    return false;
+    if (AP_BoardConfig::io_enabled()) {
+        return iomcu.enable_sbus_out(rate_hz);
+    }
 #endif
+    return false;
 }
diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp
index 0e132e14f1..b3746daeeb 100644
--- a/libraries/AP_HAL_ChibiOS/Util.cpp
+++ b/libraries/AP_HAL_ChibiOS/Util.cpp
@@ -22,6 +22,7 @@
 #include <chheap.h>
 
 #if HAL_WITH_IO_MCU
+#include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_IOMCU/AP_IOMCU.h>
 extern AP_IOMCU iomcu;
 #endif
@@ -48,16 +49,17 @@ uint32_t ChibiUtil::available_memory(void)
 ChibiUtil::safety_state ChibiUtil::safety_switch_state(void)
 {
 #if HAL_WITH_IO_MCU
-    return iomcu.get_safety_switch_state();
-#else
-    return SAFETY_NONE;
+    if (AP_BoardConfig::io_enabled()) {
+        return iomcu.get_safety_switch_state();
+    }
 #endif
+    return SAFETY_NONE;
 }
 
 void ChibiUtil::set_imu_temp(float current)
 {
 #if HAL_WITH_IO_MCU && HAL_HAVE_IMU_HEATER
-    if (!heater.target || *heater.target == -1) {
+    if (!heater.target || *heater.target == -1 || !AP_BoardConfig::io_enabled()) {
         return;
     }