Merge pull request #7841 from etracer65/dshot_bidir_quality_stats
Add DSHOT telemetry motor level packet stats
This commit is contained in:
commit
55a5d70cc8
|
@ -86,4 +86,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
|
|||
"AC_CORRECTION",
|
||||
"AC_ERROR",
|
||||
"DUAL_GYRO_SCALED",
|
||||
"DSHOT_RPM_ERRORS",
|
||||
};
|
||||
|
|
|
@ -102,6 +102,7 @@ typedef enum {
|
|||
DEBUG_AC_CORRECTION,
|
||||
DEBUG_AC_ERROR,
|
||||
DEBUG_DUAL_GYRO_SCALED,
|
||||
DEBUG_DSHOT_RPM_ERRORS,
|
||||
DEBUG_COUNT
|
||||
} debugType_e;
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue