From 946b4db0e64ae5d280770d136f97d9e6921b7b3e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 4 Aug 2018 15:29:22 +1000
Subject: [PATCH] HAL_ChibiOS: set min dshot separation to 100usec

this fixes a problem with BLHeli32 not recognising dshot
---
 libraries/AP_HAL_ChibiOS/RCOutput.cpp | 17 +++++++++++++++--
 libraries/AP_HAL_ChibiOS/RCOutput.h   |  7 +++++--
 2 files changed, 20 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
index ba37fd3f29..1269d9486f 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
@@ -68,6 +68,7 @@ void RCOutput::init()
             pwmStart(group.pwm_drv, &group.pwm_cfg);
             group.pwm_started = true;
         }
+        chVTObjectInit(&group.dma_timeout);
     }
 
 #if HAL_WITH_IO_MCU
@@ -777,7 +778,7 @@ void RCOutput::timer_tick(void)
         if (!serial_group &&
             group.current_mode >= MODE_PWM_DSHOT150 &&
             group.current_mode <= MODE_PWM_DSHOT1200 &&
-            now - group.last_dshot_send_us > 900) {
+            now - group.last_dshot_send_us > 400) {
             // do a blocking send now, to guarantee DShot sends at
             // above 1000 Hz. This makes the protocol more reliable on
             // long cables, and also keeps some ESCs happy that don't
@@ -950,6 +951,17 @@ void RCOutput::send_pulses_DMAR(pwm_group &group, uint32_t buffer_length)
     dmaStreamEnable(group.dma);
 }
 
+/*
+  unlock DMA channel after a dshot send completes
+ */
+void RCOutput::dma_unlock(void *p)
+{
+    pwm_group *group = (pwm_group *)p;
+    chSysLockFromISR();
+    group->dma_handle->unlock_from_IRQ();
+    chSysUnlockFromISR();    
+}
+
 /*
   DMA interrupt handler. Used to mark DMA completed for DShot
  */
@@ -962,7 +974,8 @@ void RCOutput::dma_irq_callback(void *p, uint32_t flags)
         // tell the waiting process we've done the DMA
         chEvtSignalI(irq.waiter, serial_event_mask);
     } else {
-        group->dma_handle->unlock_from_IRQ();
+        // this prevents us ever having two dshot pulses too close together
+        chVTSetI(&group->dma_timeout, chTimeUS2I(dshot_min_gap_us), dma_unlock, p);
     }
     chSysUnlockFromISR();
 }
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h
index d0ec44b7c0..f6c406b450 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.h
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.h
@@ -158,7 +158,8 @@ private:
         uint32_t rc_frequency;
         bool in_serial_dma;
         uint64_t last_dshot_send_us;
-
+        virtual_timer_t dma_timeout;
+        
         // serial output
         struct {
             // expected time per bit
@@ -277,9 +278,10 @@ private:
     /*
       DShot handling
      */
-    const uint8_t dshot_post = 6;
+    const uint8_t dshot_post = 2;
     const uint16_t dshot_bit_length = 16 + dshot_post;
     const uint16_t dshot_buffer_length = dshot_bit_length*4*sizeof(uint32_t);
+    static const uint16_t dshot_min_gap_us = 100;
     uint32_t dshot_pulse_time_us;
     uint16_t telem_request_mask;
     
@@ -289,6 +291,7 @@ private:
     void fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t packet, uint16_t clockmul);
     void dshot_send(pwm_group &group, bool blocking);
     static void dma_irq_callback(void *p, uint32_t flags);
+    static void dma_unlock(void *p);
     bool mode_requires_dma(enum output_mode mode) const;
     bool setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_width, bool active_high);
     void send_pulses_DMAR(pwm_group &group, uint32_t buffer_length);