Merge pull request #6130 from mikeller/fix_dshot_commands_2

Stop Dshot commands from running when ESCs not disarmed.

src/main/drivers/pwm_output.c

@@ -37,18 +37,20 @@ static FAST_RAM_ZERO_INIT pwmCompleteWriteFn *pwmCompleteWrite = NULL;
 
 #ifdef USE_DSHOT
 FAST_RAM_ZERO_INIT loadDmaBufferFn *loadDmaBuffer;
+#define DSHOT_INITIAL_DELAY_US 10000
 #define DSHOT_COMMAND_DELAY_US 1000
-#define DSHOT_ESCINFO_DELAY_US 5000
+#define DSHOT_ESCINFO_DELAY_US 12000
 #define DSHOT_BEEP_DELAY_US 100000
 
 typedef struct dshotCommandControl_s {
-    timeUs_t nextCommandAt; 
+    timeUs_t nextCommandAtUs;
-    timeUs_t delayAfterCommand; 
+    timeUs_t delayAfterCommandUs;
-    uint8_t command[MAX_SUPPORTED_MOTORS];
+    bool waitingForIdle;
     uint8_t repeats;
+    uint8_t command[MAX_SUPPORTED_MOTORS];
 } dshotCommandControl_t;
 
-dshotCommandControl_t dshotCommandControl;
+static dshotCommandControl_t dshotCommandControl;
 #endif
 
 #ifdef USE_SERVOS
@@ -386,21 +388,39 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
     }
 }
 
-FAST_CODE bool pwmIsProcessingDshotCommand(void)
+bool allMotorsAreIdle(uint8_t motorCount)
 {
-    return dshotCommandControl.nextCommandAt;
+    bool allMotorsIdle = true;
+    for (unsigned i = 0; i < motorCount; i++) {
+        const motorDmaOutput_t *motor = getMotorDmaOutput(i);
+        if (motor->value) {
+            allMotorsIdle = false;
+        }
+    }
+
+    return allMotorsIdle;
+}
+
+FAST_CODE bool pwmDshotCommandIsQueued(void)
+{
+    return dshotCommandControl.nextCommandAtUs;
+}
+
+FAST_CODE bool pwmDshotCommandIsProcessing(void)
+{
+    return dshotCommandControl.nextCommandAtUs && !dshotCommandControl.waitingForIdle && dshotCommandControl.repeats > 0;
 }
 
 void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking)
 {
     timeUs_t timeNowUs = micros();
 
-    if (!isMotorProtocolDshot() || (command > DSHOT_MAX_COMMAND)) {   
+    if (!isMotorProtocolDshot() || (command > DSHOT_MAX_COMMAND) || pwmDshotCommandIsQueued()) {
         return;
     }
 
     uint8_t repeats = 1;
-    timeUs_t timeDelayUs = DSHOT_COMMAND_DELAY_US;
+    timeUs_t delayAfterCommandUs = DSHOT_COMMAND_DELAY_US;
 
     switch (command) {
     case DSHOT_CMD_SPIN_DIRECTION_1:
@@ -411,24 +431,24 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo
     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;
+        delayAfterCommandUs = DSHOT_BEEP_DELAY_US;
-        timeDelayUs = DSHOT_BEEP_DELAY_US;
         break;
     default:
         break;
     }
 
     if (blocking) {
+        delayMicroseconds(DSHOT_INITIAL_DELAY_US - DSHOT_COMMAND_DELAY_US);
         for (; repeats; repeats--) {
-            for (uint8_t i = 0; i < motorCount; i++) {
             delayMicroseconds(DSHOT_COMMAND_DELAY_US);
+
+            for (uint8_t i = 0; i < motorCount; i++) {
                 if ((i == index) || (index == ALL_MOTORS)) {
                     motorDmaOutput_t *const motor = getMotorDmaOutput(i);
                     motor->requestTelemetry = true;
@@ -438,18 +458,20 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo
 
             pwmCompleteDshotMotorUpdate(0);
         }
-        delayMicroseconds(timeDelayUs);
+        delayMicroseconds(delayAfterCommandUs);
     } 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.nextCommandAtUs = timeNowUs + DSHOT_INITIAL_DELAY_US;
-                    dshotCommandControl.delayAfterCommand = timeDelayUs;
+        dshotCommandControl.delayAfterCommandUs = delayAfterCommandUs;
-                }
+        for (unsigned i = 0; i < motorCount; i++) {
+            if (index == i || index == ALL_MOTORS) {
+                dshotCommandControl.command[i] = command;
+            } else {
+                dshotCommandControl.command[i] = command;
             }
         }
+
+        dshotCommandControl.waitingForIdle = !allMotorsAreIdle(motorCount);
     }
 }
 
@@ -458,34 +480,44 @@ uint8_t pwmGetDshotCommand(uint8_t index)
     return dshotCommandControl.command[index];
 }
 
-FAST_CODE_NOINLINE bool pwmProcessDshotCommand(uint8_t motorCount)
+FAST_CODE_NOINLINE bool pwmDshotCommandOutputIsEnabled(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
+    if (dshotCommandControl.waitingForIdle) {
+        if (allMotorsAreIdle(motorCount)) {
+            dshotCommandControl.nextCommandAtUs = timeNowUs + DSHOT_INITIAL_DELAY_US;
+            dshotCommandControl.waitingForIdle = false;
+        }
+
+        // Send normal motor output while waiting for motors to go idle
+        return true;
+    }
+
+    if (cmpTimeUs(timeNowUs, dshotCommandControl.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) {
         dshotCommandControl.repeats--;
-        dshotCommandControl.nextCommandAt = timeNowUs + DSHOT_COMMAND_DELAY_US;
+
-        if (dshotCommandControl.repeats == 0) { 
+        if (dshotCommandControl.repeats > 0) {
-            dshotCommandControl.nextCommandAt = timeNowUs + dshotCommandControl.delayAfterCommand;
+            dshotCommandControl.nextCommandAtUs = timeNowUs + DSHOT_COMMAND_DELAY_US;
+        } else {
+            dshotCommandControl.nextCommandAtUs = timeNowUs + dshotCommandControl.delayAfterCommandUs;
         }
     } else {
-        for (uint8_t i = 0; i < motorCount; i++) {
+        dshotCommandControl.nextCommandAtUs = 0;
-            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)
+FAST_CODE uint16_t prepareDshotPacket(motorDmaOutput_t *const motor)
 {
-    uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0);
+    uint16_t packet = (motor->value << 1) | (motor->requestTelemetry ? 1 : 0);
     motor->requestTelemetry = false;    // reset telemetry request to make sure it's triggered only once in a row
 
     // compute checksum

src/main/drivers/pwm_output.h

@@ -191,7 +191,7 @@ bool isMotorProtocolDshot(void);
 #ifdef USE_DSHOT
 typedef uint8_t loadDmaBufferFn(uint32_t *dmaBuffer, int stride, uint16_t packet);  // function pointer used to encode a digital motor value into the DMA buffer representation
 
-uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, uint16_t value);
+uint16_t prepareDshotPacket(motorDmaOutput_t *const motor);
 
 extern loadDmaBufferFn *loadDmaBuffer;
 
@@ -201,9 +201,11 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo
 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);
+
+bool pwmDshotCommandIsQueued(void);
+bool pwmDshotCommandIsProcessing(void);
 uint8_t pwmGetDshotCommand(uint8_t index);
-bool pwmProcessDshotCommand(uint8_t motorCount);
+bool pwmDshotCommandOutputIsEnabled(uint8_t motorCount);
 
 #endif
 

src/main/drivers/pwm_output_dshot.c

@@ -70,12 +70,16 @@ void pwmWriteDshotInt(uint8_t index, uint16_t value)
     }
 
     /*If there is a command ready to go overwrite the value and send that instead*/
-    if (pwmIsProcessingDshotCommand()) {
+    if (pwmDshotCommandIsProcessing()) {
         value = pwmGetDshotCommand(index);
+        if (value) {
             motor->requestTelemetry = true;
         }
+    }
 
-    uint16_t packet = prepareDshotPacket(motor, value);
+    motor->value = value;
+
+    uint16_t packet = prepareDshotPacket(motor);
     uint8_t bufferSize;
 
 #ifdef USE_DSHOT_DMAR
@@ -97,8 +101,8 @@ void pwmCompleteDshotMotorUpdate(uint8_t motorCount)
     UNUSED(motorCount);
 
     /* If there is a dshot command loaded up, time it correctly with motor update*/
-    if (pwmIsProcessingDshotCommand()) {
+    if (pwmDshotCommandIsQueued()) {
-        if (!pwmProcessDshotCommand(motorCount)) {
+        if (!pwmDshotCommandOutputIsEnabled(motorCount)) {
             return;
         }
     }

src/main/drivers/pwm_output_dshot_hal.c

@@ -63,12 +63,16 @@ FAST_CODE void pwmWriteDshotInt(uint8_t index, uint16_t value)
     }
 
     /*If there is a command ready to go overwrite the value and send that instead*/
-    if (pwmIsProcessingDshotCommand()) {
+    if (pwmDshotCommandIsProcessing()) {
         value = pwmGetDshotCommand(index);
+        if (value) {
             motor->requestTelemetry = true;
         }
+    }
 
-    uint16_t packet = prepareDshotPacket(motor, value);
+    motor->value = value;
+
+    uint16_t packet = prepareDshotPacket(motor);
     uint8_t bufferSize;
 
 #ifdef USE_DSHOT_DMAR
@@ -90,8 +94,8 @@ 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 (pwmDshotCommandIsQueued()) {
-        if (!pwmProcessDshotCommand(motorCount)) {
+        if (!pwmDshotCommandOutputIsEnabled(motorCount)) {
             return;
         }
     }