diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp
index f4c577271a..42fb3a50bc 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.cpp
+++ b/libraries/AP_IOMCU/AP_IOMCU.cpp
@@ -121,8 +121,6 @@ AP_IOMCU::AP_IOMCU(AP_HAL::UARTDriver &_uart) :
  */
 void AP_IOMCU::init(void)
 {
-    //upload_fw(fw_name);
-
     // uart runs at 1.5MBit
     uart.begin(1500*1000, 256, 256);
     uart.set_blocking_writes(false);
@@ -131,7 +129,10 @@ void AP_IOMCU::init(void)
     // check IO firmware CRC
     hal.scheduler->delay(2000);
     
-    check_crc();
+    AP_BoardConfig *boardconfig = AP_BoardConfig::get_instance();
+    if (!boardconfig || boardconfig->io_enabled() == 1) {
+        check_crc();
+    }
         
     thread_ctx = chThdCreateFromHeap(NULL,
                                      THD_WORKING_AREA_SIZE(1024),
@@ -681,9 +682,11 @@ bool AP_IOMCU::check_crc(void)
 
     const uint16_t magic = REBOOT_BL_MAGIC;
     write_registers(PAGE_SETUP, PAGE_REG_SETUP_REBOOT_BL, 1, &magic);
-    hal.scheduler->delay(100);
-    upload_fw(fw_name);
 
+    if (!upload_fw(fw_name)) {
+        AP_BoardConfig::sensor_config_error("Failed to update IO firmware");
+    }
+    
     return false;
 }