From c6c9ba7229e7ad23743e470d7ce14df99fb95a49 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Fri, 17 Apr 2020 08:29:24 -0400 Subject: [PATCH] Fix blocking DSHOT commands Motor and DSHOT refactoring broke blocking DSHOT commands as the check for commands being enabled was not taking into account that "blocking" type commands need to operate only when the motors are not enabled. Fixes the CLI `dshotprog` command. --- src/main/cli/cli.c | 7 ++----- src/main/drivers/dshot_command.c | 23 ++++++++++++++--------- src/main/drivers/dshot_command.h | 9 +++++++-- src/main/fc/core.c | 8 ++++---- src/main/io/beeper.c | 2 +- 5 files changed, 28 insertions(+), 21 deletions(-) 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