diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp
index 2d92fd331b..cbb15e8c30 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.cpp
+++ b/libraries/AP_IOMCU/AP_IOMCU.cpp
@@ -38,6 +38,7 @@ enum ioevents {
     IOEVENT_SET_SAFETY_MASK,
     IOEVENT_MIXING,
     IOEVENT_GPIO,
+    IOEVENT_SET_OUTPUT_MODE
 };
 
 // max number of consecutve protocol failures we accept before raising
@@ -230,6 +231,13 @@ void AP_IOMCU::thread_main(void)
         }
         mask &= ~EVENT_MASK(IOEVENT_SET_BRUSHED_ON);
 
+        if (mask & EVENT_MASK(IOEVENT_SET_OUTPUT_MODE)) {
+            if (!write_registers(PAGE_SETUP, PAGE_REG_SETUP_OUTPUT_MODE, sizeof(mode_out)/2, (const uint16_t *)&mode_out)) {
+                event_failed(IOEVENT_SET_OUTPUT_MODE);
+                continue;
+            }
+        }
+
         if (mask & EVENT_MASK(IOEVENT_SET_SAFETY_MASK)) {
             if (!write_register(PAGE_SETUP, PAGE_REG_SETUP_IGNORE_SAFETY, pwm_out.safety_mask)) {
                 event_failed(mask);
@@ -405,7 +413,7 @@ void AP_IOMCU::write_log()
         static uint32_t last_io_print;
         if (now - last_io_print >= 5000) {
             last_io_print = now;
-            debug("t=%u num=%u mem=%u terr=%u nerr=%u crc=%u opcode=%u rd=%u wr=%u ur=%u ndel=%u\n",
+            debug("t=%lu num=%lu mem=%u terr=%lu nerr=%lu crc=%u opcode=%u rd=%u wr=%u ur=%u ndel=%lu\n",
                   now,
                   reg_status.total_pkts,
                   reg_status.freemem,
@@ -508,7 +516,7 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1
 
     // wait for the expected number of reply bytes or timeout
     if (!uart.wait_timeout(count*2+4, 10)) {
-        debug("t=%u timeout read page=%u offset=%u count=%u\n",
+        debug("t=%lu timeout read page=%u offset=%u count=%u\n",
               AP_HAL::millis(), page, offset, count);
         protocol_fail_count++;
         return false;
@@ -517,12 +525,12 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1
     uint8_t *b = (uint8_t *)&pkt;
     uint8_t n = uart.available();
     if (n < offsetof(struct IOPacket, regs)) {
-        debug("t=%u small pkt %u\n", AP_HAL::millis(), n);
+        debug("t=%lu small pkt %u\n", AP_HAL::millis(), n);
         protocol_fail_count++;
         return false;
     }
     if (pkt.get_size() != n) {
-        debug("t=%u bad len %u %u\n", AP_HAL::millis(), n, pkt.get_size());
+        debug("t=%lu bad len %u %u\n", AP_HAL::millis(), n, pkt.get_size());
         protocol_fail_count++;
         return false;
     }
@@ -532,7 +540,7 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1
     pkt.crc = 0;
     uint8_t expected_crc = crc_crc8((const uint8_t *)&pkt, pkt.get_size());
     if (got_crc != expected_crc) {
-        debug("t=%u bad crc %02x should be %02x n=%u %u/%u/%u\n",
+        debug("t=%lu bad crc %02x should be %02x n=%u %u/%u/%u\n",
               AP_HAL::millis(), got_crc, expected_crc,
               n, page, offset, count);
         protocol_fail_count++;
@@ -789,6 +797,14 @@ void AP_IOMCU::set_brushed_mode(void)
     rate.brushed_enabled = true;
 }
 
+// set output mode
+void AP_IOMCU::set_output_mode(uint16_t mask, uint16_t mode)
+{
+    mode_out.mask = mask;
+    mode_out.mode = mode;
+    trigger_event(IOEVENT_SET_OUTPUT_MODE);
+}
+
 // handling of BRD_SAFETYOPTION parameter
 void AP_IOMCU::update_safety_options(void)
 {
@@ -837,9 +853,13 @@ bool AP_IOMCU::check_crc(void)
     // flash size minus 4k bootloader
 	const uint32_t flash_size = 0x10000 - 0x1000;
 
-    fw = AP_ROMFS::find_decompress(fw_name, fw_size);
+    if (!AP_BoardConfig::io_dshot()) {
+        fw = AP_ROMFS::find_decompress(fw_name, fw_size);
+    } else {
+        fw = AP_ROMFS::find_decompress(dshot_fw_name, fw_size);
+    }
     if (!fw) {
-        DEV_PRINTF("failed to find %s\n", fw_name);
+        DEV_PRINTF("failed to find %s\n", AP_BoardConfig::io_dshot()?dshot_fw_name:fw_name);
         return false;
     }
     uint32_t crc = crc32_small(0, fw, fw_size);
diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h
index df97297017..77ec6295c9 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.h
+++ b/libraries/AP_IOMCU/AP_IOMCU.h
@@ -98,6 +98,9 @@ public:
     // set to brushed mode
     void set_brushed_mode(void);
 
+    // set output mode
+    void set_output_mode(uint16_t mask, uint16_t mode);
+
     // check if IO is healthy
     bool healthy(void);
 
@@ -222,6 +225,11 @@ private:
     } rate;
 
     struct page_GPIO GPIO;
+    // output mode values
+    struct {
+        uint16_t mask;
+        uint16_t mode;
+    } mode_out;
 
     // IMU heater duty cycle
     uint8_t heater_duty_cycle;
@@ -248,6 +256,7 @@ private:
 
     // firmware upload
     const char *fw_name = "io_firmware.bin";
+    const char *dshot_fw_name = "io_firmware_dshot.bin";
     const uint8_t *fw;
     uint32_t fw_size;
 
diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
index 70a6aa6aa8..8c36665649 100644
--- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
+++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
@@ -541,6 +541,11 @@ bool AP_IOMCU_FW::handle_code_write()
             }
             break;
 
+        case PAGE_REG_SETUP_OUTPUT_MODE:
+            mode_out.mask = rx_io_packet.regs[0];
+            mode_out.mode = rx_io_packet.regs[1];
+            break;
+
         case PAGE_REG_SETUP_HEATER_DUTY_CYCLE:
             reg_setup.heater_duty_cycle = rx_io_packet.regs[0];
             last_heater_ms = last_ms;
@@ -760,24 +765,27 @@ void AP_IOMCU_FW::safety_update(void)
  */
 void AP_IOMCU_FW::rcout_mode_update(void)
 {
-    bool use_oneshot = (reg_setup.features & P_SETUP_FEATURES_ONESHOT) != 0;
+    bool use_dshot = mode_out.mode >= AP_HAL::RCOutput::MODE_PWM_DSHOT150 && mode_out.mode <= AP_HAL::RCOutput::MODE_PWM_DSHOT600;
+    if (use_dshot && !dshot_enabled) {
+        dshot_enabled = true;
+        hal.rcout->set_output_mode(mode_out.mask, (AP_HAL::RCOutput::output_mode)mode_out.mode);
+    }
+    bool use_oneshot = mode_out.mode == AP_HAL::RCOutput::MODE_PWM_ONESHOT;
     if (use_oneshot && !oneshot_enabled) {
         oneshot_enabled = true;
-        hal.rcout->set_output_mode(reg_setup.pwm_rates, AP_HAL::RCOutput::MODE_PWM_ONESHOT);
+        hal.rcout->set_output_mode(mode_out.mask, AP_HAL::RCOutput::MODE_PWM_ONESHOT);
     }
-    bool use_brushed = (reg_setup.features & P_SETUP_FEATURES_BRUSHED) != 0;
+    bool use_brushed = mode_out.mode == AP_HAL::RCOutput::MODE_PWM_BRUSHED;
     if (use_brushed && !brushed_enabled) {
         brushed_enabled = true;
-        if (reg_setup.pwm_rates == 0) {
-            // default to 2kHz for all channels for brushed output
-            reg_setup.pwm_rates = 0xFF;
-            reg_setup.pwm_altrate = 2000;
-            hal.rcout->set_freq(reg_setup.pwm_rates, reg_setup.pwm_altrate);
-        }
+        // default to 2kHz for all channels for brushed output
+        hal.rcout->set_freq(mode_out.mask, 2000);
         hal.rcout->set_esc_scaling(1000, 2000);
-        hal.rcout->set_output_mode(reg_setup.pwm_rates, AP_HAL::RCOutput::MODE_PWM_BRUSHED);
-        hal.rcout->set_freq(reg_setup.pwm_rates, reg_setup.pwm_altrate);
+        hal.rcout->set_output_mode(mode_out.mask, AP_HAL::RCOutput::MODE_PWM_BRUSHED);
+        hal.rcout->set_freq(mode_out.mask, reg_setup.pwm_altrate);
     }
+    mode_out.mask = 0;
+    mode_out.mode = 0;
 }
 
 /*
diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.h b/libraries/AP_IOMCU/iofirmware/iofirmware.h
index 8aef68165d..64d661e269 100644
--- a/libraries/AP_IOMCU/iofirmware/iofirmware.h
+++ b/libraries/AP_IOMCU/iofirmware/iofirmware.h
@@ -102,6 +102,12 @@ public:
         uint16_t sbus_rate_hz;
     } rate;
 
+    // output mode values
+     struct {
+         uint16_t mask;
+         uint16_t mode;
+     } mode_out;
+
     // MIXER values
     struct page_mixing mixing;
 
@@ -130,6 +136,7 @@ public:
     uint32_t safety_button_counter;
     uint8_t led_counter;
     uint32_t last_loop_ms;
+    bool dshot_enabled;
     bool oneshot_enabled;
     bool brushed_enabled;
     thread_t *thread_ctx;
diff --git a/libraries/AP_IOMCU/iofirmware/ioprotocol.h b/libraries/AP_IOMCU/iofirmware/ioprotocol.h
index 85d4b9c4bc..4f1f11070a 100644
--- a/libraries/AP_IOMCU/iofirmware/ioprotocol.h
+++ b/libraries/AP_IOMCU/iofirmware/ioprotocol.h
@@ -78,6 +78,7 @@ enum iopage {
 #define PAGE_REG_SETUP_PWM_RATE_MASK 2
 #define PAGE_REG_SETUP_DEFAULTRATE   3
 #define PAGE_REG_SETUP_ALTRATE       4
+#define PAGE_REG_SETUP_OUTPUT_MODE   5
 #define PAGE_REG_SETUP_REBOOT_BL    10
 #define PAGE_REG_SETUP_CRC			11
 #define PAGE_REG_SETUP_SBUS_RATE    19