From 644d5713dfaf144956b1cd962097b8bec20ef773 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Mon, 18 Mar 2019 11:45:46 -0400 Subject: [PATCH] Add DSHOT telemetry motor level packet stats Captures packet quality statistics per motor to provide a platform for troubleshooting and in-flight alarms or warnings. Continuously monitors and captures packet stats over the past second to be used to determine DSHOT telemetry link quality. Update the CLI to move the DSHOT telemetry data/stats out of the `status` command and into a dedicated `dshot_telemetry_info` command. Reformatted the motor data to include the invalid packet percentages. Will aid in debugging by identifying ESC's that may be misconfigured (not supplying data), or those with problems generating high invalid packet percentages. Data can later be used to generate OSD and beeper-based warnings if invalid packet percentages exceed thresholds. Included a blackbox logging debug mode (`set debug_mode = DSHOT_RPM_ERRORS`) to record the per-motor invalid packet percentages in hundredths of a percent (so 123 is 1.23%). --- src/main/build/debug.c | 1 + src/main/build/debug.h | 1 + src/main/cli/cli.c | 77 +++++++++++++++------- src/main/drivers/pwm_output.h | 23 +++++++ src/main/drivers/pwm_output_dshot_shared.c | 52 ++++++++++++++- src/main/fc/core.c | 9 +++ src/main/target/common_post.h | 10 +-- src/main/target/common_pre.h | 2 + 8 files changed, 146 insertions(+), 29 deletions(-) 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 500fb78ac..67ec048d5 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 ba2b26ff1..a8c1895fb 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 46beae91e..d6c6ff55f 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