Dshot bidir blhelis support (#8554)

Dshot bidir blhelis support

src/main/cli/cli.c

@@ -261,7 +261,7 @@ static const char * const *sensorHardwareNames[] = {
 
 #if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
 extern uint32_t readDoneCount;
-extern uint32_t inputBuffer[DSHOT_TELEMETRY_INPUT_LEN];
+extern uint32_t inputBuffer[GCR_TELEMETRY_INPUT_LEN];
 extern uint32_t setDirectionMicros;
 #endif
 
@@ -5680,15 +5680,13 @@ static void cliDshotTelemetryInfo(char *cmdline)
         }
         cliPrintLinefeed();
 
-        const bool proshot = (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_PROSHOT1000);
+        const int len = MAX_GCR_EDGES;
-        const int modulo = proshot ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH;
-        const int len = proshot ? 8 : DSHOT_TELEMETRY_INPUT_LEN;
         for (int i = 0; i < len; i++) {
             cliPrintf("%u ", (int)inputBuffer[i]);
         }
         cliPrintLinefeed();
-        for (int i = 1; i < len; i+=2) {
+        for (int i = 1; i < len; i++) {
-            cliPrintf("%u ", (int)(inputBuffer[i] + modulo - inputBuffer[i-1]) % modulo);
+            cliPrintf("%u ", (int)(inputBuffer[i]  - inputBuffer[i-1]));
         }
         cliPrintLinefeed();
     } else {

src/main/drivers/dshot_dpwm.c

@@ -112,6 +112,12 @@ static void dshotPwmDisableMotors(void)
 
 static bool dshotPwmEnableMotors(void)
 {
+    for (int i = 0; i < dshotPwmDevice.count; i++) {
+        motorDmaOutput_t *motor = getMotorDmaOutput(i);
+        const IO_t motorIO = IOGetByTag(motor->timerHardware->tag);
+        IOConfigGPIOAF(motorIO, motor->iocfg, motor->timerHardware->alternateFunction);
+    }
+
     // No special processing required
     return true;
 }

src/main/drivers/dshot_dpwm.h

@@ -36,7 +36,11 @@
 #define PROSHOT_BIT_WIDTH            3
 #define MOTOR_NIBBLE_LENGTH_PROSHOT  (PROSHOT_BASE_SYMBOL * 4) // 4uS
 
-#define DSHOT_TELEMETRY_DEADTIME_US   (2 * 30 + 10) // 2 * 30uS to switch lines plus 10us grace period
+#define DSHOT_TELEMETRY_DEADTIME_US   (30 + 5) // 30 to switch lines and 5 to switch lines back
+
+#define MIN_GCR_EDGES         7
+#define MAX_GCR_EDGES         22
+
 
 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
 extern FAST_RAM_ZERO_INIT loadDmaBufferFn *loadDmaBuffer;
@@ -55,8 +59,7 @@ motorDevice_t *dshotPwmDevInit(const struct motorDevConfig_s *motorConfig, uint1
 #define DSHOT_DMA_BUFFER_SIZE   18 /* resolution + frame reset (2us) */
 #define PROSHOT_DMA_BUFFER_SIZE 6  /* resolution + frame reset (2us) */
 
-#define DSHOT_TELEMETRY_INPUT_LEN 32
+#define GCR_TELEMETRY_INPUT_LEN MAX_GCR_EDGES
-#define PROSHOT_TELEMETRY_INPUT_LEN 8
 
 // For H7, DMA buffer is placed in a dedicated segment for coherency management
 #if defined(STM32H7)
@@ -64,7 +67,7 @@ motorDevice_t *dshotPwmDevInit(const struct motorDevConfig_s *motorConfig, uint1
 #elif defined(STM32F7)
 #define DSHOT_DMA_BUFFER_ATTRIBUTE FAST_RAM_ZERO_INIT
 #else
-#define DSHOT_DMA_BUFFER_ATTRIBUTE // None
+#define DSHOT_DMA_BUFFER_ATTRIBUTE
 #endif
 
 #if defined(STM32F3) || defined(STM32F4) || defined(STM32F7) || defined(STM32H7)
@@ -74,13 +77,14 @@ motorDevice_t *dshotPwmDevInit(const struct motorDevConfig_s *motorConfig, uint1
 #endif
 
 #ifdef USE_DSHOT_TELEMETRY
-STATIC_ASSERT(DSHOT_TELEMETRY_INPUT_LEN >= DSHOT_DMA_BUFFER_SIZE, dshotBufferSizeConstrait);
+STATIC_ASSERT(GCR_TELEMETRY_INPUT_LEN >= DSHOT_DMA_BUFFER_SIZE, dshotBufferSizeConstrait);
-#define DSHOT_DMA_BUFFER_ALLOC_SIZE DSHOT_TELEMETRY_INPUT_LEN
+#define DSHOT_DMA_BUFFER_ALLOC_SIZE GCR_TELEMETRY_INPUT_LEN
 #else
 #define DSHOT_DMA_BUFFER_ALLOC_SIZE DSHOT_DMA_BUFFER_SIZE
 #endif
 
 extern DSHOT_DMA_BUFFER_UNIT dshotDmaBuffer[MAX_SUPPORTED_MOTORS][DSHOT_DMA_BUFFER_ALLOC_SIZE];
+extern DSHOT_DMA_BUFFER_UNIT dshotDmaInputBuffer[MAX_SUPPORTED_MOTORS][DSHOT_DMA_BUFFER_ALLOC_SIZE];
 
 #ifdef USE_DSHOT_DMAR
 extern DSHOT_DMA_BUFFER_UNIT dshotBurstDmaBuffer[MAX_DMA_TIMERS][DSHOT_DMA_BUFFER_SIZE * 4];
@@ -88,7 +92,9 @@ extern DSHOT_DMA_BUFFER_UNIT dshotBurstDmaBuffer[MAX_DMA_TIMERS][DSHOT_DMA_BUFFE
 
 typedef struct {
     TIM_TypeDef *timer;
-#if defined(USE_DSHOT) && defined(USE_DSHOT_DMAR)
+#if defined(USE_DSHOT)
+    uint16_t outputPeriod;
+#if defined(USE_DSHOT_DMAR)
 #if defined(STM32F7) || defined(STM32H7)
     TIM_HandleTypeDef timHandle;
     DMA_HandleTypeDef hdma_tim;
@@ -96,7 +102,7 @@ typedef struct {
     dmaResource_t *dmaBurstRef;
     uint16_t dmaBurstLength;
     uint32_t *dmaBurstBuffer;
-    timeUs_t inputDirectionStampUs;
+#endif
 #endif
     uint16_t timerDmaSources;
 } motorDmaTimer_t;
@@ -115,6 +121,7 @@ typedef struct motorDmaOutput_s {
 #endif
     uint8_t output;
     uint8_t index;
+    uint32_t iocfg;
 
 #if defined(USE_HAL_DRIVER) && defined(USE_FULL_LL_DRIVER)
     LL_DMA_InitTypeDef    dmaInitStruct;
@@ -124,9 +131,7 @@ typedef struct motorDmaOutput_s {
 #endif
 
 #ifdef USE_DSHOT_TELEMETRY
-    bool useProshot;
     volatile bool isInput;
-    volatile bool hasTelemetry;
     uint16_t dshotTelemetryValue;
     timeDelta_t dshotTelemetryDeadtimeUs;
     bool dshotTelemetryActive;

src/main/drivers/pwm_output_dshot.c

@@ -50,14 +50,6 @@
 
 #ifdef USE_DSHOT_TELEMETRY
 
-static void processInputIrq(motorDmaOutput_t * const motor)
-{
-    motor->hasTelemetry = true;
-    xDMA_Cmd(motor->dmaRef, DISABLE);
-    TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, DISABLE);
-    readDoneCount++;
-}
-
 void dshotEnableChannels(uint8_t motorCount)
 {
     for (int i = 0; i < motorCount; i++) {
@@ -101,7 +93,12 @@ FAST_CODE void pwmDshotSetDirectionOutput(
 #ifdef USE_DSHOT_TELEMETRY
     if (!output) {
         motor->isInput = true;
-        motor->timer->inputDirectionStampUs = micros();
+        if (!inputStampUs) {
+            inputStampUs = micros();
+        }
+        TIM_ARRPreloadConfig(timer, ENABLE);
+        timer->ARR = 0xffffffff;
+
         TIM_ICInit(timer, &motor->icInitStruct);
 
 #if defined(STM32F3)
@@ -142,8 +139,10 @@ FAST_CODE void pwmDshotSetDirectionOutput(
     }
 
     xDMA_Init(dmaRef, pDmaInit);
+    if (output) {
         xDMA_ITConfig(dmaRef, DMA_IT_TC, ENABLE);
     }
+}
 
 
 void pwmCompleteDshotMotorUpdate(void)
@@ -165,6 +164,9 @@ void pwmCompleteDshotMotorUpdate(void)
         } else
 #endif
         {
+            TIM_ARRPreloadConfig(dmaMotorTimers[i].timer, DISABLE);
+            dmaMotorTimers[i].timer->ARR = dmaMotorTimers[i].outputPeriod;
+            TIM_ARRPreloadConfig(dmaMotorTimers[i].timer, ENABLE);
             TIM_SetCounter(dmaMotorTimers[i].timer, 0);
             TIM_DMACmd(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources, ENABLE);
             dmaMotorTimers[i].timerDmaSources = 0;
@@ -178,11 +180,7 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor)
         motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
 #ifdef USE_DSHOT_TELEMETRY
         uint32_t irqStart = micros();
-        if (motor->isInput) {
-            processInputIrq(motor);
-        } else
 #endif
-        {
 #ifdef USE_DSHOT_DMAR
         if (useBurstDshot) {
             xDMA_Cmd(motor->timerHardware->dmaTimUPRef, DISABLE);
@@ -197,13 +195,12 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor)
 #ifdef USE_DSHOT_TELEMETRY
         if (useDshotTelemetry) {
             pwmDshotSetDirectionOutput(motor, false);
-                xDMA_SetCurrDataCounter(motor->dmaRef, motor->dmaInputLen);
+            xDMA_SetCurrDataCounter(motor->dmaRef, GCR_TELEMETRY_INPUT_LEN);
             xDMA_Cmd(motor->dmaRef, ENABLE);
             TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, ENABLE);
             setDirectionMicros = micros() - irqStart;
         }
 #endif
-        }
         DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
     }
 }
@@ -254,13 +251,7 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
     }
 
     motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
-#ifdef USE_DSHOT_TELEMETRY
-    motor->useProshot = (pwmProtocolType == PWM_TYPE_PROSHOT1000);
-#endif    
-    motor->timerHardware = timerHardware;
-
     TIM_TypeDef *timer = timerHardware->tim;
-    const IO_t motorIO = IOGetByTag(timerHardware->tag);
 
     // Boolean configureTimer is always true when different channels of the same timer are processed in sequence,
     // causing the timer and the associated DMA initialized more than once.
@@ -269,6 +260,12 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
     const uint8_t timerIndex = getTimerIndex(timer);
     const bool configureTimer = (timerIndex == dmaMotorTimerCount-1);
 
+    motor->timer = &dmaMotorTimers[timerIndex];
+    motor->index = motorIndex;
+    motor->timerHardware = timerHardware;
+
+    const IO_t motorIO = IOGetByTag(timerHardware->tag);
+
     uint8_t pupMode = 0;
     pupMode = (output & TIMER_OUTPUT_INVERTED) ? GPIO_PuPd_DOWN : GPIO_PuPd_UP;
 #ifdef USE_DSHOT_TELEMETRY
@@ -277,7 +274,8 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
     }
 #endif
 
-    IOConfigGPIOAF(motorIO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, pupMode), timerHardware->alternateFunction);
+    motor->iocfg = IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, pupMode);
+    IOConfigGPIOAF(motorIO, motor->iocfg, timerHardware->alternateFunction);
 
     if (configureTimer) {
         TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
@@ -313,11 +311,9 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
     motor->icInitStruct.TIM_ICPolarity = TIM_ICPolarity_BothEdge;
     motor->icInitStruct.TIM_ICPrescaler = TIM_ICPSC_DIV1;
     motor->icInitStruct.TIM_Channel = timerHardware->channel;
-    motor->icInitStruct.TIM_ICFilter = 0; //2;
+    motor->icInitStruct.TIM_ICFilter = 2;
 #endif
 
-    motor->timer = &dmaMotorTimers[timerIndex];
-    motor->index = motorIndex;
 
 #ifdef USE_DSHOT_DMAR
     if (useBurstDshot) {
@@ -393,10 +389,9 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
     motor->dmaRef = dmaRef;
 
 #ifdef USE_DSHOT_TELEMETRY
-    motor->dmaInputLen = motor->useProshot ? PROSHOT_TELEMETRY_INPUT_LEN : DSHOT_TELEMETRY_INPUT_LEN;
     motor->dshotTelemetryDeadtimeUs = DSHOT_TELEMETRY_DEADTIME_US + 1000000 *
-        ( 2 + (motor->useProshot ? 4 * MOTOR_NIBBLE_LENGTH_PROSHOT : 16 * MOTOR_BITLENGTH))
+        (16 * MOTOR_BITLENGTH) / getDshotHz(pwmProtocolType);
-        / getDshotHz(pwmProtocolType);
+    motor->timer->outputPeriod = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1;
     pwmDshotSetDirectionOutput(motor, true);
 #else
     pwmDshotSetDirectionOutput(motor, true, &OCINIT, &DMAINIT);

src/main/drivers/pwm_output_dshot_hal.c

@@ -53,23 +53,6 @@
 
 #ifdef USE_DSHOT_TELEMETRY
 
-static void processInputIrq(motorDmaOutput_t * const motor)
-{
-    motor->hasTelemetry = true;
-
-#ifdef USE_DSHOT_DMAR
-    if (useBurstDshot) {
-        xLL_EX_DMA_DisableResource(motor->timerHardware->dmaTimUPRef);
-        LL_TIM_DisableDMAReq_UPDATE(motor->timerHardware->tim);
-    } else
-#endif
-    {
-        xLL_EX_DMA_DisableResource(motor->dmaRef);
-        LL_EX_TIM_DisableIT(motor->timerHardware->tim, motor->timerDmaSource);
-    }
-    readDoneCount++;
-}
-
 void dshotEnableChannels(uint8_t motorCount)
 {
     for (int i = 0; i < motorCount; i++) {
@@ -83,7 +66,6 @@ void dshotEnableChannels(uint8_t motorCount)
 
 #endif
 
-
 static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor);
 
 void pwmDshotSetDirectionOutput(
@@ -106,7 +88,11 @@ void pwmDshotSetDirectionOutput(
 #ifdef USE_DSHOT_TELEMETRY
     if (!output) {
         motor->isInput = true;
-        motor->timer->inputDirectionStampUs = micros();
+        if (!inputStampUs) {
+            inputStampUs = micros();
+        }
+        LL_TIM_EnableARRPreload(timer); // Only update the period once all channels are done
+        timer->ARR = 0xffffffff;
         LL_TIM_IC_Init(timer, motor->llChannel, &motor->icInitStruct);
         motor->dmaInitStruct.Direction = LL_DMA_DIRECTION_PERIPH_TO_MEMORY;
     } else
@@ -124,8 +110,10 @@ void pwmDshotSetDirectionOutput(
         motor->dmaInitStruct.Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH;
     }
     xLL_EX_DMA_Init(motor->dmaRef, pDmaInit);
+    if (output) {
         xLL_EX_DMA_EnableIT_TC(motor->dmaRef);
     }
+}
 
 
 FAST_CODE void pwmCompleteDshotMotorUpdate(void)
@@ -148,6 +136,9 @@ FAST_CODE void pwmCompleteDshotMotorUpdate(void)
         } else
 #endif
         {
+            LL_TIM_DisableARRPreload(dmaMotorTimers[i].timer);
+            dmaMotorTimers[i].timer->ARR = dmaMotorTimers[i].outputPeriod;
+
             /* Reset timer counter */
             LL_TIM_SetCounter(dmaMotorTimers[i].timer, 0);
             /* Enable channel DMA requests */
@@ -161,13 +152,10 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
 {
     if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) {
         motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
+        if (!motor->isInput) {
 #ifdef USE_DSHOT_TELEMETRY
-        uint32_t irqStart = micros();
+            uint32_t irqStartUs = micros();
-        if (motor->isInput) {
-            processInputIrq(motor);
-        } else
 #endif
-        {
 #ifdef USE_DSHOT_DMAR
             if (useBurstDshot) {
                 xLL_EX_DMA_DisableResource(motor->timerHardware->dmaTimUPRef);
@@ -182,10 +170,10 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
 #ifdef USE_DSHOT_TELEMETRY
             if (useDshotTelemetry) {
                 pwmDshotSetDirectionOutput(motor, false);
-                xLL_EX_DMA_SetDataLength(motor->dmaRef, motor->dmaInputLen);
+                xLL_EX_DMA_SetDataLength(motor->dmaRef, GCR_TELEMETRY_INPUT_LEN);
                 xLL_EX_DMA_EnableResource(motor->dmaRef);
                 LL_EX_TIM_EnableIT(motor->timerHardware->tim, motor->timerDmaSource);
-                setDirectionMicros = micros() - irqStart;
+                setDirectionMicros = micros() - irqStartUs;
             }
 #endif
         }
@@ -231,26 +219,27 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
     }
 
     motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
-#ifdef USE_DSHOT_TELEMETRY
-    motor->useProshot = (pwmProtocolType == PWM_TYPE_PROSHOT1000);
-#endif
-    motor->timerHardware = timerHardware;
     motor->dmaRef = dmaRef;
 
     TIM_TypeDef *timer = timerHardware->tim;
-    const IO_t motorIO = IOGetByTag(timerHardware->tag);
 
     const uint8_t timerIndex = getTimerIndex(timer);
     const bool configureTimer = (timerIndex == dmaMotorTimerCount - 1);
 
+    motor->timer = &dmaMotorTimers[timerIndex];
+    motor->index = motorIndex;
+
+    const IO_t motorIO = IOGetByTag(timerHardware->tag);
     uint8_t pupMode = (output & TIMER_OUTPUT_INVERTED) ? GPIO_PULLDOWN : GPIO_PULLUP;
 #ifdef USE_DSHOT_TELEMETRY
     if (useDshotTelemetry) {
         output ^= TIMER_OUTPUT_INVERTED;
     }
 #endif
+    motor->timerHardware = timerHardware;
 
-    IOConfigGPIOAF(motorIO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, pupMode), timerHardware->alternateFunction);
+    motor->iocfg = IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, pupMode);
+    IOConfigGPIOAF(motorIO, motor->iocfg, timerHardware->alternateFunction);
 
     if (configureTimer) {
         LL_TIM_InitTypeDef init;
@@ -284,7 +273,7 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
     LL_TIM_IC_StructInit(&motor->icInitStruct);
     motor->icInitStruct.ICPolarity = LL_TIM_IC_POLARITY_BOTHEDGE;
     motor->icInitStruct.ICPrescaler = LL_TIM_ICPSC_DIV1;
-    motor->icInitStruct.ICFilter = 0; //2;
+    motor->icInitStruct.ICFilter = 2;
 #endif
 
     uint32_t channel = 0;
@@ -295,8 +284,6 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
     case TIM_CHANNEL_4: channel = LL_TIM_CHANNEL_CH4; break;
     }
     motor->llChannel = channel;
-    motor->timer = &dmaMotorTimers[timerIndex];
-    motor->index = motorIndex;
 
 #ifdef USE_DSHOT_DMAR
     if (useBurstDshot) {
@@ -355,10 +342,9 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
     motor->dmaRef = dmaRef;
 
 #ifdef USE_DSHOT_TELEMETRY
-    motor->dmaInputLen = motor->useProshot ? PROSHOT_TELEMETRY_INPUT_LEN : DSHOT_TELEMETRY_INPUT_LEN;
     motor->dshotTelemetryDeadtimeUs = DSHOT_TELEMETRY_DEADTIME_US + 1000000 *
-        ( 2 + (motor->useProshot ? 4 * MOTOR_NIBBLE_LENGTH_PROSHOT : 16 * MOTOR_BITLENGTH))
+        ( 16 * MOTOR_BITLENGTH) / getDshotHz(pwmProtocolType);
-        / getDshotHz(pwmProtocolType);
+    motor->timer->outputPeriod = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1;
     pwmDshotSetDirectionOutput(motor, true);
 #else
     pwmDshotSetDirectionOutput(motor, true, &OCINIT, &DMAINIT);

src/main/drivers/pwm_output_dshot_shared.c

@@ -79,8 +79,9 @@ uint32_t readDoneCount;
 
 // TODO remove once debugging no longer needed
 FAST_RAM_ZERO_INIT uint32_t dshotInvalidPacketCount;
-FAST_RAM_ZERO_INIT uint32_t inputBuffer[DSHOT_TELEMETRY_INPUT_LEN];
+FAST_RAM_ZERO_INIT uint32_t inputBuffer[GCR_TELEMETRY_INPUT_LEN];
 FAST_RAM_ZERO_INIT uint32_t setDirectionMicros;
+FAST_RAM_ZERO_INIT uint32_t inputStampUs;
 #endif
 
 motorDmaOutput_t *getMotorDmaOutput(uint8_t index)
@@ -152,56 +153,65 @@ FAST_CODE void pwmWriteDshotInt(uint8_t index, uint16_t value)
 
 void dshotEnableChannels(uint8_t motorCount);
 
-static uint16_t decodeDshotPacket(uint32_t buffer[])
+static uint32_t decodeTelemetryPacket(uint32_t buffer[], uint32_t count)
 {
+    uint32_t start = micros();
     uint32_t value = 0;
-    for (int i = 1; i < DSHOT_TELEMETRY_INPUT_LEN; i += 2) {
+    uint32_t oldValue = buffer[0];
-        int diff = buffer[i] - buffer[i-1];
+    int bits = 0;
-        value <<= 1;
+    int len;
-        if (diff > 0) {
+    for (uint32_t i = 1; i <= count; i++) {
-            if (diff >= 11) value |= 1;
+        if (i < count) {
-        } else {
+            int diff = buffer[i] - oldValue;
-            if (diff >= -9) value |= 1;
+            if (bits >= 21) {
+                break;
             }
+            len = (diff + 8) / 16;
+        } else {
+            len = 21 - bits;
         }
 
-    uint32_t csum = value;
+        value <<= len;
+        value |= 1 << (len - 1);
+        oldValue = buffer[i];
+        bits += len;
+    }
+    if (bits != 21) {
+        return 0xffff;
+    }
+
+    static const uint32_t decode[32] = {
+        0, 0, 0, 0, 0, 0, 0, 0, 0, 9, 10, 11, 0, 13, 14, 15,
+        0, 0, 2, 3, 0, 5, 6, 7, 0, 0, 8, 1, 0, 4, 12, 0 };
+
+    uint32_t decodedValue = decode[value & 0x1f];
+    decodedValue |= decode[(value >> 5) & 0x1f] << 4;
+    decodedValue |= decode[(value >> 10) & 0x1f] << 8;
+    decodedValue |= decode[(value >> 15) & 0x1f] << 12;
+
+    uint32_t csum = decodedValue;
     csum = csum ^ (csum >> 8); // xor bytes
     csum = csum ^ (csum >> 4); // xor nibbles
 
     if ((csum & 0xf) != 0xf) {
+        setDirectionMicros = micros() - start;
         return 0xffff;
     }
-    return value >> 4;
+    decodedValue >>= 4;
-}
 
-static uint16_t decodeProshotPacket(uint32_t buffer[])
+    if (decodedValue == 0x0fff) {
-{
+        setDirectionMicros = micros() - start;
-    uint32_t value = 0;
+        return 0;
-    for (int i = 1; i < PROSHOT_TELEMETRY_INPUT_LEN; i += 2) {
-        const int proshotModulo = MOTOR_NIBBLE_LENGTH_PROSHOT;
-        int diff = ((buffer[i] + proshotModulo - buffer[i-1]) % proshotModulo) - PROSHOT_BASE_SYMBOL;
-        int nibble;
-        if (diff < 0) {
-            nibble = 0;
-        } else {
-            nibble = (diff + PROSHOT_BIT_WIDTH / 2) / PROSHOT_BIT_WIDTH;
     }
-        value <<= 4;
+    decodedValue = (decodedValue & 0x000001ff) << ((decodedValue & 0xfffffe00) >> 9);
-        value |= (nibble & 0xf);
+    if (!decodedValue) {
-    }
-
-    uint32_t csum = value;
-    csum = csum ^ (csum >> 8); // xor bytes
-    csum = csum ^ (csum >> 4); // xor nibbles
-
-    if ((csum & 0xf) != 0xf) {
         return 0xffff;
     }
-    return value >> 4;
+    uint32_t ret = (1000000 * 60 / 100 + decodedValue / 2) / decodedValue;
+    setDirectionMicros = micros() - start;
+    return ret;
 }
 
-
 uint16_t getDshotTelemetry(uint8_t index)
 {
     return dmaMotors[index].dshotTelemetryValue;
@@ -241,7 +251,7 @@ void updateDshotTelemetryQuality(dshotTelemetryQuality_t *qualityStats, bool pac
 }
 #endif // USE_DSHOT_TELEMETRY_STATS
 
-bool pwmStartDshotMotorUpdate(void)
+FAST_CODE_NOINLINE bool pwmStartDshotMotorUpdate(void)
 {
     if (!useDshotTelemetry) {
         return true;
@@ -249,21 +259,31 @@ bool pwmStartDshotMotorUpdate(void)
 #ifdef USE_DSHOT_TELEMETRY_STATS
     const timeMs_t currentTimeMs = millis();
 #endif
+    const timeUs_t currentUs = micros();
     for (int i = 0; i < dshotPwmDevice.count; i++) {
-        if (dmaMotors[i].hasTelemetry) {
+        timeDelta_t usSinceInput = cmpTimeUs(currentUs, inputStampUs);
+        if (usSinceInput >= 0 && usSinceInput < dmaMotors[i].dshotTelemetryDeadtimeUs) {
+            return false;
+        }
+        if (dmaMotors[i].isInput) {
 #ifdef USE_FULL_LL_DRIVER
-            uint32_t edges = xLL_EX_DMA_GetDataLength(dmaMotors[i].dmaRef);
+            uint32_t edges = GCR_TELEMETRY_INPUT_LEN - xLL_EX_DMA_GetDataLength(dmaMotors[i].dmaRef);
 #else
-            uint32_t edges = xDMA_GetCurrDataCounter(dmaMotors[i].dmaRef);
+            uint32_t edges = GCR_TELEMETRY_INPUT_LEN - xDMA_GetCurrDataCounter(dmaMotors[i].dmaRef);
 #endif
+
+#ifdef USE_FULL_LL_DRIVER
+            LL_EX_TIM_DisableIT(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource);
+#else
+            TIM_DMACmd(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource, DISABLE);
+#endif
+
             uint16_t value = 0xffff;
-            if (edges == 0) {
+
-                if (dmaMotors[i].useProshot) {
+            if (edges > MIN_GCR_EDGES) {
-                    value = decodeProshotPacket(dmaMotors[i].dmaBuffer);
+                readDoneCount++;
-                } else {
+                value = decodeTelemetryPacket(dmaMotors[i].dmaBuffer, edges);
-                    value = decodeDshotPacket(dmaMotors[i].dmaBuffer);
+                
-                }
-            }
 #ifdef USE_DSHOT_TELEMETRY_STATS
                 bool validTelemetryPacket = false;
 #endif
@@ -282,23 +302,14 @@ bool pwmStartDshotMotorUpdate(void)
                         memcpy(inputBuffer,dmaMotors[i].dmaBuffer,sizeof(inputBuffer));
                     }
                 }
-            dmaMotors[i].hasTelemetry = false;
 #ifdef USE_DSHOT_TELEMETRY_STATS
                 updateDshotTelemetryQuality(&dshotTelemetryQuality[i], validTelemetryPacket, currentTimeMs);
-#endif
-        } else {
-            timeDelta_t usSinceInput = cmpTimeUs(micros(), dmaMotors[i].timer->inputDirectionStampUs);
-            if (usSinceInput >= 0 && usSinceInput < dmaMotors[i].dshotTelemetryDeadtimeUs) {
-                return false;
             }
-#ifdef USE_FULL_LL_DRIVER
-            LL_EX_TIM_DisableIT(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource);
-#else
-            TIM_DMACmd(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource, DISABLE);
 #endif
         }
         pwmDshotSetDirectionOutput(&dmaMotors[i], true);
     }
+    inputStampUs = 0;
     dshotEnableChannels(dshotPwmDevice.count);
     return true;
 }

src/main/drivers/pwm_output_dshot_shared.h

@@ -45,8 +45,9 @@ extern uint32_t readDoneCount;
 
 // TODO remove once debugging no longer needed
 FAST_RAM_ZERO_INIT extern uint32_t dshotInvalidPacketCount;
-FAST_RAM_ZERO_INIT extern uint32_t inputBuffer[DSHOT_TELEMETRY_INPUT_LEN];
+FAST_RAM_ZERO_INIT extern uint32_t inputBuffer[GCR_TELEMETRY_INPUT_LEN];
 FAST_RAM_ZERO_INIT extern uint32_t setDirectionMicros;
+FAST_RAM_ZERO_INIT extern uint32_t inputStampUs;
 #endif
 
 uint8_t getTimerIndex(TIM_TypeDef *timer);