Refactor dshot command output scheduling logic

Change to a state machine that tracks the progress of each dshot command in the queue as it moves through the various phases. Simplifies the code to make it easier to understand and maintain.

Transition to timing based on motor output cycle counts calculated from desired delays instead of using direct time comparisons. Since the output timing is always based on the motor update schedule, there were cases where if the time between motor updates was a significant percentage of the desired dshot command timing, then the output could get irregular and skip cycles (for example trying to use 2K pid loop with 500us timing conflicting with 1ms timing between dshot command outputs).

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) {
+    switch (command->state) {
+    case DSHOT_COMMAND_STATE_IDLEWAIT:
         if (allMotorsAreIdle(motorCount)) {
-                command->nextCommandAtUs = timeNowUs + DSHOT_INITIAL_DELAY_US;
-                command->waitingForIdle = false;
+            command->state = DSHOT_COMMAND_STATE_STARTDELAY;
+            command->nextCommandCycleDelay = dshotCommandCyclesFromTime(DSHOT_INITIAL_DELAY_US);
         }
-            // Send normal motor output while waiting for motors to go idle
-            return true;
+        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
+        command->repeats--;
+        if (command->repeats) {
+            command->nextCommandCycleDelay = dshotCommandCyclesFromTime(DSHOT_COMMAND_DELAY_US);
+        } else {
+            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;
         }
-  
-    //Timed motor update happening with dshot command
-    if (command->repeats > 0) {
-        command->repeats--;
-
-        if (command->repeats > 0) {
-            command->nextCommandAtUs = timeNowUs + DSHOT_COMMAND_DELAY_US;
-        } else {
-            command->nextCommandAtUs = timeNowUs + command->delayAfterCommandUs;
-        }
-    } else {
-        command->nextCommandAtUs = 0;
     }
 
     return true;

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

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)

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)

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"
 
@@ -223,6 +224,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;