diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 7162f341d8..2f9931e95a 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -46,14 +46,24 @@ FAST_RAM_ZERO_INIT loadDmaBufferFn *loadDmaBuffer; #define DSHOT_BEEP_DELAY_US 100000 #define DSHOT_MAX_COMMANDS 3 +typedef enum { + DSHOT_COMMAND_STATE_IDLEWAIT, // waiting for motors to go idle + DSHOT_COMMAND_STATE_STARTDELAY, // initial delay period before a sequence of commands + DSHOT_COMMAND_STATE_ACTIVE, // actively sending the command (with optional repeated output) + DSHOT_COMMAND_STATE_POSTDELAY // delay period after the command has been sent +} dshotCommandState_e; + typedef struct dshotCommandControl_s { - timeUs_t nextCommandAtUs; + dshotCommandState_e state; + uint32_t nextCommandCycleDelay; timeUs_t delayAfterCommandUs; - bool waitingForIdle; uint8_t repeats; uint8_t command[MAX_SUPPORTED_MOTORS]; } dshotCommandControl_t; +static timeUs_t dshotCommandPidLoopTimeUs = 125; // default to 8KHz (125us) loop to prevent possible div/0 + // gets set to the actual value when the PID loop is initialized + static dshotCommandControl_t commandQueue[DSHOT_MAX_COMMANDS + 1]; static uint8_t commandQueueHead; static uint8_t commandQueueTail; @@ -189,6 +199,11 @@ static uint8_t loadDmaBufferProshot(uint32_t *dmaBuffer, int stride, uint16_t pa return PROSHOT_DMA_BUFFER_SIZE; } + +void setDshotPidLoopTime(uint32_t pidLoopTime) +{ + dshotCommandPidLoopTimeUs = pidLoopTime; +} #endif void pwmWriteMotor(uint8_t index, float value) @@ -450,34 +465,49 @@ FAST_CODE bool pwmDshotCommandIsQueued(void) return commandQueueHead != commandQueueTail; } +static FAST_CODE bool isLastDshotCommand(void) +{ + return ((commandQueueTail + 1) % (DSHOT_MAX_COMMANDS + 1) == commandQueueHead); +} + FAST_CODE bool pwmDshotCommandIsProcessing(void) { if (!pwmDshotCommandIsQueued()) { return false; } dshotCommandControl_t* command = &commandQueue[commandQueueTail]; - // Check if this is the last command in the queue. If not then we want to - // keep sending the command instead of falling back to the motor value to - // prevent a motor command frame to slip through between the this and the next command. - const bool isLastCommand = (commandQueueTail + 1) % (DSHOT_MAX_COMMANDS + 1) == commandQueueHead; - return (command->nextCommandAtUs && !command->waitingForIdle && command->repeats > 0) || !isLastCommand; + const bool commandIsProcessing = command->state == DSHOT_COMMAND_STATE_STARTDELAY + || command->state == DSHOT_COMMAND_STATE_ACTIVE + || (command->state == DSHOT_COMMAND_STATE_POSTDELAY && !isLastDshotCommand()); + return commandIsProcessing; } -FAST_CODE void pwmDshotCommandQueueUpdate(void) +static FAST_CODE bool pwmDshotCommandQueueUpdate(void) { - if (!pwmDshotCommandIsQueued()) { - return; - } - dshotCommandControl_t* command = &commandQueue[commandQueueTail]; - if (!command->nextCommandAtUs && !command->waitingForIdle && !command->repeats) { + if (pwmDshotCommandIsQueued()) { commandQueueTail = (commandQueueTail + 1) % (DSHOT_MAX_COMMANDS + 1); if (pwmDshotCommandIsQueued()) { - // there's another command in the queue + // There is another command in the queue so update it so it's ready to output in + // sequence. It can go directly to the DSHOT_COMMAND_STATE_ACTIVE state and bypass + // the DSHOT_COMMAND_STATE_IDLEWAIT and DSHOT_COMMAND_STATE_STARTDELAY states. dshotCommandControl_t* nextCommand = &commandQueue[commandQueueTail]; - nextCommand->waitingForIdle = false; - nextCommand->nextCommandAtUs = micros() + command->delayAfterCommandUs; + nextCommand->state = DSHOT_COMMAND_STATE_ACTIVE; + nextCommand->nextCommandCycleDelay = 0; + return true; } } + return false; +} + +static FAST_CODE uint32_t dshotCommandCyclesFromTime(timeUs_t delayUs) +{ + // Find the minimum number of motor output cycles needed to + // provide at least delayUs time delay + uint32_t ret = delayUs / dshotCommandPidLoopTimeUs; + if (delayUs % dshotCommandPidLoopTimeUs) { + ret++; + } + return ret; } static dshotCommandControl_t* addCommand() @@ -494,8 +524,6 @@ static dshotCommandControl_t* addCommand() void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking) { - timeUs_t timeNowUs = micros(); - if (!isMotorProtocolDshot() || (command > DSHOT_MAX_COMMAND) || pwmDshotCommandQueueFull()) { return; } @@ -551,7 +579,6 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo dshotCommandControl_t *commandControl = addCommand(); if (commandControl) { commandControl->repeats = repeats; - commandControl->nextCommandAtUs = timeNowUs + DSHOT_INITIAL_DELAY_US; commandControl->delayAfterCommandUs = delayAfterCommandUs; for (unsigned i = 0; i < motorCount; i++) { if (index == i || index == ALL_MOTORS) { @@ -560,7 +587,14 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo commandControl->command[i] = DSHOT_CMD_MOTOR_STOP; } } - commandControl->waitingForIdle = !allMotorsAreIdle(motorCount); + if (allMotorsAreIdle(motorCount)) { + // we can skip the motors idle wait state + commandControl->state = DSHOT_COMMAND_STATE_STARTDELAY; + commandControl->nextCommandCycleDelay = dshotCommandCyclesFromTime(DSHOT_INITIAL_DELAY_US); + } else { + commandControl->state = DSHOT_COMMAND_STATE_STARTDELAY; + commandControl->nextCommandCycleDelay = 0; // will be set after idle wait completes + } } } } @@ -570,38 +604,58 @@ uint8_t pwmGetDshotCommand(uint8_t index) return commandQueue[commandQueueTail].command[index]; } +// This function is used to synchronize the dshot command output timing with +// the normal motor output timing tied to the PID loop frequency. A "true" result +// allows the motor output to be sent, "false" means delay until next loop. So take +// the example of a dshot command that needs to repeat 10 times at 1ms intervals. +// If we have a 8KHz PID loop we'll end up sending the dshot command every 8th motor output. FAST_CODE_NOINLINE bool pwmDshotCommandOutputIsEnabled(uint8_t motorCount) { - timeUs_t timeNowUs = micros(); - dshotCommandControl_t* command = &commandQueue[commandQueueTail]; - if (pwmDshotCommandIsQueued()) { - if (command->waitingForIdle) { - if (allMotorsAreIdle(motorCount)) { - command->nextCommandAtUs = timeNowUs + DSHOT_INITIAL_DELAY_US; - command->waitingForIdle = false; - } - // Send normal motor output while waiting for motors to go idle - return true; + switch (command->state) { + case DSHOT_COMMAND_STATE_IDLEWAIT: + if (allMotorsAreIdle(motorCount)) { + command->state = DSHOT_COMMAND_STATE_STARTDELAY; + command->nextCommandCycleDelay = dshotCommandCyclesFromTime(DSHOT_INITIAL_DELAY_US); + } + break; + + case DSHOT_COMMAND_STATE_STARTDELAY: + if (command->nextCommandCycleDelay-- > 1) { + return false; // Delay motor output until the start of the command seequence + } + command->state = DSHOT_COMMAND_STATE_ACTIVE; + command->nextCommandCycleDelay = 0; // first iteration of the repeat happens now + FALLTHROUGH; + + case DSHOT_COMMAND_STATE_ACTIVE: + if (command->nextCommandCycleDelay-- > 1) { + return false; // Delay motor output until the next command repeat } - } - if (cmpTimeUs(timeNowUs, command->nextCommandAtUs) < 0) { - //Skip motor update because it isn't time yet for a new command - return false; - } - - //Timed motor update happening with dshot command - if (command->repeats > 0) { command->repeats--; - - if (command->repeats > 0) { - command->nextCommandAtUs = timeNowUs + DSHOT_COMMAND_DELAY_US; + if (command->repeats) { + command->nextCommandCycleDelay = dshotCommandCyclesFromTime(DSHOT_COMMAND_DELAY_US); } else { - command->nextCommandAtUs = timeNowUs + command->delayAfterCommandUs; + command->state = DSHOT_COMMAND_STATE_POSTDELAY; + command->nextCommandCycleDelay = dshotCommandCyclesFromTime(command->delayAfterCommandUs); + if (!isLastDshotCommand() && command->nextCommandCycleDelay > 0) { + // Account for the 1 extra motor output loop between commands. + // Otherwise the inter-command delay will be DSHOT_COMMAND_DELAY_US + 1 loop. + command->nextCommandCycleDelay--; + } + } + break; + + case DSHOT_COMMAND_STATE_POSTDELAY: + if (command->nextCommandCycleDelay-- > 1) { + return false; // Delay motor output until the end of the post-command delay + } + if (pwmDshotCommandQueueUpdate()) { + // Will be true if the command queue is not empty and we + // want to wait for the next command to start in sequence. + return false; } - } else { - command->nextCommandAtUs = 0; } return true; diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 87244aad0b..f5da5d162a 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -254,14 +254,13 @@ bool pwmStartDshotMotorUpdate(uint8_t motorCount); #endif void pwmCompleteDshotMotorUpdate(uint8_t motorCount); -void pwmDshotCommandQueueUpdate(void); bool pwmDshotCommandIsQueued(void); bool pwmDshotCommandIsProcessing(void); uint8_t pwmGetDshotCommand(uint8_t index); bool pwmDshotCommandOutputIsEnabled(uint8_t motorCount); uint16_t getDshotTelemetry(uint8_t index); bool isDshotMotorTelemetryActive(uint8_t motorIndex); - +void setDshotPidLoopTime(uint32_t pidLoopTime); #endif #ifdef USE_BEEPER diff --git a/src/main/drivers/pwm_output_dshot.c b/src/main/drivers/pwm_output_dshot.c index ed895710ea..b68e359607 100644 --- a/src/main/drivers/pwm_output_dshot.c +++ b/src/main/drivers/pwm_output_dshot.c @@ -169,7 +169,6 @@ void pwmCompleteDshotMotorUpdate(uint8_t motorCount) dmaMotorTimers[i].timerDmaSources = 0; } } - pwmDshotCommandQueueUpdate(); } static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) diff --git a/src/main/drivers/pwm_output_dshot_hal.c b/src/main/drivers/pwm_output_dshot_hal.c index 3c3c8f770b..e61fc0c8ba 100644 --- a/src/main/drivers/pwm_output_dshot_hal.c +++ b/src/main/drivers/pwm_output_dshot_hal.c @@ -153,7 +153,6 @@ FAST_CODE void pwmCompleteDshotMotorUpdate(uint8_t motorCount) dmaMotorTimers[i].timerDmaSources = 0; } } - pwmDshotCommandQueueUpdate(); } static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 32beb4b8ac..8a54114225 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -34,6 +34,7 @@ #include "config/config_reset.h" +#include "drivers/pwm_output.h" #include "drivers/sound_beeper.h" #include "drivers/time.h" @@ -222,6 +223,9 @@ static void pidSetTargetLooptime(uint32_t pidLooptime) targetPidLooptime = pidLooptime; dT = targetPidLooptime * 1e-6f; pidFrequency = 1.0f / dT; +#ifdef USE_DSHOT + setDshotPidLoopTime(targetPidLooptime); +#endif } static FAST_RAM float itermAccelerator = 1.0f;