diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
index f2db63ba94..6a3466e855 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
@@ -197,6 +197,16 @@ void RCOutput::rcout_thread()
 
         // process any pending RC output requests
         timer_tick(time_out_us);
+#if RCOU_DSHOT_TIMING_DEBUG
+        static bool output_masks = true;
+        if (AP_HAL::millis() > 5000 && output_masks) {
+            output_masks = false;
+            hal.console->printf("bdmask 0x%x, en_mask 0x%lx, 3dmask 0x%x:\n", _bdshot.mask, en_mask, _reversible_mask);
+            for (auto &group : pwm_group_list) {
+                hal.console->printf("  timer %u: ch_mask 0x%x, en_mask 0x%x\n", group.timer_id, group.ch_mask, group.en_mask);
+            }
+        }
+#endif
     }
 }
 
@@ -1328,8 +1338,9 @@ void RCOutput::dshot_send(pwm_group &group, uint32_t time_out_us)
     // assume that we won't be able to get the input capture lock
     group.bdshot.enabled = false;
 
+    uint16_t active_channels = group.ch_mask & group.en_mask;
     // now grab the input capture lock if we are able, we can only enable bi-dir on a group basis
-    if (((_bdshot.mask & group.ch_mask) == group.ch_mask) && group.has_ic()) {
+    if (((_bdshot.mask & active_channels) == active_channels) && group.has_ic()) {
         if (group.has_shared_ic_up_dma()) {
             // no locking required
             group.bdshot.enabled = true;
@@ -1388,18 +1399,19 @@ void RCOutput::dshot_send(pwm_group &group, uint32_t time_out_us)
 #endif
             _bdshot.erpm[chan] = erpm;
 #endif
+            if (safety_on && !(safety_mask & (1U<<(chan+chan_offset)))) {
+                // safety is on, don't output anything
+                continue;
+            }
+
             uint16_t pwm = period[chan];
 
-            if (safety_on && !(safety_mask & (1U<<(chan+chan_offset)))) {
-                // safety is on, overwride pwm
-                pwm = 0;
+            if (pwm == 0) {
+                // no pwm, don't output anything
+                continue;
             }
 
             const uint16_t chan_mask = (1U<<chan);
-            if (pwm == 0) {
-                // no output
-                continue;
-            }
 
             pwm = constrain_int16(pwm, 1000, 2000);
             uint16_t value = MIN(2 * (pwm - 1000), 1999);
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp
index 6b970a1335..73a99a4d40 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp
@@ -62,18 +62,12 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group)
         return true;
     }
 
-    bool set_curr_chan = false;
-
+    // allocate input capture DMA handles
     for (uint8_t i = 0; i < 4; i++) {
         if (!group.is_chan_enabled(i) ||
             !group.dma_ch[i].have_dma || !(_bdshot.mask & (1 << group.chan[i]))) {
             continue;
         }
-        // make sure we don't start on a disabled channel
-        if (!set_curr_chan) {
-            group.bdshot.curr_telem_chan = i;
-            set_curr_chan = true;
-        }
         pwmmode_t mode = group.pwm_cfg.channels[i].mode;
         if (mode == PWM_COMPLEMENTARY_OUTPUT_ACTIVE_LOW ||
             mode == PWM_COMPLEMENTARY_OUTPUT_ACTIVE_HIGH) {
@@ -99,6 +93,15 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group)
     // We might need to do sharing of timers for telemetry feedback
     // due to lack of available DMA channels
     for (uint8_t i = 0; i < 4; i++) {
+        // we must pull all the allocated channels high to prevent them going low
+        // when the pwm peripheral is stopped
+        if (group.chan[i] != CHAN_DISABLED && _bdshot.mask & group.ch_mask) {
+            // bi-directional dshot requires less than MID2 speed and PUSHPULL in order to avoid noise on the line
+            // when switching from output to input
+            palSetLineMode(group.pal_lines[i], PAL_MODE_ALTERNATE(group.alt_functions[i])
+                | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_PUPDR_PULLUP | PAL_STM32_OSPEED_MID1);
+        }
+
         if (!group.is_chan_enabled(i) || !(_bdshot.mask & (1 << group.chan[i]))) {
             continue;
         }
@@ -137,10 +140,14 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group)
             group.bdshot.telem_tim_ch[i] = curr_chan;
             group.dma_ch[i] = group.dma_ch[curr_chan];
         }
-        // bi-directional dshot requires less than MID2 speed and PUSHPULL in order to avoid noise on the line
-        // when switching from output to input
-        palSetLineMode(group.pal_lines[i], PAL_MODE_ALTERNATE(group.alt_functions[i])
-            | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_PUPDR_PULLUP | PAL_STM32_OSPEED_MID1);
+    }
+
+    // now allocate the starting channel
+    for (uint8_t i = 0; i < 4; i++) {
+        if (group.chan[i] != CHAN_DISABLED && group.bdshot.ic_dma_handle[i] != nullptr) {
+            group.bdshot.curr_telem_chan = i;
+            break;
+        }
     }
 
     return true;
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp
index 3fddedc1a4..9183a4ca2f 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp
@@ -38,7 +38,9 @@ bool RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t cha
         return false;
     }
 
+#ifdef HAL_GPIO_LINE_GPIO81
     TOGGLE_PIN_DEBUG(81);
+#endif
     // first make sure we have the DMA channel before anything else
 
     osalDbgAssert(!group.dma_handle->is_locked(), "DMA handle is already locked");
@@ -48,9 +50,10 @@ bool RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t cha
     group.dshot_waiter = rcout_thread_ctx;
     bool bdshot_telem = false;
 #ifdef HAL_WITH_BIDIR_DSHOT
+    uint16_t active_channels = group.ch_mask & group.en_mask;
     // no need to get the input capture lock
     group.bdshot.enabled = false;
-    if ((_bdshot.mask & group.ch_mask) == group.ch_mask) {
+    if ((_bdshot.mask & active_channels) == active_channels) {
         bdshot_telem = true;
         // it's not clear why this is required, but without it we get no output
         if (group.pwm_started) {
@@ -68,9 +71,13 @@ bool RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t cha
     const uint16_t packet = create_dshot_packet(command, true, bdshot_telem);
 
     for (uint8_t i = 0; i < 4; i++) {
-        if (group.chan[i] == chan || (chan == RCOutput::ALL_CHANNELS && group.is_chan_enabled(i))) {
+        if (!group.is_chan_enabled(i)) {
+            continue;
+        }
+
+        if (group.chan[i] == chan || chan == RCOutput::ALL_CHANNELS) {
             fill_DMA_buffer_dshot(group.dma_buffer + i, 4, packet, group.bit_width_mul);
-        } else if (group.is_chan_enabled(i)) {
+        } else {
             fill_DMA_buffer_dshot(group.dma_buffer + i, 4, zero_packet, group.bit_width_mul);
         }
     }
@@ -78,7 +85,9 @@ bool RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t cha
     chEvtGetAndClearEvents(group.dshot_event_mask);
     // start sending the pulses out
     send_pulses_DMAR(group, DSHOT_BUFFER_LENGTH);
+#ifdef HAL_GPIO_LINE_GPIO81
     TOGGLE_PIN_DEBUG(81);
+#endif
 
     return true;
 }