diff --git a/src/main/build/debug.c b/src/main/build/debug.c index e7efff66e..0eba50aea 100644 --- a/src/main/build/debug.c +++ b/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", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 6126f5e21..ddbc52f29 100644 --- a/src/main/build/debug.h +++ b/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; diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index de08dbb57..1c113b2e7 100644 --- a/src/main/cli/cli.c +++ b/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; idev.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; idev.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)", " +", cliDshotProg), #endif diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index f5da5d162..9da6fd98d 100644 --- a/src/main/drivers/pwm_output.h +++ b/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 diff --git a/src/main/drivers/pwm_output_dshot_shared.c b/src/main/drivers/pwm_output_dshot_shared.c index 83f45daa1..0f6f4bbe6 100644 --- a/src/main/drivers/pwm_output_dshot_shared.c +++ b/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 -#endif +#ifdef USE_DSHOT_TELEMETRY_STATS +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 diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 85d33fb75..a598907e8 100644 --- a/src/main/fc/core.c +++ b/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); } diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 1dd29e6f9..3c9458acb 100644 --- a/src/main/target/common_post.h +++ b/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) diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index e65a3248f..ff1001e5c 100644 --- a/src/main/target/common_pre.h +++ b/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