diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index 7d2fa32093..d710f97873 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -3803,7 +3803,7 @@ static void executeEscInfoCommand(const char *cmdName, uint8_t escIndex) startEscDataRead(escInfoBuffer, ESC_INFO_BLHELI32_EXPECTED_FRAME_SIZE); - dshotCommandWrite(escIndex, getMotorCount(), DSHOT_CMD_ESC_INFO, true); + dshotCommandWrite(escIndex, getMotorCount(), DSHOT_CMD_ESC_INFO, DSHOT_CMD_TYPE_BLOCKING); delay(10); @@ -3811,9 +3811,6 @@ static void executeEscInfoCommand(const char *cmdName, uint8_t escIndex) } #endif // USE_ESC_SENSOR && USE_ESC_SENSOR_INFO - -// XXX Review dshotprog command under refactored motor handling - static void cliDshotProg(const char *cmdName, char *cmdline) { if (isEmpty(cmdline) || !isMotorProtocolDshot()) { @@ -3854,7 +3851,7 @@ static void cliDshotProg(const char *cmdName, char *cmdline) } if (command != DSHOT_CMD_ESC_INFO) { - dshotCommandWrite(escIndex, getMotorCount(), command, true); + dshotCommandWrite(escIndex, getMotorCount(), command, DSHOT_CMD_TYPE_BLOCKING); } else { #if defined(USE_ESC_SENSOR) && defined(USE_ESC_SENSOR_INFO) if (featureIsEnabled(FEATURE_ESC_SENSOR)) { diff --git a/src/main/drivers/dshot_command.c b/src/main/drivers/dshot_command.c index 97863dd226..47790a7e33 100644 --- a/src/main/drivers/dshot_command.c +++ b/src/main/drivers/dshot_command.c @@ -151,18 +151,23 @@ static bool allMotorsAreIdle(void) return true; } -bool dshotCommandsAreEnabled(void) +bool dshotCommandsAreEnabled(dshotCommandType_e commandType) { - if (motorIsEnabled() && motorGetMotorEnableTimeMs() && millis() > motorGetMotorEnableTimeMs() + DSHOT_PROTOCOL_DETECTION_DELAY_MS) { - return true; - } else { - return false; + bool ret = false; + + if (commandType == DSHOT_CMD_TYPE_BLOCKING) { + ret = !motorIsEnabled(); + } else if (commandType == DSHOT_CMD_TYPE_INLINE) { + if (motorIsEnabled() && motorGetMotorEnableTimeMs() && millis() > motorGetMotorEnableTimeMs() + DSHOT_PROTOCOL_DETECTION_DELAY_MS) { + ret = true; + } } + return ret; } -void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking) +void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshotCommandType_e commandType) { - if (!isMotorProtocolDshot() || !dshotCommandsAreEnabled() || (command > DSHOT_MAX_COMMAND) || dshotCommandQueueFull()) { + if (!isMotorProtocolDshot() || !dshotCommandsAreEnabled(commandType) || (command > DSHOT_MAX_COMMAND) || dshotCommandQueueFull()) { return; } @@ -192,7 +197,7 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, bool break; } - if (blocking) { + if (commandType == DSHOT_CMD_TYPE_BLOCKING) { delayMicroseconds(DSHOT_INITIAL_DELAY_US - DSHOT_COMMAND_DELAY_US); for (; repeats; repeats--) { delayMicroseconds(DSHOT_COMMAND_DELAY_US); @@ -213,7 +218,7 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, bool dshotPwmDevice.vTable.updateComplete(); } delayMicroseconds(delayAfterCommandUs); - } else { + } else if (commandType == DSHOT_CMD_TYPE_INLINE) { dshotCommandControl_t *commandControl = addCommand(); if (commandControl) { commandControl->repeats = repeats; diff --git a/src/main/drivers/dshot_command.h b/src/main/drivers/dshot_command.h index 86e5b21438..9351ab49fe 100644 --- a/src/main/drivers/dshot_command.h +++ b/src/main/drivers/dshot_command.h @@ -62,10 +62,15 @@ typedef enum { DSHOT_CMD_MAX = 47 } dshotCommands_e; -void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking); +typedef enum { + DSHOT_CMD_TYPE_INLINE = 0, // dshot commands sent inline with motor signal (motors must be enabled) + DSHOT_CMD_TYPE_BLOCKING // dshot commands sent in blocking method (motors must be disabled) +} dshotCommandType_e; + +void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshotCommandType_e commandType); void dshotSetPidLoopTime(uint32_t pidLoopTime); bool dshotCommandQueueEmpty(void); bool dshotCommandIsProcessing(void); uint8_t dshotCommandGetCurrent(uint8_t index); bool dshotCommandOutputIsEnabled(uint8_t motorCount); -bool dshotCommandsAreEnabled(void); +bool dshotCommandsAreEnabled(dshotCommandType_e commandType); diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 8aed83b1b4..26539e23d5 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -277,7 +277,7 @@ void updateArmingStatus(void) // 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() + && dshotCommandsAreEnabled(DSHOT_CMD_TYPE_INLINE) #endif ) { // If so, unset the grace time arming disable flag @@ -458,7 +458,7 @@ void disarm(flightLogDisarmReason_e reason) BEEP_OFF; #ifdef USE_DSHOT if (isMotorProtocolDshot() && flipOverAfterCrashActive && !featureIsEnabled(FEATURE_3D)) { - dshotCommandWrite(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false); + dshotCommandWrite(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, DSHOT_CMD_TYPE_INLINE); } #endif flipOverAfterCrashActive = false; @@ -509,7 +509,7 @@ void tryArm(void) if (!(IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) || (tryingToArm == ARMING_DELAYED_CRASHFLIP))) { flipOverAfterCrashActive = false; if (!featureIsEnabled(FEATURE_3D)) { - dshotCommandWrite(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false); + dshotCommandWrite(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, DSHOT_CMD_TYPE_INLINE); } } else { flipOverAfterCrashActive = true; @@ -517,7 +517,7 @@ void tryArm(void) runawayTakeoffCheckDisabled = false; #endif if (!featureIsEnabled(FEATURE_3D)) { - dshotCommandWrite(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED, false); + dshotCommandWrite(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED, DSHOT_CMD_TYPE_INLINE); } } } diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 7014dcd2d9..aa4f953d55 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -401,7 +401,7 @@ void beeperUpdate(timeUs_t currentTimeUs) if ((currentTimeUs - getLastDisarmTimeUs() > DSHOT_BEACON_GUARD_DELAY_US) && !isTryingToArm()) { lastDshotBeaconCommandTimeUs = currentTimeUs; - dshotCommandWrite(ALL_MOTORS, getMotorCount(), beeperConfig()->dshotBeaconTone, false); + dshotCommandWrite(ALL_MOTORS, getMotorCount(), beeperConfig()->dshotBeaconTone, DSHOT_CMD_TYPE_INLINE); } } #endif