From 6bdb523630494acafbbd180ae5609cedad1282d2 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 2 Apr 2018 10:39:17 +1000
Subject: [PATCH] HAL_ChibiOS: guarantee 1kHz output for DShot

---
 libraries/AP_HAL_ChibiOS/RCOutput.cpp | 16 ++++++++++------
 libraries/AP_HAL_ChibiOS/RCOutput.h   |  6 ++----
 2 files changed, 12 insertions(+), 10 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
index 6606401969..427456da4d 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
@@ -723,10 +723,16 @@ void RCOutput::trigger_groups(void)
 
 void RCOutput::timer_tick(void)
 {
+    uint64_t now = AP_HAL::micros64();
     for (uint8_t i = 0; i < NUM_GROUPS; i++ ) {
         pwm_group &group = pwm_group_list[i];
-        if (dshot_delayed_trigger_mask & (1U<<i)) {
-            // do a blocking send now
+        if (group.current_mode >= MODE_PWM_DSHOT150 &&
+            group.current_mode <= MODE_PWM_DSHOT1200 &&
+            now - group.last_dshot_send_us > 900) {
+            // 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
+            // like low rates
             dshot_send(group, true);
         }
     }
@@ -734,7 +740,6 @@ void RCOutput::timer_tick(void)
         min_pulse_trigger_us == 0) {
         return;
     }
-    uint64_t now = AP_HAL::micros64();
     if (now > min_pulse_trigger_us &&
         now - min_pulse_trigger_us > 10000) {
         // trigger at a minimum of 100Hz
@@ -814,13 +819,10 @@ void RCOutput::dshot_send(pwm_group &group, bool blocking)
         return;
     }
     
-    uint8_t groupidx = &group - pwm_group_list;
     if (blocking) {
         group.dma_handle->lock();
-        dshot_delayed_trigger_mask &= ~(1U<<groupidx);
     } else {
         if (!group.dma_handle->lock_nonblock()) {
-            dshot_delayed_trigger_mask |= 1U<<groupidx;
             return;
         }
     }
@@ -841,6 +843,8 @@ void RCOutput::dshot_send(pwm_group &group, bool blocking)
 
     // start sending the pulses out
     send_pulses_DMAR(group, dshot_buffer_length);
+
+    group.last_dshot_send_us = AP_HAL::micros64();
 }
 
 /*
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h
index f4b34aa379..1d25773012 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.h
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.h
@@ -135,6 +135,7 @@ private:
         uint32_t bit_width_mul;
         uint32_t rc_frequency;
         bool in_serial_dma;
+        uint64_t last_dshot_send_us;
 
         // serial output
         struct {
@@ -212,9 +213,6 @@ private:
     // which output groups need triggering
     uint8_t trigger_groupmask;
 
-    // mask of groups needing retriggering for DShot
-    uint8_t dshot_delayed_trigger_mask;
-    
     // widest pulse for oneshot triggering
     uint16_t trigger_widest_pulse;
 
@@ -230,7 +228,7 @@ private:
     /*
       DShot handling
      */
-    const uint8_t dshot_post = 2;
+    const uint8_t dshot_post = 6;
     const uint16_t dshot_bit_length = 16 + dshot_post;
     const uint16_t dshot_buffer_length = dshot_bit_length*4*sizeof(uint32_t);
     uint32_t dshot_pulse_time_us;