Add dshot command queue and limit max notch filter frequency

src/main/drivers/pwm_output.c

@@ -44,6 +44,7 @@ FAST_RAM_ZERO_INIT loadDmaBufferFn *loadDmaBuffer;
 #define DSHOT_COMMAND_DELAY_US 1000
 #define DSHOT_ESCINFO_DELAY_US 12000
 #define DSHOT_BEEP_DELAY_US 100000
+#define DSHOT_MAX_COMMANDS 3
 
 typedef struct dshotCommandControl_s {
     timeUs_t nextCommandAtUs;
@@ -53,7 +54,9 @@ typedef struct dshotCommandControl_s {
     uint8_t command[MAX_SUPPORTED_MOTORS];
 } dshotCommandControl_t;
 
-static dshotCommandControl_t dshotCommandControl;
+static dshotCommandControl_t commandQueue[DSHOT_MAX_COMMANDS + 1];
+static uint8_t commandQueueHead;
+static uint8_t commandQueueTail;
 #endif
 
 #ifdef USE_SERVOS
@@ -436,21 +439,53 @@ bool allMotorsAreIdle(uint8_t motorCount)
     return allMotorsIdle;
 }
 
+FAST_CODE bool pwmDshotCommandQueueFull()
+{
+    return (commandQueueHead + 1) % (DSHOT_MAX_COMMANDS + 1) == commandQueueTail;
+}
+
 FAST_CODE bool pwmDshotCommandIsQueued(void)
 {
-    return dshotCommandControl.nextCommandAtUs;
+    return commandQueueHead != commandQueueTail;
 }
 
 FAST_CODE bool pwmDshotCommandIsProcessing(void)
 {
-    return dshotCommandControl.nextCommandAtUs && !dshotCommandControl.waitingForIdle && dshotCommandControl.repeats > 0;
+    if (!pwmDshotCommandIsQueued()) {
+        return false;
+    }
+    dshotCommandControl_t* command = &commandQueue[commandQueueTail];
+    return command->nextCommandAtUs && !command->waitingForIdle && command->repeats > 0;
 }
 
+FAST_CODE void pwmDshotCommandQueueUpdate(void)
+{
+    if (!pwmDshotCommandIsQueued()) {
+        return;
+    }
+    dshotCommandControl_t* command = &commandQueue[commandQueueTail];
+    if (!command->nextCommandAtUs && !command->waitingForIdle && !command->repeats) {
+        commandQueueTail = (commandQueueTail + 1) % (DSHOT_MAX_COMMANDS + 1);
+    }
+}
+
+static dshotCommandControl_t* addCommand()
+{
+    int newHead = (commandQueueHead + 1) % (DSHOT_MAX_COMMANDS + 1);
+    if (newHead == commandQueueTail) {
+        return NULL;
+    }
+    dshotCommandControl_t* control = &commandQueue[commandQueueHead];
+    commandQueueHead = newHead;
+    return control;
+}
+
+
 void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking)
 {
     timeUs_t timeNowUs = micros();
 
-    if (!isMotorProtocolDshot() || (command > DSHOT_MAX_COMMAND) || pwmDshotCommandIsQueued()) {
+    if (!isMotorProtocolDshot() || (command > DSHOT_MAX_COMMAND) || pwmDshotCommandQueueFull()) {
         return;
     }
 
@@ -500,56 +535,60 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo
         }
         delayMicroseconds(delayAfterCommandUs);
     } else {
-        dshotCommandControl.repeats = repeats;
+        dshotCommandControl_t *commandControl = addCommand();
-        dshotCommandControl.nextCommandAtUs = timeNowUs + DSHOT_INITIAL_DELAY_US;
+        if (commandControl) {
-        dshotCommandControl.delayAfterCommandUs = delayAfterCommandUs;
+            commandControl->repeats = repeats;
-        for (unsigned i = 0; i < motorCount; i++) {
+            commandControl->nextCommandAtUs = timeNowUs + DSHOT_INITIAL_DELAY_US;
-            if (index == i || index == ALL_MOTORS) {
+            commandControl->delayAfterCommandUs = delayAfterCommandUs;
-                dshotCommandControl.command[i] = command;
+            for (unsigned i = 0; i < motorCount; i++) {
-            } else {
+                if (index == i || index == ALL_MOTORS) {
-                dshotCommandControl.command[i] = command;
+                    commandControl->command[i] = command;
+                } else {
+                    commandControl->command[i] = command;
+                }
             }
+            commandControl->waitingForIdle = !allMotorsAreIdle(motorCount);
         }
-
-        dshotCommandControl.waitingForIdle = !allMotorsAreIdle(motorCount);
     }
 }
 
 uint8_t pwmGetDshotCommand(uint8_t index)
 {
-    return dshotCommandControl.command[index];
+    return commandQueue[commandQueueTail].command[index];
 }
 
 FAST_CODE_NOINLINE bool pwmDshotCommandOutputIsEnabled(uint8_t motorCount)
 {
     timeUs_t timeNowUs = micros();
 
-    if (dshotCommandControl.waitingForIdle) {
+    dshotCommandControl_t* command = &commandQueue[commandQueueTail];
-        if (allMotorsAreIdle(motorCount)) {
+    if (pwmDshotCommandIsQueued()) {
-            dshotCommandControl.nextCommandAtUs = timeNowUs + DSHOT_INITIAL_DELAY_US;
+        if (command->waitingForIdle) {
-            dshotCommandControl.waitingForIdle = false;
+            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;
         }
-
-        // Send normal motor output while waiting for motors to go idle
-        return true;
     }
 
-    if (cmpTimeUs(timeNowUs, dshotCommandControl.nextCommandAtUs) < 0) {
+    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 (dshotCommandControl.repeats > 0) {
+    if (command->repeats > 0) {
-        dshotCommandControl.repeats--;
+        command->repeats--;
 
-        if (dshotCommandControl.repeats > 0) {
+        if (command->repeats > 0) {
-            dshotCommandControl.nextCommandAtUs = timeNowUs + DSHOT_COMMAND_DELAY_US;
+            command->nextCommandAtUs = timeNowUs + DSHOT_COMMAND_DELAY_US;
         } else {
-            dshotCommandControl.nextCommandAtUs = timeNowUs + dshotCommandControl.delayAfterCommandUs;
+            command->nextCommandAtUs = timeNowUs + command->delayAfterCommandUs;
         }
     } else {
-        dshotCommandControl.nextCommandAtUs = 0;
+        command->nextCommandAtUs = 0;
     }
 
     return true;

src/main/drivers/pwm_output.h

@@ -237,6 +237,7 @@ void 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);

src/main/drivers/pwm_output_dshot.c

@@ -324,6 +324,7 @@ 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

@@ -120,6 +120,7 @@ FAST_CODE void pwmCompleteDshotMotorUpdate(uint8_t motorCount)
             dmaMotorTimers[i].timerDmaSources = 0;
         }
     }
+    pwmDshotCommandQueueUpdate();
 }
 
 static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)

src/main/fc/core.c

@@ -382,14 +382,6 @@ void tryArm(void)
         const timeUs_t currentTimeUs = micros();
 
 #ifdef USE_DSHOT
-#ifdef USE_DSHOT_TELEMETRY
-        pwmWriteDshotCommand(
-            255, getMotorCount(),
-            motorConfig()->dev.useDshotTelemetry ?
-            DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY :
-            DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE, true);
-#endif
-
         if (currentTimeUs - getLastDshotBeaconCommandTimeUs() < DSHOT_BEACON_GUARD_DELAY_US) {
             if (tryingToArm == ARMING_DELAYED_DISARMED) {
                 if (IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
@@ -404,6 +396,15 @@ void tryArm(void)
             }
             return;
         }
+
+#ifdef USE_DSHOT_TELEMETRY
+        if (isMotorProtocolDshot()) {
+            pwmWriteDshotCommand(
+                255, getMotorCount(), motorConfig()->dev.useDshotTelemetry ?
+                DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY : DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE, false);
+        }
+#endif
+
         if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH)) {
             if (!(IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) || (tryingToArm == ARMING_DELAYED_CRASHFLIP))) {
                 flipOverAfterCrashActive = false;

src/main/fc/init.c

@@ -214,10 +214,8 @@ void activateDshotTelemetry(struct dispatchEntry_s* self)
     if (!ARMING_FLAG(ARMED))
     {
         pwmWriteDshotCommand(
-            255, getMotorCount(),
+            255, getMotorCount(), motorConfig()->dev.useDshotTelemetry ?
-            motorConfig()->dev.useDshotTelemetry ?
+            DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY : DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE, false);
-            DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY :
-            DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE, true);
     }
 }
 
@@ -772,8 +770,10 @@ void init(void)
 
 // TODO: potentially delete when feature is stable. Activation when arming is enough for flight.
 #if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
-    dispatchEnable();
+    if (motorConfig()->dev.useDshotTelemetry) {
-    dispatchAdd(&activateDshotTelemetryEntry, 5000000);
+        dispatchEnable();
+        dispatchAdd(&activateDshotTelemetryEntry, 5000000);
+    }
 #endif
 
     fcTasksInit();

src/main/sensors/rpm_filter.c

@@ -177,6 +177,9 @@ void rpmFilterUpdate()
                 frequency = currentFilter->minHz;
             }
         }
+        if (frequency > deactivateFreq) {
+            frequency = deactivateFreq;
+        }
 
         biquadFilter_t* template = &currentFilter->notch[0][motor][harmonic];
         // uncomment below to debug filter stepping. Need to also comment out motor rpm DEBUG_SET above

src/main/target/common_post.h

@@ -247,3 +247,7 @@
 #ifndef USE_CMS
 #undef USE_CMS_FAILSAFE_MENU
 #endif
+
+#ifndef USE_DSHOT_TELEMETRY
+#undef USE_RPM_FILTER
+#endif