Merge pull request #6014 from mikeller/fix_dshot_beacon_interference

Made Dshot commands work in a non-blocking way.

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,10 +386,17 @@ 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)) {
+    timeUs_t timeNowUs = micros();
-        unsigned repeats;
+
+    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:
@@ -387,14 +406,24 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command)
     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:
-            repeats = 1;
         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,9 +432,55 @@ 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)

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

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) {

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) {

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();
+                pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false);
-                delay(1);
-                pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL);
-                pwmEnableMotors();
             }
         }
 #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
 

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);
                 }

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();
+            pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), beeperConfig()->dshotBeaconTone, false);
-            delay(1);
-
-            pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), beeperConfig()->dshotBeaconTone);
-
-            pwmEnableMotors();
         }
 #endif