diff --git a/1.8 b/1.8 new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 7578b2ecb6..166d5c8882 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -37,6 +37,18 @@ static FAST_RAM_ZERO_INIT pwmCompleteWriteFn *pwmCompleteWrite = NULL; #ifdef USE_DSHOT FAST_RAM_ZERO_INIT loadDmaBufferFn *loadDmaBuffer; +#define DSHOT_COMMAND_DELAY_US 1000 +#define DSHOT_ESCINFO_DELAY_US 5000 +#define DSHOT_BEEP_DELAY_US 100000 + +typedef struct dshotCommandControl_s { + timeUs_t nextCommandAt; + timeUs_t delayAfterCommand; + uint8_t command[MAX_SUPPORTED_MOTORS]; + uint8_t repeats; +} dshotCommandControl_t; + +dshotCommandControl_t dshotCommandControl; #endif #ifdef USE_SERVOS @@ -374,27 +386,44 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) } } -void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command) +void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking) { - if (isDshot && (command <= DSHOT_MAX_COMMAND)) { - unsigned repeats; - switch (command) { - case DSHOT_CMD_SPIN_DIRECTION_1: - case DSHOT_CMD_SPIN_DIRECTION_2: - case DSHOT_CMD_3D_MODE_OFF: - case DSHOT_CMD_3D_MODE_ON: - case DSHOT_CMD_SAVE_SETTINGS: - case DSHOT_CMD_SPIN_DIRECTION_NORMAL: - case DSHOT_CMD_SPIN_DIRECTION_REVERSED: - repeats = 10; - break; - default: - repeats = 1; - break; - } + timeUs_t timeNowUs = micros(); + if (!isMotorProtocolDshot() || (command > DSHOT_MAX_COMMAND)) { + return; + } + + uint8_t repeats = 1; + timeUs_t timeDelayUs = DSHOT_COMMAND_DELAY_US; + + switch (command) { + case DSHOT_CMD_SPIN_DIRECTION_1: + case DSHOT_CMD_SPIN_DIRECTION_2: + case DSHOT_CMD_3D_MODE_OFF: + case DSHOT_CMD_3D_MODE_ON: + case DSHOT_CMD_SAVE_SETTINGS: + case DSHOT_CMD_SPIN_DIRECTION_NORMAL: + case DSHOT_CMD_SPIN_DIRECTION_REVERSED: + repeats = 10; + timeDelayUs = DSHOT_COMMAND_DELAY_US; + break; + case DSHOT_CMD_BEACON1: + case DSHOT_CMD_BEACON2: + case DSHOT_CMD_BEACON3: + case DSHOT_CMD_BEACON4: + case DSHOT_CMD_BEACON5: + repeats = 1; + timeDelayUs = DSHOT_BEEP_DELAY_US; + break; + default: + break; + } + + if (blocking) { for (; repeats; repeats--) { for (uint8_t i = 0; i < motorCount; i++) { + delayMicroseconds(DSHOT_COMMAND_DELAY_US); if ((i == index) || (index == ALL_MOTORS)) { motorDmaOutput_t *const motor = getMotorDmaOutput(i); motor->requestTelemetry = true; @@ -403,11 +432,57 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command) } pwmCompleteDshotMotorUpdate(0); - delay(1); + } + delayMicroseconds(timeDelayUs); + } else { + if (!pwmIsProcessingDshotCommand()) { + for (uint8_t i = 0; i < motorCount; i++) { + if ((index == i) || (index == ALL_MOTORS)) { + dshotCommandControl.command[i] = command; + dshotCommandControl.repeats = repeats; + dshotCommandControl.nextCommandAt = timeNowUs + DSHOT_COMMAND_DELAY_US; + dshotCommandControl.delayAfterCommand = timeDelayUs; + } + } } } } +FAST_RAM bool pwmIsProcessingDshotCommand(void) +{ + return dshotCommandControl.nextCommandAt; +} + +uint8_t pwmGetDshotCommand(uint8_t index) +{ + return dshotCommandControl.command[index]; +} + +bool FAST_CODE_NOINLINE pwmProcessDshotCommand(uint8_t motorCount) +{ + timeUs_t timeNowUs = micros(); + if (cmpTimeUs(timeNowUs, dshotCommandControl.nextCommandAt) < 0) { + return false; //Skip motor update because it isn't time yet for a new command + } + + //Timed motor update happening with dshot command + if (dshotCommandControl.repeats > 0) { + dshotCommandControl.repeats--; + dshotCommandControl.nextCommandAt = timeNowUs + DSHOT_COMMAND_DELAY_US; + if (dshotCommandControl.repeats == 0) { + dshotCommandControl.nextCommandAt = timeNowUs + dshotCommandControl.delayAfterCommand; + } + } else { + for (uint8_t i = 0; i < motorCount; i++) { + dshotCommandControl.command[i] = 0; + } + dshotCommandControl.nextCommandAt = 0; + dshotCommandControl.delayAfterCommand = 0; + } + + return true; +} + FAST_CODE uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, const uint16_t value) { uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0); diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 7677ec3ebe..74f1594632 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -196,10 +196,15 @@ uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, uint16_t value); extern loadDmaBufferFn *loadDmaBuffer; uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType); -void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command); +void pwmWriteDshotCommandControl(uint8_t index); +void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking); void pwmWriteDshotInt(uint8_t index, uint16_t value); void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output); void pwmCompleteDshotMotorUpdate(uint8_t motorCount); +bool pwmIsProcessingDshotCommand(void); +uint8_t pwmGetDshotCommand(uint8_t index); +bool pwmProcessDshotCommand(uint8_t motorCount); + #endif #ifdef USE_BEEPER diff --git a/src/main/drivers/pwm_output_dshot.c b/src/main/drivers/pwm_output_dshot.c index a7cdf9b176..43b94480fc 100644 --- a/src/main/drivers/pwm_output_dshot.c +++ b/src/main/drivers/pwm_output_dshot.c @@ -37,6 +37,7 @@ #endif #include "pwm_output.h" #include "drivers/nvic.h" +#include "drivers/time.h" #include "dma.h" #include "rcc.h" @@ -57,7 +58,7 @@ uint8_t getTimerIndex(TIM_TypeDef *timer) } } dmaMotorTimers[dmaMotorTimerCount++].timer = timer; - return dmaMotorTimerCount-1; + return dmaMotorTimerCount - 1; } void pwmWriteDshotInt(uint8_t index, uint16_t value) @@ -68,6 +69,12 @@ void pwmWriteDshotInt(uint8_t index, uint16_t value) return; } + /*If there is a command ready to go overwrite the value and send that instead*/ + if (pwmIsProcessingDshotCommand()) { + value = pwmGetDshotCommand(index); + motor->requestTelemetry = true; + } + uint16_t packet = prepareDshotPacket(motor, value); uint8_t bufferSize; @@ -89,6 +96,11 @@ void pwmCompleteDshotMotorUpdate(uint8_t motorCount) { UNUSED(motorCount); + /* If there is a dshot command loaded up, time it correctly with motor update*/ + if (!pwmProcessDshotCommand(motorCount)) { + return; //Skip motor update + } + for (int i = 0; i < dmaMotorTimerCount; i++) { #ifdef USE_DSHOT_DMAR if (useBurstDshot) { diff --git a/src/main/drivers/pwm_output_dshot_hal.c b/src/main/drivers/pwm_output_dshot_hal.c index 8e36a8ebd7..e0ba048639 100644 --- a/src/main/drivers/pwm_output_dshot_hal.c +++ b/src/main/drivers/pwm_output_dshot_hal.c @@ -30,6 +30,7 @@ #include "timer.h" #include "pwm_output.h" #include "drivers/nvic.h" +#include "drivers/time.h" #include "dma.h" #include "rcc.h" @@ -61,6 +62,12 @@ FAST_CODE void pwmWriteDshotInt(uint8_t index, uint16_t value) return; } + /*If there is a command ready to go overwrite the value and send that instead*/ + if (pwmIsProcessingDshotCommand()) { + value = pwmGetDshotCommand(index); + motor->requestTelemetry = true; + } + uint16_t packet = prepareDshotPacket(motor, value); uint8_t bufferSize; @@ -82,6 +89,13 @@ FAST_CODE void pwmCompleteDshotMotorUpdate(uint8_t motorCount) { UNUSED(motorCount); + /* If there is a dshot command loaded up, time it correctly with motor update*/ + if (pwmIsProcessingDshotCommand()) { + if (!pwmProcessDshotCommand(motorCount)) { + return; + } + } + for (int i = 0; i < dmaMotorTimerCount; i++) { #ifdef USE_DSHOT_DMAR if (useBurstDshot) { diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index bfcac3863b..748020edaa 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -329,10 +329,7 @@ void disarm(void) if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH) && !feature(FEATURE_3D)) { flipOverAfterCrashMode = false; if (!feature(FEATURE_3D)) { - pwmDisableMotors(); - delay(1); - pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL); - pwmEnableMotors(); + pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false); } } #endif @@ -357,13 +354,10 @@ void tryArm(void) } #ifdef USE_DSHOT if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH)) { - pwmDisableMotors(); - delay(1); - if (!IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { flipOverAfterCrashMode = false; if (!feature(FEATURE_3D)) { - pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL); + pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false); } } else { flipOverAfterCrashMode = true; @@ -371,11 +365,9 @@ void tryArm(void) runawayTakeoffCheckDisabled = false; #endif if (!feature(FEATURE_3D)) { - pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED); + pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED, false); } } - - pwmEnableMotors(); } #endif diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index bfc6daf3db..a1b884c2b8 100644 --- a/src/main/interface/cli.c +++ b/src/main/interface/cli.c @@ -2891,7 +2891,7 @@ static void executeEscInfoCommand(uint8_t escIndex) startEscDataRead(escInfoBuffer, ESC_INFO_BLHELI32_EXPECTED_FRAME_SIZE); - pwmWriteDshotCommand(escIndex, getMotorCount(), DSHOT_CMD_ESC_INFO); + pwmWriteDshotCommand(escIndex, getMotorCount(), DSHOT_CMD_ESC_INFO, true); delay(10); @@ -2938,7 +2938,7 @@ static void cliDshotProg(char *cmdline) } if (command != DSHOT_CMD_ESC_INFO) { - pwmWriteDshotCommand(escIndex, getMotorCount(), command); + pwmWriteDshotCommand(escIndex, getMotorCount(), command, true); } else { #if defined(USE_ESC_SENSOR) && defined(USE_ESC_SENSOR_INFO) if (feature(FEATURE_ESC_SENSOR)) { @@ -2958,9 +2958,6 @@ static void cliDshotProg(char *cmdline) cliPrintLinef("Command Sent: %d", command); - if (command <= 5) { - delay(20); // wait for sound output to finish - } } else { cliPrintErrorLinef("Invalid command. Range: 1 - %d.", DSHOT_MIN_THROTTLE - 1); } diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index cd134c96e1..f0653f7097 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -394,12 +394,7 @@ void beeperUpdate(timeUs_t currentTimeUs) if (!areMotorsRunning() && ((currentBeeperEntry->mode == BEEPER_RX_SET && beeperConfig()->dshotBeaconOffFlags & BEEPER_GET_FLAG(BEEPER_RX_SET)) || (currentBeeperEntry->mode == BEEPER_RX_LOST && beeperConfig()->dshotBeaconOffFlags & BEEPER_GET_FLAG(BEEPER_RX_LOST)))) { - pwmDisableMotors(); - delay(1); - - pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), beeperConfig()->dshotBeaconTone); - - pwmEnableMotors(); + pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), beeperConfig()->dshotBeaconTone, false); } #endif