From 4fa0e275fd1b255b6c37c9d511ab784bd02cae4b Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Fri, 25 Mar 2022 08:09:20 +0000
Subject: [PATCH] AP_HAL_ChibiOS: fix ESCs constantly arming on rover with
 dshot commands

make sure debug will compile
take into account active channels when configuring bdshot
add channel mask debug output
correct set bdshot telemetry position at startup
make sure all channels in a bdshot group are pulled high to prevent spurious pulses
---
 libraries/AP_HAL_ChibiOS/RCOutput.cpp        | 28 +++++++++++++------
 libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp | 29 ++++++++++++--------
 libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp | 15 ++++++++--
 3 files changed, 50 insertions(+), 22 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
index f80006dd5cf..3ad81dc9e2f 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
@@ -201,6 +201,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
     }
 }
 
@@ -1338,8 +1348,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;
@@ -1398,18 +1409,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 6b970a13358..73a99a4d40b 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 3fddedc1a4d..9183a4ca2f8 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;
 }