If DSHOT telemetry is still being received, wait (#12612)

src/main/build/debug.c

@@ -112,4 +112,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "ANGLE_MODE",
     "ANGLE_TARGET",
     "CURRENT_ANGLE",
+    "DSHOT_TELEMETRY_COUNTS",
 };

src/main/build/debug.h

@@ -110,6 +110,7 @@ typedef enum {
     DEBUG_ANGLE_MODE,
     DEBUG_ANGLE_TARGET,
     DEBUG_CURRENT_ANGLE,
+    DEBUG_DSHOT_TELEMETRY_COUNTS,
     DEBUG_COUNT
 } debugType_e;
 

src/main/drivers/dshot_bitbang_decode.c

@@ -1,3 +1,23 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
 #include <stdint.h>
 #include <string.h>
 #include <stdio.h>
@@ -8,6 +28,7 @@
 
 #include "common/maths.h"
 #include "common/utils.h"
+#include "build/debug.h"
 #include "drivers/dshot.h"
 #include "drivers/dshot_bitbang_decode.h"
 
@@ -28,7 +49,8 @@ uint16_t bbBuffer[134];
 #define BITBAND_SRAM_BASE  0x22000000
 #define BITBAND_SRAM(a,b) ((BITBAND_SRAM_BASE + (((a)-BITBAND_SRAM_REF)<<5) + ((b)<<2)))  // Convert SRAM address
 
-
+#define DSHOT_TELEMETRY_START_MARGIN 10
+static uint8_t preambleSkip = 0;
 
 typedef struct bitBandWord_s {
     uint32_t value;
@@ -84,6 +106,8 @@ static uint32_t decode_bb_value(uint32_t value, uint16_t buffer[], uint32_t coun
 
 uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
 {
+    uint8_t startMargin;
+
 #ifdef DEBUG_BBDECODE
     memset(sequence, 0, sizeof(sequence));
     sequenceIndex = 0;
@@ -94,6 +118,9 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
     bitBandWord_t* b = p;
     bitBandWord_t* endP = p + (count - MIN_VALID_BBSAMPLES);
 
+    // Jump forward in the buffer to just before where we anticipate the first zero
+    p += preambleSkip;
+
     // Eliminate leading high signal level by looking for first zero bit in data stream.
     // Manual loop unrolling and branch hinting to produce faster code.
     while (p < endP) {
@@ -105,6 +132,9 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
         }
     }
 
+    startMargin = p - b;
+    DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 3, startMargin);
+
     if (p >= endP) {
         // not returning telemetry is ok if the esc cpu is
         // overburdened.  in that case no edge will be found and
@@ -188,6 +218,10 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
     sequence[sequenceIndex] = sequence[sequenceIndex] + (nlen) * 3;
     sequenceIndex++;
 #endif
+
+    // The anticipated edges were observed
+    preambleSkip = startMargin - DSHOT_TELEMETRY_START_MARGIN;
+
     if (nlen > 0) {
         value <<= nlen;
         value |= 1 << (nlen - 1);
@@ -198,6 +232,8 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
 
 FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
 {
+    uint8_t startMargin;
+
 #ifdef DEBUG_BBDECODE
     memset(sequence, 0, sizeof(sequence));
     sequenceIndex = 0;
@@ -209,12 +245,15 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
     memset(sequence, 0, sizeof(sequence));
     int sequenceIndex = 0;
 #endif
-
     uint16_t lastValue = 0;
     uint32_t value = 0;
 
     uint16_t* p = buffer;
     uint16_t* endP = p + count - MIN_VALID_BBSAMPLES;
+
+    // Jump forward in the buffer to just before where we anticipate the first zero
+    p += preambleSkip;
+
     // Eliminate leading high signal level by looking for first zero bit in data stream.
     // Manual loop unrolling and branch hinting to produce faster code.
     while (p < endP) {
@@ -226,10 +265,15 @@ 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) {
         // 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
+        // Increase the start margin
+        preambleSkip--;
         return DSHOT_TELEMETRY_NOEDGE;
     }
 
@@ -268,6 +312,8 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
 
     // length of last sequence has to be inferred since the last bit with inverted dshot is high
     if (bits < 18) {
+        // Increase the start margin
+        preambleSkip--;
         return DSHOT_TELEMETRY_NOEDGE;
     }
 
@@ -278,9 +324,14 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
 #endif
 
     if (nlen < 0) {
+        // Increase the start margin
+        preambleSkip--;
         return DSHOT_TELEMETRY_NOEDGE;
     }
 
+    // The anticipated edges were observed
+    preambleSkip = startMargin - DSHOT_TELEMETRY_START_MARGIN;
+
     if (nlen > 0) {
         value <<= nlen;
         value |= 1 << (nlen - 1);

src/main/drivers/dshot_bitbang_impl.h

@@ -167,6 +167,7 @@ typedef struct bbPort_s {
     uint16_t *portInputBuffer;
     uint32_t portInputCount;
     bool inputActive;
+    volatile bool telemetryPending;
 
     // Misc
 #ifdef DEBUG_COUNT_INTERRUPT

src/main/drivers/stm32/dshot_bitbang.c

@@ -47,6 +47,15 @@
 
 #include "pg/motor.h"
 
+// DEBUG_DSHOT_TELEMETRY_COUNTS
+// 0 - Count of telemetry packets read
+// 1 - Count of missing edge
+// 2 - Count of reception not complete in time
+// 3 - Number of high bits before telemetry start
+
+// Maximum time to wait for telemetry reception to complete
+#define DSHOT_TELEMETRY_TIMEOUT 2000
+
 FAST_DATA_ZERO_INIT bbPacer_t bbPacers[MAX_MOTOR_PACERS];  // TIM1 or TIM8
 FAST_DATA_ZERO_INIT int usedMotorPacers = 0;
 
@@ -324,6 +333,7 @@ FAST_IRQ_HANDLER void bbDMAIrqHandler(dmaChannelDescriptor_t *descriptor)
 #ifdef USE_DSHOT_TELEMETRY
     if (useDshotTelemetry) {
         if (bbPort->direction == DSHOT_BITBANG_DIRECTION_INPUT) {
+            bbPort->telemetryPending = false;
 #ifdef DEBUG_COUNT_INTERRUPT
             bbPort->inputIrq++;
 #endif
@@ -335,6 +345,7 @@ FAST_IRQ_HANDLER void bbDMAIrqHandler(dmaChannelDescriptor_t *descriptor)
             // Switch to input
 
             bbSwitchToInput(bbPort);
+            bbPort->telemetryPending = true;
 
             bbTIM_DMACmd(bbPort->timhw->tim, bbPort->dmaSource, ENABLE);
         }
@@ -500,62 +511,81 @@ static bool bbUpdateStart(void)
 #ifdef USE_DSHOT_TELEMETRY_STATS
         const timeMs_t currentTimeMs = millis();
 #endif
-        timeUs_t currentUs = micros();
 
-        // don't send while telemetry frames might still be incoming
+        // Wait for telemetry reception to complete before decode
-        if (cmpTimeUs(currentUs, lastSendUs) < (timeDelta_t)(40 + 2 * dshotFrameUs)) {
+        bool telemetryPending;
-            return false;
+        bool telemetryWait = false;
-        }
+        const timeUs_t startTimeUs = micros();
 
-        for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
+        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 {
+            for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
 #ifdef USE_DSHOT_CACHE_MGMT
-            // Only invalidate the buffer once. If all motors are on a common port they'll share a buffer.
+                // Only invalidate the buffer once. If all motors are on a common port they'll share a buffer.
-            bool invalidated = false;
+                bool invalidated = false;
-            for (int i = 0; i < motorIndex; i++) {
+                for (int i = 0; i < motorIndex; i++) {
-                if (bbMotors[motorIndex].bbPort->portInputBuffer == bbMotors[i].bbPort->portInputBuffer) {
+                    if (bbMotors[motorIndex].bbPort->portInputBuffer == bbMotors[i].bbPort->portInputBuffer) {
-                    invalidated = true;
+                        invalidated = true;
+                    }
+                }
+                if (!invalidated) {
+                    SCB_InvalidateDCache_by_Addr((uint32_t *)bbMotors[motorIndex].bbPort->portInputBuffer,
+                                                 DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_BYTES);
                 }
-            }
-            if (!invalidated) {
-                SCB_InvalidateDCache_by_Addr((uint32_t *)bbMotors[motorIndex].bbPort->portInputBuffer,
-                                             DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_BYTES);
-            }
 #endif
 
 #ifdef STM32F4
-            uint32_t rawValue = decode_bb_bitband(
+                uint32_t rawValue = decode_bb_bitband(
-                bbMotors[motorIndex].bbPort->portInputBuffer,
+                    bbMotors[motorIndex].bbPort->portInputBuffer,
-                bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort),
+                    bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort),
-                bbMotors[motorIndex].pinIndex);
+                    bbMotors[motorIndex].pinIndex);
 #else
-            uint32_t rawValue = decode_bb(
+                uint32_t rawValue = decode_bb(
-                bbMotors[motorIndex].bbPort->portInputBuffer,
+                    bbMotors[motorIndex].bbPort->portInputBuffer,
-                bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort),
+                    bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort),
-                bbMotors[motorIndex].pinIndex);
+                    bbMotors[motorIndex].pinIndex);
 #endif
-            if (rawValue == DSHOT_TELEMETRY_NOEDGE) {
+                if (rawValue == DSHOT_TELEMETRY_NOEDGE) {
-                continue;
+                    DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 1, debug[1] + 1);
-            }
+                    continue;
-            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 {
+                DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 0, debug[0] + 1);
-                dshotTelemetryState.invalidPacketCount++;
+                dshotTelemetryState.readCount++;
-            }
-#ifdef USE_DSHOT_TELEMETRY_STATS
-            updateDshotTelemetryQuality(&dshotTelemetryQuality[motorIndex], rawValue != DSHOT_TELEMETRY_INVALID, currentTimeMs);
-#endif
-        }
 
-        dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_NOT_PROCESSED;
+                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 i = 0; i < usedMotorPorts; i++) {
         bbDMA_Cmd(&bbPorts[i], DISABLE);
         bbOutputDataClear(bbPorts[i].portOutputBuffer);