From d4b1c06bf8fc378dfdc4dfb31bf45412e862458d Mon Sep 17 00:00:00 2001
From: mikeller <github@ike.ch>
Date: Sun, 10 Sep 2017 12:55:47 +1200
Subject: [PATCH] Added delay before sending Dshot commands to avoid read
 errors.

---
 src/main/fc/cli.c     | 47 ++++++++++++++++++++++++++-----------------
 src/main/fc/fc_core.c |  1 +
 src/main/io/beeper.c  |  5 +++--
 3 files changed, 32 insertions(+), 21 deletions(-)

diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index b1a5c742e0..d969a61685 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -2456,6 +2456,7 @@ static void cliDshotProg(char *cmdline)
     char *pch = strtok_r(cmdline, " ", &saveptr);
     int pos = 0;
     int escIndex = 0;
+    bool firstCommand = true;
     while (pch != NULL) {
         switch (pos) {
         case 0:
@@ -2466,33 +2467,41 @@ static void cliDshotProg(char *cmdline)
 
             break;
         default:
-            pwmDisableMotors();
+            {
+                int command = atoi(pch);
+                if (command >= 0 && command < DSHOT_MIN_THROTTLE) {
+                    if (firstCommand) {
+                        pwmDisableMotors();
 
-            int command = atoi(pch);
-            if (command >= 0 && command < DSHOT_MIN_THROTTLE) {
-                if (command == DSHOT_CMD_ESC_INFO) {
-                    delay(5); // Wait for potential ESC telemetry transmission to finish
-                }
+                        if (command == DSHOT_CMD_ESC_INFO) {
+                            delay(5); // Wait for potential ESC telemetry transmission to finish
+                        } else {
+                            delay(1);
+                        }
 
-                if (command != DSHOT_CMD_ESC_INFO) {
-                    pwmWriteDshotCommand(escIndex, getMotorCount(), command);
-                } else {
-                    if (escIndex != ALL_MOTORS) {
-                        executeEscInfoCommand(escIndex);
+                        firstCommand = false;
+                    }
+
+                    if (command != DSHOT_CMD_ESC_INFO) {
+                        pwmWriteDshotCommand(escIndex, getMotorCount(), command);
                     } else {
-                        for (uint8_t i = 0; i < getMotorCount(); i++) {
-                            executeEscInfoCommand(i);
+                        if (escIndex != ALL_MOTORS) {
+                            executeEscInfoCommand(escIndex);
+                        } else {
+                            for (uint8_t i = 0; i < getMotorCount(); i++) {
+                                executeEscInfoCommand(i);
+                            }
                         }
                     }
-                }
 
-                cliPrintLinef("Command %d written.", command);
+                    cliPrintLinef("Command %d written.", command);
 
-                if (command <= 5) {
-                    delay(20); // wait for sound output to finish
+                    if (command <= 5) {
+                        delay(20); // wait for sound output to finish
+                    }
+                } else {
+                    cliPrintLinef("Invalid command, range 1 to %d.", DSHOT_MIN_THROTTLE - 1);
                 }
-            } else {
-                cliPrintLinef("Invalid command, range 1 to %d.", DSHOT_MIN_THROTTLE - 1);
             }
 
             break;
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 6a37dadaae..22c3ea0084 100644
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -265,6 +265,7 @@ void tryArm(void)
 #ifdef USE_DSHOT
         if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH)) {
             pwmDisableMotors();
+            delay(1);
 
             if (!IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
                 flipOverAfterCrashMode = false;
diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c
index a51cb925cd..4f803b430a 100755
--- a/src/main/io/beeper.c
+++ b/src/main/io/beeper.c
@@ -364,11 +364,12 @@ void beeperUpdate(timeUs_t currentTimeUs)
 
     #ifdef USE_DSHOT
     if (!areMotorsRunning() && beeperConfig()->dshotForward && currentBeeperEntry->mode == BEEPER_RX_SET) {
-	pwmDisableMotors();
+        pwmDisableMotors();
+        delay(1);
 
         pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_BEEP3);
 
-	pwmEnableMotors();
+        pwmEnableMotors();
     }
     #endif