Merge pull request #7841 from etracer65/dshot_bidir_quality_stats

Add DSHOT telemetry motor level packet stats

src/main/build/debug.c

@@ -86,4 +86,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "AC_CORRECTION",
     "AC_ERROR",
     "DUAL_GYRO_SCALED",
+    "DSHOT_RPM_ERRORS",
 };

src/main/build/debug.h

@@ -102,6 +102,7 @@ typedef enum {
     DEBUG_AC_CORRECTION,
     DEBUG_AC_ERROR,
     DEBUG_DUAL_GYRO_SCALED,
+    DEBUG_DSHOT_RPM_ERRORS,
     DEBUG_COUNT
 } debugType_e;
 

src/main/cli/cli.c

@@ -75,6 +75,7 @@ extern uint8_t __config_end;
 #include "drivers/io.h"
 #include "drivers/io_impl.h"
 #include "drivers/light_led.h"
+#include "drivers/pwm_output.h"
 #include "drivers/rangefinder/rangefinder_hcsr04.h"
 #include "drivers/sdcard.h"
 #include "drivers/sensor.h"
@@ -4270,28 +4271,6 @@ static void cliStatus(char *cmdline)
         cliPrintf(" %s", armingDisableFlagNames[bitpos]);
     }
     cliPrintLinefeed();
-#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
-    if (useDshotTelemetry) {
-        cliPrintLinef("Dshot reads: %u", readDoneCount);
-        cliPrintLinef("Dshot invalid pkts: %u", dshotInvalidPacketCount);
-        extern uint32_t setDirectionMicros;
-        cliPrintLinef("Dshot irq micros: %u", setDirectionMicros);
-        for (int i = 0; i<getMotorCount(); i++) {
-            cliPrintLinef( "Dshot RPM Motor %u: %u", i, (int)getDshotTelemetry(i));
-        }
-        bool proshot = (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_PROSHOT1000);
-        int modulo = proshot ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH;
-        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) {
-            cliPrintf("%u ", (int)(inputBuffer[i] + modulo - inputBuffer[i-1]) % modulo);
-        }
-        cliPrintLinefeed();
-    }
-#endif
 }
 
 #if defined(USE_TASK_STATISTICS)
@@ -5345,6 +5324,57 @@ static void cliTimer(char *cmdline)
 }
 #endif
 
+#ifdef USE_DSHOT_TELEMETRY
+static void cliDshotTelemetryInfo(char *cmdline)
+{
+    UNUSED(cmdline);
+
+    if (useDshotTelemetry) {
+        cliPrintLinef("Dshot reads: %u", readDoneCount);
+        cliPrintLinef("Dshot invalid pkts: %u", dshotInvalidPacketCount);
+        extern uint32_t setDirectionMicros;
+        cliPrintLinef("Dshot irq micros: %u", setDirectionMicros);
+        cliPrintLinefeed();
+
+#ifdef USE_DSHOT_TELEMETRY_STATS
+        cliPrintLine("Motor     RPM   Invalid");
+        cliPrintLine("=====   =====   =======");
+#else
+        cliPrintLine("Motor     RPM");
+        cliPrintLine("=====   =====");
+#endif
+        for (uint8_t i = 0; i < getMotorCount(); i++) {
+            cliPrintf("%5d   %5d   ", i, (int)getDshotTelemetry(i));
+#ifdef USE_DSHOT_TELEMETRY_STATS
+            if (isDshotMotorTelemetryActive(i)) {
+                const int calcPercent = getDshotTelemetryMotorInvalidPercent(i);
+                cliPrintLinef("%3d.%02d%%", calcPercent / 100, calcPercent % 100);
+            } else {
+                cliPrintLine("NO DATA");
+            }
+#else
+            cliPrintLinefeed();
+#endif
+        }
+        cliPrintLinefeed();
+
+        const bool proshot = (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_PROSHOT1000);
+        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) {
+            cliPrintf("%u ", (int)(inputBuffer[i] + modulo - inputBuffer[i-1]) % modulo);
+        }
+        cliPrintLinefeed();
+    } else {
+        cliPrintLine("Dshot telemetry not enabled");
+    }
+}
+#endif
+
 static void printConfig(char *cmdline, bool doDiff)
 {
     dumpFlags_t dumpMask = DUMP_MASTER;
@@ -5675,6 +5705,9 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("dma", "show DMA assignments", "show", cliDma),
 #endif
 #endif
+#ifdef USE_DSHOT_TELEMETRY
+    CLI_COMMAND_DEF("dshot_telemetry_info", "disply dshot telemetry info and stats", NULL, cliDshotTelemetryInfo),
+#endif
 #ifdef USE_DSHOT
     CLI_COMMAND_DEF("dshotprog", "program DShot ESC(s)", "<index> <command>+", cliDshotProg),
 #endif

src/main/drivers/pwm_output.h

@@ -122,6 +122,22 @@ typedef enum {
 #define DSHOT_DMA_BUFFER_SIZE   18 /* resolution + frame reset (2us) */
 #define PROSHOT_DMA_BUFFER_SIZE 6  /* resolution + frame reset (2us) */
 
+#ifdef USE_DSHOT_TELEMETRY
+#ifdef USE_DSHOT_TELEMETRY_STATS
+#define DSHOT_TELEMETRY_QUALITY_WINDOW 1       // capture a rolling 1 second of packet stats
+#define DSHOT_TELEMETRY_QUALITY_BUCKET_MS 100  // determines the granularity of the stats and the overall number of rolling buckets
+#define DSHOT_TELEMETRY_QUALITY_BUCKET_COUNT (DSHOT_TELEMETRY_QUALITY_WINDOW * 1000 / DSHOT_TELEMETRY_QUALITY_BUCKET_MS)
+
+typedef struct dshotTelemetryQuality_s {
+    uint32_t packetCountSum;
+    uint32_t invalidCountSum;
+    uint32_t packetCountArray[DSHOT_TELEMETRY_QUALITY_BUCKET_COUNT];
+    uint32_t invalidCountArray[DSHOT_TELEMETRY_QUALITY_BUCKET_COUNT];
+    uint8_t lastBucketIndex;
+}  dshotTelemetryQuality_t;
+#endif // USE_DSHOT_TELEMETRY_STATS
+#endif // USE_DSHOT_TELEMETRY
+
 typedef struct {
     TIM_TypeDef *timer;
 #if defined(USE_DSHOT) && defined(USE_DSHOT_DMAR)
@@ -153,6 +169,9 @@ typedef struct {
     uint16_t dshotTelemetryValue;
     timeDelta_t dshotTelemetryDeadtimeUs;
     bool dshotTelemetryActive;
+#ifdef USE_DSHOT_TELEMETRY_STATS
+    dshotTelemetryQuality_t dshotTelemetryQuality;
+#endif
 #ifdef USE_HAL_DRIVER
     LL_TIM_OC_InitTypeDef ocInitStruct;
     LL_TIM_IC_InitTypeDef icInitStruct;
@@ -261,6 +280,10 @@ bool pwmDshotCommandOutputIsEnabled(uint8_t motorCount);
 uint16_t getDshotTelemetry(uint8_t index);
 bool isDshotMotorTelemetryActive(uint8_t motorIndex);
 void setDshotPidLoopTime(uint32_t pidLoopTime);
+#ifdef USE_DSHOT_TELEMETRY_STATS
+int16_t getDshotTelemetryMotorInvalidPercent(uint8_t motorIndex);
+#endif
+
 #endif
 
 #ifdef USE_BEEPER

src/main/drivers/pwm_output_dshot_shared.c

@@ -202,12 +202,34 @@ FAST_CODE void pwmDshotSetDirectionOutput(
 );
 
 #ifdef USE_DSHOT_TELEMETRY
+#ifdef USE_DSHOT_TELEMETRY_STATS
+void updateDshotTelemetryQuality(dshotTelemetryQuality_t *qualityStats, bool packetValid, timeMs_t currentTimeMs)
+{
+    uint8_t statsBucketIndex = (currentTimeMs / DSHOT_TELEMETRY_QUALITY_BUCKET_MS) % DSHOT_TELEMETRY_QUALITY_BUCKET_COUNT;
+    if (statsBucketIndex != qualityStats->lastBucketIndex) {
+        qualityStats->packetCountSum -= qualityStats->packetCountArray[statsBucketIndex];
+        qualityStats->invalidCountSum -= qualityStats->invalidCountArray[statsBucketIndex];
+        qualityStats->packetCountArray[statsBucketIndex] = 0;
+        qualityStats->invalidCountArray[statsBucketIndex] = 0;
+        qualityStats->lastBucketIndex = statsBucketIndex;
+    }
+    qualityStats->packetCountSum++;
+    qualityStats->packetCountArray[statsBucketIndex]++;
+    if (!packetValid) {
+        qualityStats->invalidCountSum++;
+        qualityStats->invalidCountArray[statsBucketIndex]++;
+    }
+}
+#endif // USE_DSHOT_TELEMETRY_STATS
 
 bool pwmStartDshotMotorUpdate(uint8_t motorCount)
 {
     if (!useDshotTelemetry) {
         return true;
     }
+#ifdef USE_DSHOT_TELEMETRY_STATS
+    const timeMs_t currentTimeMs = millis();
+#endif
     for (int i = 0; i < motorCount; i++) {
         if (dmaMotors[i].hasTelemetry) {
 #ifdef STM32F7
@@ -223,12 +245,18 @@ bool pwmStartDshotMotorUpdate(uint8_t motorCount)
                     value = decodeDshotPacket(dmaMotors[i].dmaBuffer);
                 }
             }
+#ifdef USE_DSHOT_TELEMETRY_STATS
+            bool validTelemetryPacket = false;
+#endif
             if (value != 0xffff) {
                 dmaMotors[i].dshotTelemetryValue = value;
                 dmaMotors[i].dshotTelemetryActive = true;
                 if (i < 4) {
                     DEBUG_SET(DEBUG_DSHOT_RPM_TELEMETRY, i, value);
                 }
+#ifdef USE_DSHOT_TELEMETRY_STATS
+                validTelemetryPacket = true;
+#endif
             } else {
                 dshotInvalidPacketCount++;
                 if (i == 0) {
@@ -236,6 +264,9 @@ bool pwmStartDshotMotorUpdate(uint8_t motorCount)
                 }
             }
             dmaMotors[i].hasTelemetry = false;
+#ifdef USE_DSHOT_TELEMETRY_STATS
+            updateDshotTelemetryQuality(&dmaMotors[i].dshotTelemetryQuality, validTelemetryPacket, currentTimeMs);
+#endif
         } else {
             timeDelta_t usSinceInput = cmpTimeUs(micros(), dmaMotors[i].timer->inputDirectionStampUs);
             if (usSinceInput >= 0 && usSinceInput < dmaMotors[i].dshotTelemetryDeadtimeUs) {
@@ -258,5 +289,22 @@ bool isDshotMotorTelemetryActive(uint8_t motorIndex)
     return dmaMotors[motorIndex].dshotTelemetryActive;
 }
 
-#endif
+#ifdef USE_DSHOT_TELEMETRY_STATS
-#endif
+int16_t getDshotTelemetryMotorInvalidPercent(uint8_t motorIndex)
+{
+    int16_t invalidPercent = 0;
+
+    if (dmaMotors[motorIndex].dshotTelemetryActive) {
+        const uint32_t totalCount = dmaMotors[motorIndex].dshotTelemetryQuality.packetCountSum;
+        const uint32_t invalidCount = dmaMotors[motorIndex].dshotTelemetryQuality.invalidCountSum;
+        if (totalCount > 0) {
+            invalidPercent = lrintf(invalidCount * 10000.0f / totalCount);
+        }
+    } else {
+        invalidPercent = 10000;  // 100.00%
+    }
+    return invalidPercent;
+}
+#endif // USE_DSHOT_TELEMETRY_STATS
+#endif // USE_DSHOT_TELEMETRY
+#endif // USE_DSHOT

src/main/fc/core.c

@@ -1104,6 +1104,15 @@ static FAST_CODE void subTaskMotorUpdate(timeUs_t currentTimeUs)
 
     writeMotors();
 
+#ifdef USE_DSHOT_TELEMETRY_STATS
+    if (debugMode == DEBUG_DSHOT_RPM_ERRORS && useDshotTelemetry) {
+        const uint8_t motorCount = MIN(getMotorCount(), 4);
+        for (uint8_t i = 0; i < motorCount; i++) {
+            debug[i] = getDshotTelemetryMotorInvalidPercent(i);
+        }
+    }
+#endif
+
     DEBUG_SET(DEBUG_PIDLOOP, 2, micros() - startTime);
 }
 

src/main/target/common_post.h

@@ -258,17 +258,17 @@
 #undef USE_GYRO_DATA_ANALYSE
 #endif
 
-#ifndef USE_DSHOT
-#undef USE_DSHOT_TELEMETRY
-#undef USE_RPM_FILTER
-#endif
-
 #ifndef USE_CMS
 #undef USE_CMS_FAILSAFE_MENU
 #endif
 
+#ifndef USE_DSHOT
+#undef USE_DSHOT_TELEMETRY
+#endif
+
 #ifndef USE_DSHOT_TELEMETRY
 #undef USE_RPM_FILTER
+#undef USE_DSHOT_TELEMETRY_STATS
 #endif
 
 #if !defined(USE_BOARD_INFO)

src/main/target/common_pre.h

@@ -54,6 +54,7 @@
 #endif
 #define USE_DSHOT
 #define USE_DSHOT_TELEMETRY
+#define USE_DSHOT_TELEMETRY_STATS
 #define USE_RPM_FILTER
 #define I2C3_OVERCLOCK true
 #define USE_GYRO_DATA_ANALYSE
@@ -80,6 +81,7 @@
 #define USE_FAST_RAM
 #define USE_DSHOT
 #define USE_DSHOT_TELEMETRY
+#define USE_DSHOT_TELEMETRY_STATS
 #define USE_RPM_FILTER
 #define I2C3_OVERCLOCK true
 #define I2C4_OVERCLOCK true