AP_HAL_ChibiOS: add support for dshot commands to rcout

add support for dshot beepcodes through tonealarm
add support for dshot reversal and command queue
add support for dshot commands to all channels
correctly manage channel enablement in PWM groups
Correctly send dshot commands when using bi-dir dshot
allow reversible settings to be changed
ChibiOS: allow more than one type of ESC for dshot commands
Only execute reverse/reversible commands on BLHeli
add support for checking active ESCS
mark ESCs active when bdshot telemetry is returned
allow dshot alarm to be disabled
allow priroitized dshot commands

libraries/AP_HAL_ChibiOS/RCOutput.cpp

@@ -40,12 +40,6 @@ extern AP_IOMCU iomcu;
 
 #define RCOU_SERIAL_TIMING_DEBUG 0
 
-#if RCOU_DSHOT_TIMING_DEBUG
-#define TOGGLE_PIN_DEBUG(pin) do { palToggleLine(HAL_GPIO_LINE_GPIO ## pin); } while (0)
-#else
-#define TOGGLE_PIN_DEBUG(pin) do {} while (0)
-#endif
-
 #define TELEM_IC_SAMPLE 16
 
 struct RCOutput::pwm_group RCOutput::pwm_group_list[] = { HAL_PWM_GROUPS };
@@ -58,9 +52,6 @@ static const eventmask_t EVT_PWM_START  = EVENT_MASK(12);
 static const eventmask_t EVT_PWM_SYNTHETIC_SEND  = EVENT_MASK(13);
 static const eventmask_t EVT_PWM_SEND_NEXT  = EVENT_MASK(14);
 
-// marker for a disabled channel
-#define CHAN_DISABLED 255
-
 // #pragma GCC optimize("Og")
 
 /*
@@ -104,7 +95,6 @@ void RCOutput::init()
 #endif
     chMtxObjectInit(&trigger_mutex);
     chVTObjectInit(&_dshot_rate_timer);
-
     // setup default output rate of 50Hz
     set_freq(0xFFFF ^ ((1U<<chan_offset)-1), 50);
 
@@ -510,6 +500,7 @@ void RCOutput::enable_ch(uint8_t chan)
     pwm_group *grp = find_chan(chan, i);
     if (grp) {
         en_mask |= 1U << (chan - chan_offset);
+        grp->ch_mask |= 1U << chan;
     }
 }
 
@@ -520,9 +511,18 @@ void RCOutput::disable_ch(uint8_t chan)
     if (grp) {
         pwmDisableChannel(grp->pwm_drv, i);
         en_mask &= ~(1U<<(chan - chan_offset));
+        grp->ch_mask &= ~(1U << chan);
     }
 }
 
+bool RCOutput::prepare_for_arming()
+{
+    // force all the ESCs to be active, in the future consider returning false
+    // if ESCs are not active that we require
+    _active_escs_mask = (en_mask << chan_offset);
+    return true;
+}
+
 void RCOutput::write(uint8_t chan, uint16_t period_us)
 {
     if (chan >= max_channels) {
@@ -587,7 +587,7 @@ void RCOutput::push_local(void)
         }
         for (uint8_t j = 0; j < 4; j++) {
             uint8_t chan = group.chan[j];
-            if (chan == CHAN_DISABLED) {
+            if (!group.is_chan_enabled(j)) {
                 continue;
             }
             if (outmask & (1UL<<chan)) {
@@ -832,7 +832,7 @@ bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_
     group.pwm_started = true;
 
     for (uint8_t j=0; j<4; j++) {
-        if (group.chan[j] != CHAN_DISABLED) {
+        if (group.is_chan_enabled(j)) {
             pwmEnableChannel(group.pwm_drv, j, 0);
         }
     }
@@ -944,7 +944,7 @@ void RCOutput::set_group_mode(pwm_group &group)
         pwmStart(group.pwm_drv, &group.pwm_cfg);
         group.pwm_started = true;
         for (uint8_t j=0; j<4; j++) {
-            if (group.chan[j] != CHAN_DISABLED) {
+            if (group.is_chan_enabled(j)) {
                 pwmEnableChannel(group.pwm_drv, j, 0);
             }
         }
@@ -1181,15 +1181,27 @@ void RCOutput::timer_tick(uint32_t time_out_us)
 // send dshot for all groups that support it
 void RCOutput::dshot_send_groups(uint32_t time_out_us)
 {
+#ifndef DISABLE_DSHOT
     if (serial_group) {
         return;
     }
-    // actually do a dshot send
+
     for (auto &group : pwm_group_list) {
-        if (group.can_send_dshot_pulse()) {
+        // send a dshot command
+        if (!_dshot_calibrating
+            && !hal.util->get_soft_armed()
+            && is_dshot_protocol(group.current_mode)
+            && group_escs_active(group) // only send when someone is listening
+            && (dshot_command_is_active(group)
+                || (_dshot_command_queue.pop(_dshot_current_command) && dshot_command_is_active(group)))) {
+            dshot_send_command(group, _dshot_current_command.command, _dshot_current_command.chan);
+            _dshot_current_command.cycle--;
+        // actually do a dshot send
+        } else if (group.can_send_dshot_pulse()) {
             dshot_send(group, time_out_us);
         }
     }
+#endif //#ifndef DISABLE_DSHOT
 }
 
 void RCOutput::dshot_send_next_group(void* p)
@@ -1333,6 +1345,7 @@ void RCOutput::dshot_send(pwm_group &group, uint32_t time_out_us)
         uint32_t now = AP_HAL::millis();
         if (bdshot_decode_dshot_telemetry(group, group.bdshot.prev_telem_chan)) {
             _bdshot.erpm_clean_frames[chan]++;
+            _active_escs_mask |= (1<<chan); // we know the ESC is functional at this point
         } else {
             _bdshot.erpm_errors[chan]++;
         }
@@ -1361,7 +1374,7 @@ void RCOutput::dshot_send(pwm_group &group, uint32_t time_out_us)
 
     for (uint8_t i=0; i<4; i++) {
         uint8_t chan = group.chan[i];
-        if (chan != CHAN_DISABLED) {
+        if (group.is_chan_enabled(i)) {
             // retrieve the last erpm values
             _bdshot.erpm[chan] = group.bdshot.erpm[i];
 
@@ -1381,7 +1394,7 @@ void RCOutput::dshot_send(pwm_group &group, uint32_t time_out_us)
             pwm = constrain_int16(pwm, 1000, 2000);
             uint16_t value = 2 * (pwm - 1000);
 
-            if (chan_mask & (reversible_mask>>chan_offset)) {
+            if (chan_mask & (_reversible_mask>>chan_offset)) {
                 // this is a DShot-3D output, map so that 1500 PWM is zero throttle reversed
                 if (value < 1000) {
                     value = 2000 - value;

libraries/AP_HAL_ChibiOS/RCOutput.h

@@ -32,6 +32,9 @@
 
 class ChibiOS::RCOutput : public AP_HAL::RCOutput {
 public:
+    // disabled channel marker
+    const static uint8_t CHAN_DISABLED = 255;
+
     void     init() override;
     void     set_freq(uint32_t chmask, uint16_t freq_hz) override;
     uint16_t get_freq(uint8_t ch) override;
@@ -138,14 +141,14 @@ public:
 
     /*
       enable telemetry request for a mask of channels. This is used
-      with DShot to get telemetry feedback
+      with Dshot to get telemetry feedback
      */
     void set_telem_request_mask(uint16_t mask) override { telem_request_mask = (mask >> chan_offset); }
 
 #ifdef HAL_WITH_BIDIR_DSHOT
     /*
       enable bi-directional telemetry request for a mask of channels. This is used
-      with DShot to get telemetry feedback
+      with Dshot to get telemetry feedback
      */
     void set_bidir_dshot_mask(uint16_t mask) override;
 #endif
@@ -155,6 +158,15 @@ public:
      */
     void set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz) override;
 
+#ifndef DISABLE_DSHOT
+    /*
+      Set/get the dshot esc_type
+     */
+    void set_dshot_esc_type(DshotEscType dshot_esc_type) override { _dshot_esc_type = dshot_esc_type; }
+
+    DshotEscType get_dshot_esc_type() const override { return _dshot_esc_type; }
+#endif
+
     /*
       get safety switch state, used by Util.cpp
     */
@@ -170,14 +182,37 @@ public:
      */
     void set_safety_mask(uint16_t mask) { safety_mask = mask; }
 
+#ifndef DISABLE_DSHOT
     /*
-     * mark the channels in chanmask as reversible. This is needed for some ESC types (such as DShot)
+     * mark the channels in chanmask as reversible. This is needed for some ESC types (such as Dshot)
      * so that output scaling can be performed correctly. The chanmask passed is added (ORed) into
      * any existing mask.
      */
-    void set_reversible_mask(uint16_t chanmask) override {
+    void set_reversible_mask(uint16_t chanmask) override;
-        reversible_mask |= chanmask;
+
-    }
+    /*
+     * mark the channels in chanmask as reversed. The chanmask passed is added (ORed) into
+     * any existing mask.
+     */
+    void set_reversed_mask(uint16_t chanmask) override;
+
+    /*
+      mark escs as active for the purpose of sending dshot commands
+     */
+    void set_active_escs_mask(uint16_t chanmask) override { _active_escs_mask |= chanmask; }
+
+    /*
+      Send a dshot command, if command timout is 0 then 10 commands are sent
+      chan is the servo channel to send the command to
+     */
+    void send_dshot_command(uint8_t command, uint8_t chan, uint32_t command_timeout_ms = 0, uint16_t repeat_count = 10, bool priority = false) override;
+
+#endif
+
+    /*
+      If not already done flush any dshot commands still pending
+     */
+    bool prepare_for_arming() override;
 
     /*
       setup serial LED output for a given channel number, with
@@ -342,6 +377,11 @@ private:
         bool can_send_dshot_pulse() const {
           return is_dshot_protocol(current_mode) && AP_HAL::micros() - last_dmar_send_us > (dshot_pulse_time_us + 50);
         }
+
+        // return whether the group channel is both enabled in the group and for output
+        bool is_chan_enabled(uint8_t c) const {
+          return chan[c] != CHAN_DISABLED && (ch_mask & (1U << chan[c]));
+        }
     };
     /*
       timer thread for use by dshot events
@@ -409,6 +449,7 @@ private:
     // these values are for the local channels. Non-local channels are handled by IOMCU
     uint32_t en_mask;
     uint16_t period[max_channels];
+
     // handling of bi-directional dshot
     struct {
         uint16_t mask;
@@ -431,12 +472,36 @@ private:
     // virtual timer for post-push() pulses
     virtual_timer_t _dshot_rate_timer;
 
+#ifndef DISABLE_DSHOT
+    // dshot commands
+    // RingBuffer to store outgoing request.
+    struct DshotCommandPacket {
+      uint8_t command;
+      uint32_t cycle;
+      uint8_t chan;
+    };
+
+    ObjectBuffer<DshotCommandPacket> _dshot_command_queue{8};
+    DshotCommandPacket _dshot_current_command;
+
+    DshotEscType _dshot_esc_type;
+
+    bool dshot_command_is_active(const pwm_group& group) const {
+      return (_dshot_current_command.chan == RCOutput::ALL_CHANNELS || (group.ch_mask & (1UL << _dshot_current_command.chan)))
+                && _dshot_current_command.cycle > 0;
+    }
+#endif
     uint16_t safe_pwm[max_channels]; // pwm to use when safety is on
     bool corked;
     // mask of channels that are running in high speed
     uint16_t fast_channel_mask;
     uint16_t io_fast_channel_mask;
-    uint16_t reversible_mask;
+    // mask of channels that are 3D capable
+    uint16_t _reversible_mask;
+    // mask of channels that should be reversed at startup
+    uint16_t _reversed_mask;
+    // mask of active ESCs
+    uint16_t _active_escs_mask;
 
     // min time to trigger next pulse to prevent overlap
     uint64_t min_pulse_trigger_us;
@@ -457,6 +522,11 @@ private:
 
     bool is_bidir_dshot_enabled() const { return _bdshot.mask != 0; }
 
+    // are all the ESCs returning data
+    bool group_escs_active(const pwm_group& group) const {
+      return group.ch_mask > 0 && (group.ch_mask & _active_escs_mask) == group.ch_mask;
+    }
+
     // find a channel group given a channel number
     struct pwm_group *find_chan(uint8_t chan, uint8_t &group_idx);
 
@@ -501,6 +571,7 @@ private:
 
     void dshot_send_groups(uint32_t time_out_us);
     void dshot_send(pwm_group &group, uint32_t time_out_us);
+    void dshot_send_command(pwm_group &group, uint8_t command, uint8_t chan);
     static void dshot_update_tick(void* p);
     static void dshot_send_next_group(void* p);
     // release locks on the groups that are pending in reverse order
@@ -523,7 +594,7 @@ private:
     void bdshot_ic_dma_allocate(Shared_DMA *ctx);
     void bdshot_ic_dma_deallocate(Shared_DMA *ctx);
     static uint32_t bdshot_decode_telemetry_packet(uint32_t* buffer, uint32_t count);
-    static bool bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan);
+    bool bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan);
     static uint8_t bdshot_find_next_ic_channel(const pwm_group& group);
     static void bdshot_dma_ic_irq_callback(void *p, uint32_t flags);
     static void bdshot_finish_dshot_gcr_transaction(void *p);
@@ -553,4 +624,10 @@ private:
 
 };
 
+#if RCOU_DSHOT_TIMING_DEBUG
+#define TOGGLE_PIN_DEBUG(pin) do { palToggleLine(HAL_GPIO_LINE_GPIO ## pin); } while (0)
+#else
+#define TOGGLE_PIN_DEBUG(pin) do {} while (0)
+#endif
+
 #endif // HAL_USE_PWM

libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp

@@ -29,17 +29,12 @@ extern const AP_HAL::HAL& hal;
 #if RCOU_DSHOT_TIMING_DEBUG
 #define DEBUG_CHANNEL 1
 #define TOGGLE_PIN_CH_DEBUG(pin, channel) do { if (channel == DEBUG_CHANNEL) palToggleLine(HAL_GPIO_LINE_GPIO ## pin); } while (0)
-#define TOGGLE_PIN_DEBUG(pin) do { palToggleLine(HAL_GPIO_LINE_GPIO ## pin); } while (0)
 #else
 #define TOGGLE_PIN_CH_DEBUG(pin, channel) do {} while (0)
-#define TOGGLE_PIN_DEBUG(pin) do {} while (0)
 #endif
 
 #define TELEM_IC_SAMPLE 16
 
-// marker for a disabled channel
-#define CHAN_DISABLED 255
-
 /*
  * enable bi-directional telemetry request for a mask of channels. This is used
  * with DShot to get telemetry feedback
@@ -68,7 +63,7 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group)
     bool set_curr_chan = false;
 
     for (uint8_t i = 0; i < 4; i++) {
-        if (group.chan[i] == CHAN_DISABLED ||
+        if (!group.is_chan_enabled(i) ||
             !group.dma_ch[i].have_dma || !(_bdshot.mask & (1 << group.chan[i]))) {
             continue;
         }
@@ -102,7 +97,7 @@ 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++) {
-        if (group.chan[i] == CHAN_DISABLED || !(_bdshot.mask & (1 << group.chan[i]))) {
+        if (!group.is_chan_enabled(i) || !(_bdshot.mask & (1 << group.chan[i]))) {
             continue;
         }
         uint8_t curr_chan = i;
@@ -403,7 +398,7 @@ void RCOutput::bdshot_finish_dshot_gcr_transaction(void *p)
  */
 bool RCOutput::bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan)
 {
-    if (group.chan[chan] == CHAN_DISABLED) {
+    if (!group.is_chan_enabled(chan)) {
         return true;
     }
 
@@ -450,7 +445,7 @@ uint8_t RCOutput::bdshot_find_next_ic_channel(const pwm_group& group)
     uint8_t chan = group.bdshot.curr_telem_chan;
     for (uint8_t i = 1; i < 4; i++) {
         const uint8_t next_chan = (chan + i) % 4;
-        if (group.chan[next_chan] != CHAN_DISABLED &&
+        if (group.is_chan_enabled(next_chan) &&
             group.bdshot.ic_dma_handle[next_chan] != nullptr) {
             return next_chan;
         }
@@ -486,11 +481,15 @@ void RCOutput::dma_up_irq_callback(void *p, uint32_t flags)
         // sending is done, in 30us the ESC will send telemetry
         bdshot_receive_pulses_DMAR(group);
     } else {
-        // non-bidir case
+        // non-bidir case, this prevents us ever having two dshot pulses too close together
-        // this prevents us ever having two dshot pulses too close together
+        if (is_dshot_protocol(group->current_mode)) {
-        // dshot mandates a minimum pulse separation of 40us, WS2812 mandates 50us so we
+            // since we could be sending a dshot command, wait the full telemetry pulse width
-        // pick the higher value
+            // dshot mandates a minimum pulse separation of 40us
-        chVTSetI(&group->dma_timeout, chTimeUS2I(50), dma_unlock, p);
+            chVTSetI(&group->dma_timeout, chTimeUS2I(group->dshot_pulse_send_time_us + 30U + 40U), dma_unlock, p);
+        } else {
+            // WS2812 mandates a minimum pulse separation of 50us
+            chVTSetI(&group->dma_timeout, chTimeUS2I(50U), dma_unlock, p);
+        }
     }
 
     chSysUnlockFromISR();

libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp

@@ -0,0 +1,143 @@
+/*
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include "RCOutput.h"
+#include <AP_Math/AP_Math.h>
+#include "hwdef/common/stm32_util.h"
+#include <AP_InternalError/AP_InternalError.h>
+#include <AP_Vehicle/AP_Vehicle_Type.h>
+
+#if HAL_USE_PWM == TRUE
+#ifndef DISABLE_DSHOT
+
+using namespace ChibiOS;
+
+extern const AP_HAL::HAL& hal;
+
+void RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t chan)
+{
+    if (!group.can_send_dshot_pulse()) {
+        return;
+    }
+
+    if (irq.waiter || (group.dshot_state != DshotState::IDLE && group.dshot_state != DshotState::RECV_COMPLETE)) {
+        // doing serial output or DMAR input, don't send DShot pulses
+        return;
+    }
+
+    TOGGLE_PIN_DEBUG(81);
+    // first make sure we have the DMA channel before anything else
+
+    osalDbgAssert(!group.dma_handle->is_locked(), "DMA handle is already locked");
+    group.dma_handle->lock();
+
+    // only the timer thread releases the locks
+    group.dshot_waiter = rcout_thread_ctx;
+    bool bdshot_telem = false;
+#ifdef HAL_WITH_BIDIR_DSHOT
+    // no need to get the input capture lock
+    group.bdshot.enabled = false;
+    if ((_bdshot.mask & group.ch_mask) == group.ch_mask) {
+        bdshot_telem = true;
+        // it's not clear why this is required, but without it we get no output
+        if (group.pwm_started) {
+            pwmStop(group.pwm_drv);
+        }
+        pwmStart(group.pwm_drv, &group.pwm_cfg);
+        group.pwm_started = true;
+    }
+#endif    
+
+    memset((uint8_t *)group.dma_buffer, 0, DSHOT_BUFFER_LENGTH);
+
+    // keep the other ESCs armed rather than sending nothing
+    const uint16_t zero_packet = create_dshot_packet(0, false, bdshot_telem);
+    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))) {
+            fill_DMA_buffer_dshot(group.dma_buffer + i, 4, packet, group.bit_width_mul);
+        } else if (group.is_chan_enabled(i)) {
+            fill_DMA_buffer_dshot(group.dma_buffer + i, 4, zero_packet, group.bit_width_mul);
+        }
+    }
+
+    chEvtGetAndClearEvents(group.dshot_event_mask);
+    // start sending the pulses out
+    send_pulses_DMAR(group, DSHOT_BUFFER_LENGTH);
+    TOGGLE_PIN_DEBUG(81);
+}
+
+// Send a dshot command, if command timout is 0 then 10 commands are sent
+// chan is the servo channel to send the command to
+void RCOutput::send_dshot_command(uint8_t command, uint8_t chan, uint32_t command_timeout_ms, uint16_t repeat_count, bool priority)
+{
+    // don't accept any LED or BEEP commands while initializing
+    if ((_dshot_period_us == 0 || _dshot_calibrating) && !priority) {
+        return;
+    }
+
+    DshotCommandPacket pkt;
+    pkt.command = command;
+    pkt.chan = chan;
+    if (_dshot_period_us == 0 || _dshot_calibrating || command_timeout_ms == 0) {
+        pkt.cycle = MAX(10, repeat_count);
+    } else {
+        pkt.cycle = MAX(command_timeout_ms * 1000 / _dshot_period_us, repeat_count);
+    }
+
+    // prioritize anything that is not an LED or BEEP command
+    if (!_dshot_command_queue.push(pkt) && priority) {
+        _dshot_command_queue.push_force(pkt);
+    }
+}
+
+// Set the dshot outputs that should be reversed (as opposed to 3D)
+// The chanmask passed is added (ORed) into any existing mask.
+void RCOutput::set_reversed_mask(uint16_t chanmask) {
+    _reversed_mask |= chanmask;
+
+    for (uint8_t i=0; i<HAL_PWM_COUNT; i++) {
+        if (chanmask & (1U<<i)) {
+            switch (_dshot_esc_type) {
+                case DSHOT_ESC_BLHELI:
+                    send_dshot_command(DSHOT_REVERSE, i, 0, 10, true);
+                    break;
+                default:
+                    break;
+            }
+        }
+    }
+}
+
+// Set the dshot outputs that should be reversed (as opposed to 3D)
+// The chanmask passed is added (ORed) into any existing mask.
+void RCOutput::set_reversible_mask(uint16_t chanmask) {
+    _reversible_mask |= chanmask;
+
+    for (uint8_t i=0; i<HAL_PWM_COUNT; i++) {
+        if (chanmask & (1U<<i)) {
+            switch (_dshot_esc_type) {
+                case DSHOT_ESC_BLHELI:
+                    send_dshot_command(DSHOT_3D_ON, i, 0, 10, true);
+                    break;
+                default:
+                    break;
+            }
+        }
+    }
+}
+
+#endif // DISABLE_DSHOT
+#endif // HAL_USE_PWM

libraries/AP_HAL_ChibiOS/Util.cpp

@@ -153,17 +153,24 @@ Util::safety_state Util::safety_switch_state(void)
 
 #ifdef HAL_PWM_ALARM
 struct Util::ToneAlarmPwmGroup Util::_toneAlarm_pwm_group = HAL_PWM_ALARM;
+#endif
 
-bool Util::toneAlarm_init()
+#if defined(HAL_PWM_ALARM) || HAL_DSHOT_ALARM
+uint8_t  Util::_toneAlarm_types = 0;
+
+bool Util::toneAlarm_init(uint8_t types)
 {
+#ifdef HAL_PWM_ALARM
     _toneAlarm_pwm_group.pwm_cfg.period = 1000;
     pwmStart(_toneAlarm_pwm_group.pwm_drv, &_toneAlarm_pwm_group.pwm_cfg);
-
+#endif
+    _toneAlarm_types = types;
     return true;
 }
 
 void Util::toneAlarm_set_buzzer_tone(float frequency, float volume, uint32_t duration_ms)
 {
+#ifdef HAL_PWM_ALARM
     if (is_zero(frequency) || is_zero(volume)) {
         pwmDisableChannel(_toneAlarm_pwm_group.pwm_drv, _toneAlarm_pwm_group.chan);
     } else {
@@ -172,8 +179,29 @@ void Util::toneAlarm_set_buzzer_tone(float frequency, float volume, uint32_t dur
 
         pwmEnableChannel(_toneAlarm_pwm_group.pwm_drv, _toneAlarm_pwm_group.chan, roundf(volume*_toneAlarm_pwm_group.pwm_cfg.frequency/frequency)/2);
     }
+#endif
+#if HAL_DSHOT_ALARM
+    // don't play the motors while flying
+    if ((_toneAlarm_types & ALARM_DSHOT) && (get_soft_armed() || hal.rcout->get_dshot_esc_type() != RCOutput::DSHOT_ESC_BLHELI)) {
+        return;
+    }
+
+    if (is_zero(frequency)) {   // silence
+        hal.rcout->send_dshot_command(RCOutput::DSHOT_RESET, RCOutput::ALL_CHANNELS, duration_ms);
+    } else if (frequency < 1047) { // C
+        hal.rcout->send_dshot_command(RCOutput::DSHOT_BEEP1, RCOutput::ALL_CHANNELS, duration_ms);
+    } else if (frequency < 1175) {  // D
+        hal.rcout->send_dshot_command(RCOutput::DSHOT_BEEP2, RCOutput::ALL_CHANNELS, duration_ms);
+    } else if (frequency < 1319) {  // E
+        hal.rcout->send_dshot_command(RCOutput::DSHOT_BEEP3, RCOutput::ALL_CHANNELS, duration_ms);
+    } else if (frequency < 1397) {  // F
+        hal.rcout->send_dshot_command(RCOutput::DSHOT_BEEP4, RCOutput::ALL_CHANNELS, duration_ms);
+    } else {  // G+
+        hal.rcout->send_dshot_command(RCOutput::DSHOT_BEEP5, RCOutput::ALL_CHANNELS, duration_ms);
     }
 #endif // HAL_PWM_ALARM
+}
+#endif
 
 /*
   set HW RTC in UTC microseconds

libraries/AP_HAL_ChibiOS/Util.h

@@ -59,9 +59,10 @@ public:
     bool get_system_id(char buf[40]) override;
     bool get_system_id_unformatted(uint8_t buf[], uint8_t &len) override;
 
-#ifdef HAL_PWM_ALARM
+#if defined(HAL_PWM_ALARM) || HAL_DSHOT_ALARM
-    bool toneAlarm_init() override;
+    bool toneAlarm_init(uint8_t types) override;
     void toneAlarm_set_buzzer_tone(float frequency, float volume, uint32_t duration_ms) override;
+    static uint8_t _toneAlarm_types;
 #endif
 
     // return true if the reason for the reboot was a watchdog reset

libraries/AP_HAL_ChibiOS/hwdef/MatekH743/hwdef.dat

@@ -215,4 +215,3 @@ define HAL_OSD_TYPE_DEFAULT 1
 ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin
 
 define BOARD_PWM_COUNT_DEFAULT 13
-