Prevent DSHOT commands during protocol initialization/detection (#8927)

Prevent DSHOT commands during protocol initialization/detection

src/main/drivers/dshot_command.c

@@ -37,6 +37,7 @@
 #include "drivers/dshot_command.h"
 #include "drivers/pwm_output.h"
 
+#define DSHOT_PROTOCOL_DETECTION_DELAY_MS 3000
 #define DSHOT_INITIAL_DELAY_US 10000
 #define DSHOT_COMMAND_DELAY_US 1000
 #define DSHOT_ESCINFO_DELAY_US 12000
@@ -150,9 +151,18 @@ static bool allMotorsAreIdle(void)
     return true;
 }
 
+bool dshotCommandsAreEnabled(void)
+{
+    if (motorIsEnabled() && motorGetMotorEnableTimeMs() && millis() > motorGetMotorEnableTimeMs() + DSHOT_PROTOCOL_DETECTION_DELAY_MS) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
 void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking)
 {
-    if (!isMotorProtocolDshot() || (command > DSHOT_MAX_COMMAND) || dshotCommandQueueFull()) {
+    if (!isMotorProtocolDshot() || !dshotCommandsAreEnabled() || (command > DSHOT_MAX_COMMAND) || dshotCommandQueueFull()) {
         return;
     }
 

src/main/drivers/dshot_command.h

@@ -68,3 +68,4 @@ bool dshotCommandQueueEmpty(void);
 bool dshotCommandIsProcessing(void);
 uint8_t dshotCommandGetCurrent(uint8_t index);
 bool dshotCommandOutputIsEnabled(uint8_t motorCount);
+bool dshotCommandsAreEnabled(void);

src/main/drivers/motor.c

@@ -50,6 +50,7 @@ void motorShutdown(void)
 {
     motorDevice->vTable.shutdown();
     motorDevice->enabled = false;
+    motorDevice->motorEnableTimeMs = 0;
     motorDevice->initialized = false;
     delayMicroseconds(1500);
 }
@@ -246,6 +247,7 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
     if (motorDevice) {
         motorDevice->count = motorCount;
         motorDevice->initialized = true;
+        motorDevice->motorEnableTimeMs = 0;
         motorDevice->enabled = false;
     } else {
         motorNullDevice.vTable = motorNullVTable;
@@ -257,12 +259,14 @@ void motorDisable(void)
 {
     motorDevice->vTable.disable();
     motorDevice->enabled = false;
+    motorDevice->motorEnableTimeMs = 0;
 }
 
 void motorEnable(void)
 {
     if (motorDevice->initialized && motorDevice->vTable.enable()) {
         motorDevice->enabled = true;
+        motorDevice->motorEnableTimeMs = millis();
     }
 }
 
@@ -281,6 +285,13 @@ bool isMotorProtocolDshot(void)
     return isDshot;
 }
 
+#ifdef USE_DSHOT
+timeMs_t motorGetMotorEnableTimeMs(void)
+{
+    return motorDevice->motorEnableTimeMs;
+}
+#endif
+
 #ifdef USE_DSHOT_BITBANG
 bool isDshotBitbangActive(const motorDevConfig_t *motorConfig) {
     return motorConfig->useDshotBitbang == DSHOT_BITBANG_ON ||

src/main/drivers/motor.h

@@ -22,6 +22,8 @@
 
 #pragma once
 
+#include "common/time.h"
+
 typedef enum {
     PWM_TYPE_STANDARD = 0,
     PWM_TYPE_ONESHOT125,
@@ -62,6 +64,7 @@ typedef struct motorDevice_s {
     uint8_t       count;
     bool          initialized;
     bool          enabled;
+    timeMs_t      motorEnableTimeMs;
 } motorDevice_t;
 
 void motorPostInitNull();
@@ -85,6 +88,7 @@ void motorDisable(void);
 void motorEnable(void);
 bool motorIsEnabled(void);
 bool motorIsMotorEnabled(uint8_t index);
+timeMs_t motorGetMotorEnableTimeMs(void);
 void motorShutdown(void); // Replaces stopPwmAllMotors
 
 #ifdef USE_DSHOT_BITBANG

src/main/fc/core.c

@@ -220,7 +220,14 @@ void updateArmingStatus(void)
         LED0_ON;
     } else {
         // Check if the power on arming grace time has elapsed
-        if ((getArmingDisableFlags() & ARMING_DISABLED_BOOT_GRACE_TIME) && (millis() >= systemConfig()->powerOnArmingGraceTime * 1000)) {
+        if ((getArmingDisableFlags() & ARMING_DISABLED_BOOT_GRACE_TIME) && (millis() >= systemConfig()->powerOnArmingGraceTime * 1000)
+#ifdef USE_DSHOT
+            // We also need to prevent arming until it's possible to send DSHOT commands.
+            // Otherwise if the initial arming is in crash-flip the motor direction commands
+            // might not be sent.
+            && dshotCommandsAreEnabled()
+#endif
+        ) {
             // If so, unset the grace time arming disable flag
             unsetArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME);
         }