Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 05:15:25 +03:00
Code Issues Wiki Activity

Add DSHOT telemetry motor level packet stats

Browse source 
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%).
This commit is contained in:
Bruce Luckcuck 2019-03-18 11:45:46 -04:00
parent 01e0c95810
commit 644d5713df
8 changed files with 146 additions and 29 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/build/debug.c
View file

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

1
src/main/build/debug.h
View file

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

77
src/main/cli/cli.c
View file

@ -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

23
src/main/drivers/pwm_output.h
View file

@ -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

52
src/main/drivers/pwm_output_dshot_shared.c
View file

@ -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

9
src/main/fc/core.c
View file

@ -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);
}

10
src/main/target/common_post.h
View file

@ -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)

2
src/main/target/common_pre.h
View file

@ -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