Send motor data and then immediately decode prior telemetry data for bitbanged DSHOT (#12685)

* Send motor data and then immediately decode prior telemetry data for bitbanged DSHOT
F4 can't handle 8K PID loop

* Fix missing cfg_pms.version

* Limit G4 to 4K PID rate as per F4

src/main/config/config.c

@@ -670,8 +670,8 @@ void validateAndFixGyroConfig(void)
         // check for looptime restrictions based on motor protocol. Motor times have safety margin
         float motorUpdateRestriction;
 
-#if defined(STM32F411xE)
+#if defined(STM32F4) || defined(STM32G4)
-        /* If bidirectional DSHOT is being used on an F411 then force DSHOT300. The motor update restrictions then applied
+        /* If bidirectional DSHOT is being used on an F4 or G4 then force DSHOT300. The motor update restrictions then applied
          * will automatically consider the loop time and adjust pid_process_denom appropriately
          */
         if (motorConfig()->dev.useDshotTelemetry && (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_DSHOT600)) {

src/main/drivers/at32/exti_at32.c

@@ -183,4 +183,4 @@ _EXTI_IRQ_HANDLER(EXINT4_IRQHandler, 0x0010);
 _EXTI_IRQ_HANDLER(EXINT9_5_IRQHandler, 0x03e0);
 _EXTI_IRQ_HANDLER(EXINT15_10_IRQHandler, 0xfc00);
 
-#endif
+#endif

src/main/drivers/at32/pwm_output_at32bsp.c

@@ -184,7 +184,7 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
     }
 
     motorPwmDevice.vTable.write = pwmWriteStandard;
-    motorPwmDevice.vTable.updateStart = motorUpdateStartNull;
+    motorPwmDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
     motorPwmDevice.vTable.updateComplete = useUnsyncedPwm ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate;
 
     for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {

src/main/drivers/at32/pwm_output_dshot.c

@@ -112,7 +112,7 @@ tmr_channel_select_type toCHSelectType(const uint8_t bfChannel, const bool useNC
  * Enable the timer channels for all motors
  * 
  *   Called once for every dshot update if telemetry is being used (not just enabled by #def)
- *   Called from pwm_output_dshot_shared.c pwmStartDshotMotorUpdate
+ *   Called from pwm_output_dshot_shared.c pwmTelemetryDecode
 */
 void dshotEnableChannels(uint8_t motorCount)
 {
@@ -130,7 +130,7 @@ void dshotEnableChannels(uint8_t motorCount)
  * Set the timer and dma of the specified motor for use as an output
  * 
  * Called from pwmDshotMotorHardwareConfig in this file and also from
- * pwmStartDshotMotorUpdate in src/main/drivers/pwm_output_dshot_shared.c
+ * pwmTelemetryDecode in src/main/drivers/pwm_output_dshot_shared.c
 */
 FAST_CODE void pwmDshotSetDirectionOutput(
     motorDmaOutput_t * const motor

src/main/drivers/at32/serial_usb_vcp_at32f4.c

@@ -519,4 +519,4 @@ uint32_t usbVcpGetBaudRate(serialPort_t *instance)
     return pcdc->linecoding.bitrate;
 }
 
-#endif
+#endif

src/main/drivers/dshot_bitbang_decode.c

@@ -268,7 +268,7 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
     startMargin = p - buffer;
     DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 3, startMargin);
 
-    if(*p & mask) {
+    if (p >= endP) {
         // not returning telemetry is ok if the esc cpu is
         // overburdened.  in that case no edge will be found and
         // BB_NOEDGE indicates the condition to caller

src/main/drivers/dshot_command.c

@@ -218,7 +218,7 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshot
 
 #ifdef USE_DSHOT_TELEMETRY
             timeUs_t timeoutUs = micros() + 1000;
-            while (!motorGetVTable().updateStart() &&
+            while (!motorGetVTable().decodeTelemetry() &&
                    cmpTimeUs(timeoutUs, micros()) > 0);
 #endif
             for (uint8_t i = 0; i < motorDeviceCount(); i++) {

src/main/drivers/dshot_dpwm.c

@@ -140,7 +140,7 @@ static motorVTable_t dshotPwmVTable = {
     .enable = dshotPwmEnableMotors,
     .disable = dshotPwmDisableMotors,
     .isMotorEnabled = dshotPwmIsMotorEnabled,
-    .updateStart = motorUpdateStartNull, // May be updated after copying
+    .decodeTelemetry = motorDecodeTelemetryNull, // May be updated after copying
     .write = dshotWrite,
     .writeInt = dshotWriteInt,
     .updateComplete = pwmCompleteDshotMotorUpdate,
@@ -160,7 +160,7 @@ motorDevice_t *dshotPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
 
 #ifdef USE_DSHOT_TELEMETRY
     useDshotTelemetry = motorConfig->useDshotTelemetry;
-    dshotPwmDevice.vTable.updateStart = pwmStartDshotMotorUpdate;
+    dshotPwmDevice.vTable.decodeTelemetry = pwmTelemetryDecode;
 #endif
 
     switch (motorConfig->motorPwmProtocol) {

src/main/drivers/dshot_dpwm.h

@@ -162,7 +162,7 @@ motorDmaOutput_t *getMotorDmaOutput(uint8_t index);
 void pwmWriteDshotInt(uint8_t index, uint16_t value);
 bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
 #ifdef USE_DSHOT_TELEMETRY
-bool pwmStartDshotMotorUpdate(void);
+bool pwmTelemetryDecode(void);
 #endif
 void pwmCompleteDshotMotorUpdate(void);
 

src/main/drivers/motor.c

@@ -60,15 +60,49 @@ void motorWriteAll(float *values)
 {
 #ifdef USE_PWM_OUTPUT
     if (motorDevice->enabled) {
+#ifdef USE_DSHOT_BITBANG
+        if (isDshotBitbangActive(&motorConfig()->dev)) {
+            // Initialise the output buffers
+            if (motorDevice->vTable.updateInit) {
+                motorDevice->vTable.updateInit();
+            }
+
+            // Update the motor data
+            for (int i = 0; i < motorDevice->count; i++) {
+                motorDevice->vTable.write(i, values[i]);
+            }
+
+            // Don't attempt to write commands to the motors if telemetry is still being received
+            if (motorDevice->vTable.telemetryWait) {
+                (void)motorDevice->vTable.telemetryWait();
+            }
+
+            // Trigger the transmission of the motor data
+            motorDevice->vTable.updateComplete();
+
+            // Perform the decode of the last data received
+            // New data will be received once the send of motor data, triggered above, completes
 #if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
-        if (!motorDevice->vTable.updateStart()) {
+            motorDevice->vTable.decodeTelemetry();
-            return;
-        }
 #endif
-        for (int i = 0; i < motorDevice->count; i++) {
+        } else
-            motorDevice->vTable.write(i, values[i]);
+#endif
+        {
+            // Perform the decode of the last data received
+            // New data will be received once the send of motor data, triggered above, completes
+#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
+            motorDevice->vTable.decodeTelemetry();
+#endif
+
+            // Update the motor data
+            for (int i = 0; i < motorDevice->count; i++) {
+                motorDevice->vTable.write(i, values[i]);
+            }
+
+            // Trigger the transmission of the motor data
+            motorDevice->vTable.updateComplete();
+
         }
-        motorDevice->vTable.updateComplete();
     }
 #else
     UNUSED(values);
@@ -193,7 +227,7 @@ static bool motorIsEnabledNull(uint8_t index)
     return false;
 }
 
-bool motorUpdateStartNull(void)
+bool motorDecodeTelemetryNull(void)
 {
     return true;
 }
@@ -235,7 +269,7 @@ static const motorVTable_t motorNullVTable = {
     .enable = motorEnableNull,
     .disable = motorDisableNull,
     .isMotorEnabled = motorIsEnabledNull,
-    .updateStart = motorUpdateStartNull,
+    .decodeTelemetry = motorDecodeTelemetryNull,
     .write = motorWriteNull,
     .writeInt = motorWriteIntNull,
     .updateComplete = motorUpdateCompleteNull,

src/main/drivers/motor.h

@@ -50,7 +50,9 @@ typedef struct motorVTable_s {
     bool (*enable)(void);
     void (*disable)(void);
     bool (*isMotorEnabled)(uint8_t index);
-    bool (*updateStart)(void);
+    bool (*telemetryWait)(void);
+    bool (*decodeTelemetry)(void);
+    void (*updateInit)(void);
     void (*write)(uint8_t index, float value);
     void (*writeInt)(uint8_t index, uint16_t value);
     void (*updateComplete)(void);
@@ -70,7 +72,7 @@ typedef struct motorDevice_s {
 
 void motorPostInitNull();
 void motorWriteNull(uint8_t index, float value);
-bool motorUpdateStartNull(void);
+bool motorDecodeTelemetryNull(void);
 void motorUpdateCompleteNull(void);
 
 void motorPostInit();

src/main/drivers/pwm_output_dshot_shared.c

@@ -199,7 +199,7 @@ static uint32_t decodeTelemetryPacket(uint32_t buffer[], uint32_t count)
  * Process dshot telemetry packets before switching the channels back to outputs
  * 
 */
-FAST_CODE_NOINLINE bool pwmStartDshotMotorUpdate(void)
+FAST_CODE_NOINLINE bool pwmTelemetryDecode(void)
 {
     if (!useDshotTelemetry) {
         return true;

src/main/drivers/pwm_output_dshot_shared.h

@@ -59,7 +59,7 @@ void pwmDshotSetDirectionOutput(
 #endif
 );
 
-bool pwmStartDshotMotorUpdate(void);
+bool pwmTelemetryDecode(void);
 
 #endif
 #endif

src/main/drivers/stm32/dshot_bitbang.c

@@ -46,6 +46,7 @@
 #include "drivers/timer.h"
 
 #include "pg/motor.h"
+#include "pg/pinio.h"
 
 // DEBUG_DSHOT_TELEMETRY_COUNTS
 // 0 - Count of telemetry packets read
@@ -337,6 +338,8 @@ FAST_IRQ_HANDLER void bbDMAIrqHandler(dmaChannelDescriptor_t *descriptor)
 #ifdef DEBUG_COUNT_INTERRUPT
             bbPort->inputIrq++;
 #endif
+            // Disable DMA as telemetry reception is complete
+            bbDMA_Cmd(bbPort, DISABLE);
         } else {
 #ifdef DEBUG_COUNT_INTERRUPT
             bbPort->outputIrq++;
@@ -452,7 +455,7 @@ static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorPwmProtocolTypes_e p
 
         if (!bbPort || !dmaAllocate(dmaGetIdentifier(bbPort->dmaResource), bbPort->owner.owner, bbPort->owner.resourceIndex)) {
             bbDevice.vTable.write = motorWriteNull;
-            bbDevice.vTable.updateStart = motorUpdateStartNull;
+            bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
             bbDevice.vTable.updateComplete = motorUpdateCompleteNull;
 
             return false;
@@ -504,7 +507,41 @@ static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorPwmProtocolTypes_e p
     return true;
 }
 
-static bool bbUpdateStart(void)
+static bool bbTelemetryWait(void)
+{
+    // Wait for telemetry reception to complete
+    bool telemetryPending;
+    bool telemetryWait = false;
+    const timeUs_t startTimeUs = micros();
+
+    do {
+        telemetryPending = false;
+        for (int i = 0; i < usedMotorPorts; i++) {
+            telemetryPending |= bbPorts[i].telemetryPending;
+        }
+
+        telemetryWait |= telemetryPending;
+
+        if (cmpTimeUs(micros(), startTimeUs) > DSHOT_TELEMETRY_TIMEOUT) {
+            break;
+        }
+    } while (telemetryPending);
+
+    if (telemetryWait) {
+        DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 2, debug[2] + 1);
+    }
+
+    return telemetryWait;
+}
+
+static void bbUpdateInit(void)
+{
+    for (int i = 0; i < usedMotorPorts; i++) {
+        bbOutputDataClear(bbPorts[i].portOutputBuffer);
+    }
+}
+
+static bool bbDecodeTelemetry(void)
 {
 #ifdef USE_DSHOT_TELEMETRY
     if (useDshotTelemetry) {
@@ -512,76 +549,49 @@ static bool bbUpdateStart(void)
         const timeMs_t currentTimeMs = millis();
 #endif
 
-        // Wait for telemetry reception to complete before decode
-        bool telemetryPending;
-        bool telemetryWait = false;
-        const timeUs_t startTimeUs = micros();
-
-        do {
-            telemetryPending = false;
-            for (int i = 0; i < usedMotorPorts; i++) {
-                telemetryPending |= bbPorts[i].telemetryPending;
-            }
-
-            telemetryWait |= telemetryPending;
-
-            if (cmpTimeUs(micros(), startTimeUs) > DSHOT_TELEMETRY_TIMEOUT) {
-                return false;
-            }
-        } while (telemetryPending);
-
-        if (telemetryWait) {
-            DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 2, debug[2] + 1);
-        } else {
 #ifdef USE_DSHOT_CACHE_MGMT
-            for (int i = 0; i < usedMotorPorts; i++) {
+        for (int i = 0; i < usedMotorPorts; i++) {
-                bbPort_t *bbPort = &bbPorts[i];
+            bbPort_t *bbPort = &bbPorts[i];
-                SCB_InvalidateDCache_by_Addr((uint32_t *)bbPort->portInputBuffer, DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_BYTES);
+            SCB_InvalidateDCache_by_Addr((uint32_t *)bbPort->portInputBuffer, DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_BYTES);
-            }
-#endif
-            for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
-#ifdef STM32F4
-                uint32_t rawValue = decode_bb_bitband(
-                    bbMotors[motorIndex].bbPort->portInputBuffer,
-                    bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort),
-                    bbMotors[motorIndex].pinIndex);
-#else
-                uint32_t rawValue = decode_bb(
-                    bbMotors[motorIndex].bbPort->portInputBuffer,
-                    bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort),
-                    bbMotors[motorIndex].pinIndex);
-#endif
-                if (rawValue == DSHOT_TELEMETRY_NOEDGE) {
-                    DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 1, debug[1] + 1);
-                    continue;
-                }
-                DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 0, debug[0] + 1);
-                dshotTelemetryState.readCount++;
-
-                if (rawValue != DSHOT_TELEMETRY_INVALID) {
-                    // Check EDT enable or store raw value
-                    if ((rawValue == 0x0E00) && (dshotCommandGetCurrent(motorIndex) == DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE)) {
-                        dshotTelemetryState.motorState[motorIndex].telemetryTypes = 1 << DSHOT_TELEMETRY_TYPE_STATE_EVENTS;
-                    } else {
-                        dshotTelemetryState.motorState[motorIndex].rawValue = rawValue;
-                    }
-                } else {
-                    dshotTelemetryState.invalidPacketCount++;
-                }
-#ifdef USE_DSHOT_TELEMETRY_STATS
-                updateDshotTelemetryQuality(&dshotTelemetryQuality[motorIndex], rawValue != DSHOT_TELEMETRY_INVALID, currentTimeMs);
-#endif
-            }
-
-            dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_NOT_PROCESSED;
         }
 #endif
-    }
+        for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
+#ifdef STM32F4
+            uint32_t rawValue = decode_bb_bitband(
+                bbMotors[motorIndex].bbPort->portInputBuffer,
+                bbMotors[motorIndex].bbPort->portInputCount,
+                bbMotors[motorIndex].pinIndex);
+#else
+            uint32_t rawValue = decode_bb(
+                bbMotors[motorIndex].bbPort->portInputBuffer,
+                bbMotors[motorIndex].bbPort->portInputCount,
+                bbMotors[motorIndex].pinIndex);
+#endif
+            if (rawValue == DSHOT_TELEMETRY_NOEDGE) {
+                DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 1, debug[1] + 1);
+                continue;
+            }
+            DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 0, debug[0] + 1);
+            dshotTelemetryState.readCount++;
 
-    for (int i = 0; i < usedMotorPorts; i++) {
+            if (rawValue != DSHOT_TELEMETRY_INVALID) {
-        bbDMA_Cmd(&bbPorts[i], DISABLE);
+                // Check EDT enable or store raw value
-        bbOutputDataClear(bbPorts[i].portOutputBuffer);
+                if ((rawValue == 0x0E00) && (dshotCommandGetCurrent(motorIndex) == DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE)) {
+                    dshotTelemetryState.motorState[motorIndex].telemetryTypes = 1 << DSHOT_TELEMETRY_TYPE_STATE_EVENTS;
+                } else {
+                    dshotTelemetryState.motorState[motorIndex].rawValue = rawValue;
+                }
+            } else {
+                dshotTelemetryState.invalidPacketCount++;
+            }
+#ifdef USE_DSHOT_TELEMETRY_STATS
+            updateDshotTelemetryQuality(&dshotTelemetryQuality[motorIndex], rawValue != DSHOT_TELEMETRY_INVALID, currentTimeMs);
+#endif
+        }
+
+        dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_NOT_PROCESSED;
     }
+#endif
 
     return true;
 }
@@ -718,7 +728,9 @@ static motorVTable_t bbVTable = {
     .enable = bbEnableMotors,
     .disable = bbDisableMotors,
     .isMotorEnabled = bbIsMotorEnabled,
-    .updateStart = bbUpdateStart,
+    .telemetryWait = bbTelemetryWait,
+    .decodeTelemetry = bbDecodeTelemetry,
+    .updateInit = bbUpdateInit,
     .write = bbWrite,
     .writeInt = bbWriteInt,
     .updateComplete = bbUpdateComplete,
@@ -765,7 +777,7 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t
         if (!IOIsFreeOrPreinit(io)) {
             /* not enough motors initialised for the mixer or a break in the motors */
             bbDevice.vTable.write = motorWriteNull;
-            bbDevice.vTable.updateStart = motorUpdateStartNull;
+            bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
             bbDevice.vTable.updateComplete = motorUpdateCompleteNull;
             bbStatus = DSHOT_BITBANG_STATUS_MOTOR_PIN_CONFLICT;
             return NULL;

src/main/drivers/stm32/pwm_output.c

@@ -215,7 +215,7 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
     }
 
     motorPwmDevice.vTable.write = pwmWriteStandard;
-    motorPwmDevice.vTable.updateStart = motorUpdateStartNull;
+    motorPwmDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
     motorPwmDevice.vTable.updateComplete = useUnsyncedPwm ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate;
 
     for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {

src/main/target/SITL/sitl.c

@@ -619,7 +619,7 @@ static motorDevice_t motorPwmDevice = {
         .enable = pwmEnableMotors,
         .disable = pwmDisableMotors,
         .isMotorEnabled = pwmIsMotorEnabled,
-        .updateStart = motorUpdateStartNull,
+        .decodeTelemetry = motorDecodeTelemetryNull,
         .write = pwmWriteMotor,
         .writeInt = pwmWriteMotorInt,
         .updateComplete = pwmCompleteMotorUpdate,