diff --git a/src/main/drivers/dshot_command.c b/src/main/drivers/dshot_command.c index 087fa68495..4c65d2c933 100644 --- a/src/main/drivers/dshot_command.c +++ b/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; } diff --git a/src/main/drivers/dshot_command.h b/src/main/drivers/dshot_command.h index 0d2db40f98..86e5b21438 100644 --- a/src/main/drivers/dshot_command.h +++ b/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); diff --git a/src/main/drivers/motor.c b/src/main/drivers/motor.c index 336f5652a2..6fe7dc769d 100644 --- a/src/main/drivers/motor.c +++ b/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 || diff --git a/src/main/drivers/motor.h b/src/main/drivers/motor.h index 67221fa389..87bff1716f 100644 --- a/src/main/drivers/motor.h +++ b/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 diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 032f4754e6..62b59d4c80 100644 --- a/src/main/fc/core.c +++ b/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); }