From 9458ac043a27d20b5077ba48b95b75644984d22f Mon Sep 17 00:00:00 2001
From: ke deng <degkxp@hotmail.com>
Date: Wed, 9 Jul 2025 15:23:07 +0800
Subject: [PATCH] refactor dshot_decode_telemetry_value to prevent EDT data in
 RPM (#14490)

* refactor dshot_decode_telemetry_value to prevent EDT data in RPM

* Optimize telemetry type detection for extended DShot protocol

* Fix extended telemetry array size and eRPM fallback logic

* Update src/main/drivers/dshot.c

Co-authored-by: Petr Ledvina <ledvinap@gmail.com>

* Format alignment

---------

Co-authored-by: Petr Ledvina <ledvinap@gmail.com>
---
 src/main/drivers/dshot.c | 116 ++++++++++++---------------------------
 1 file changed, 36 insertions(+), 80 deletions(-)

diff --git a/src/main/drivers/dshot.c b/src/main/drivers/dshot.c
index d39a5531f1..b0585e094f 100644
--- a/src/main/drivers/dshot.c
+++ b/src/main/drivers/dshot.c
@@ -151,6 +151,26 @@ FAST_DATA_ZERO_INIT static float erpmToHz;
 FAST_DATA_ZERO_INIT static float dshotRpmAverage;
 FAST_DATA_ZERO_INIT static float dshotRpm[MAX_SUPPORTED_MOTORS];
 
+// Lookup table for extended telemetry type decoding
+// Only contains extended telemetry types, eRPM is handled by conditional logic
+static const dshotTelemetryType_t extendedTelemetryLookup[8] = {
+    DSHOT_TELEMETRY_TYPE_eRPM,
+    // Temperature range (in degree Celsius, just like Blheli_32 and KISS)
+    DSHOT_TELEMETRY_TYPE_TEMPERATURE,
+    // Voltage range (0-63,75V step 0,25V)
+    DSHOT_TELEMETRY_TYPE_VOLTAGE,
+    // Current range (0-255A step 1A)
+    DSHOT_TELEMETRY_TYPE_CURRENT,
+    // Debug 1 value
+    DSHOT_TELEMETRY_TYPE_DEBUG1,
+    // Debug 2 value
+    DSHOT_TELEMETRY_TYPE_DEBUG2,
+    // Debug 3 value
+    DSHOT_TELEMETRY_TYPE_DEBUG3,
+    // State / events
+    DSHOT_TELEMETRY_TYPE_STATE_EVENTS,
+};
+
 void initDshotTelemetry(const timeUs_t looptimeUs)
 {
     // if bidirectional DShot is not available
@@ -193,93 +213,29 @@ static uint32_t dshot_decode_eRPM_telemetry_value(uint16_t value)
 static void dshot_decode_telemetry_value(uint8_t motorIndex, uint32_t *pDecoded, dshotTelemetryType_t *pType)
 {
     uint16_t value = dshotTelemetryState.motorState[motorIndex].rawValue;
-    const unsigned motorCount = dshotMotorCount;
+    bool isEdtEnabled = (dshotTelemetryState.motorState[motorIndex].telemetryTypes & DSHOT_EXTENDED_TELEMETRY_MASK) != 0;
 
-    if (dshotTelemetryState.motorState[motorIndex].telemetryTypes == DSHOT_NORMAL_TELEMETRY_MASK) {   /* Check DSHOT_TELEMETRY_TYPE_eRPM mask */
-        // Decode eRPM telemetry
+    // https://github.com/bird-sanctuary/extended-dshot-telemetry   
+    // Extract telemetry type field and check for eRPM conditions in one operation
+    unsigned telemetryType = (value & 0x0f00) >> 8;  // 3 bits type + telemetry marker
+    bool isErpm = !isEdtEnabled || (telemetryType & 0x01) || (telemetryType == 0);
+    
+    if (isErpm) {
         *pDecoded = dshot_decode_eRPM_telemetry_value(value);
-
+        *pType = DSHOT_TELEMETRY_TYPE_eRPM;
+        
         // Update debug buffer
-        if (motorIndex < motorCount && motorIndex < DEBUG16_VALUE_COUNT) {
+        if (motorIndex < dshotMotorCount && motorIndex < DEBUG16_VALUE_COUNT) {
             DEBUG_SET(DEBUG_DSHOT_RPM_TELEMETRY, motorIndex, *pDecoded);
         }
-
-        // Set telemetry type
-        *pType = DSHOT_TELEMETRY_TYPE_eRPM;
     } else {
-        // Decode Extended DSHOT telemetry
-        switch (value & 0x0f00) {
-
-        case 0x0200:
-            // Temperature range (in degree Celsius, just like Blheli_32 and KISS)
-            *pDecoded = value & 0x00ff;
-
-            // Set telemetry type
-            *pType = DSHOT_TELEMETRY_TYPE_TEMPERATURE;
-            break;
-
-        case 0x0400:
-            // Voltage range (0-63,75V step 0,25V)
-            *pDecoded = value & 0x00ff;
-
-            // Set telemetry type
-            *pType = DSHOT_TELEMETRY_TYPE_VOLTAGE;
-            break;
-
-        case 0x0600:
-            // Current range (0-255A step 1A)
-            *pDecoded = value & 0x00ff;
-
-            // Set telemetry type
-            *pType = DSHOT_TELEMETRY_TYPE_CURRENT;
-            break;
-
-        case 0x0800:
-            // Debug 1 value
-            *pDecoded = value & 0x00ff;
-
-            // Set telemetry type
-            *pType = DSHOT_TELEMETRY_TYPE_DEBUG1;
-            break;
-
-        case 0x0A00:
-            // Debug 2 value
-            *pDecoded = value & 0x00ff;
-
-            // Set telemetry type
-            *pType = DSHOT_TELEMETRY_TYPE_DEBUG2;
-            break;
-
-        case 0x0C00:
-            // Debug 3 value
-            *pDecoded = value & 0x00ff;
-
-            // Set telemetry type
-            *pType = DSHOT_TELEMETRY_TYPE_DEBUG3;
-            break;
-
-        case 0x0E00:
-            // State / events
-            *pDecoded = value & 0x00ff;
-
-            // Set telemetry type
-            *pType = DSHOT_TELEMETRY_TYPE_STATE_EVENTS;
-            break;
-
-        default:
-            // Decode as eRPM
-            *pDecoded = dshot_decode_eRPM_telemetry_value(value);
-
-            // Update debug buffer
-            if (motorIndex < motorCount && motorIndex < DEBUG16_VALUE_COUNT) {
-                DEBUG_SET(DEBUG_DSHOT_RPM_TELEMETRY, motorIndex, *pDecoded);
-            }
-
-            // Set telemetry type
-            *pType = DSHOT_TELEMETRY_TYPE_eRPM;
-            break;
-
+        // Use lookup table for extended telemetry types
+        unsigned typeIndex = telemetryType >> 1;  // drop tag bit containing zero
+        if (typeIndex < ARRAYLEN(extendedTelemetryLookup)) {
+            *pType = extendedTelemetryLookup[typeIndex];
         }
+        // Extract data field
+        *pDecoded = value & 0x00ff;
     }
 }