diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index ee895408ae..1aaba47b69 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -847,7 +847,6 @@ const clivalue_t valueTable[] = { #ifdef USE_DSHOT_BITBANG { "dshot_bitbang", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON_AUTO }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.useDshotBitbang) }, { "dshot_bitbang_timer", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DSHOT_BITBANGED_TIMER }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.useDshotBitbangedTimer) }, - { "dshot_telemetry_start_margin", VAR_UINT8 | HARDWARE_VALUE , .config.minmaxUnsigned = { 0, 100 }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.telemetryStartMargin) }, #endif #endif { PARAM_NAME_USE_UNSYNCED_PWM, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.useUnsyncedPwm) }, diff --git a/src/main/drivers/dshot_bitbang_decode.c b/src/main/drivers/dshot_bitbang_decode.c index 8b723acd84..54e6aa178f 100644 --- a/src/main/drivers/dshot_bitbang_decode.c +++ b/src/main/drivers/dshot_bitbang_decode.c @@ -31,6 +31,7 @@ #include "build/debug.h" #include "drivers/dshot.h" #include "drivers/dshot_bitbang_decode.h" +#include "drivers/time.h" #define MIN_VALID_BBSAMPLES ((21 - 2) * 3) #define MAX_VALID_BBSAMPLES ((21 + 2) * 3) @@ -49,6 +50,15 @@ uint16_t bbBuffer[134]; #define BITBAND_SRAM_BASE 0x22000000 #define BITBAND_SRAM(a,b) ((BITBAND_SRAM_BASE + (((a)-BITBAND_SRAM_REF)<<5) + ((b)<<2))) // Convert SRAM address +// Period at which to check preamble length +#define MARGIN_CHECK_INTERVAL_US 500000 + +// Target 5 clock cycles of input data ahead of leading edge +#define DSHOT_TELEMETRY_START_MARGIN 5 + +static uint32_t minMargin = UINT32_MAX; +static timeUs_t nextMarginCheckUs = 0; + static uint8_t preambleSkip = 0; typedef struct bitBandWord_s { @@ -102,11 +112,10 @@ static uint32_t decode_bb_value(uint32_t value, uint16_t buffer[], uint32_t coun return value; } - +#ifdef USE_DSHOT_BITBAND uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit) { - uint8_t startMargin; - + timeUs_t now = micros(); #ifdef DEBUG_BBDECODE memset(sequence, 0, sizeof(sequence)); sequenceIndex = 0; @@ -120,6 +129,8 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit) // Jump forward in the buffer to just before where we anticipate the first zero p += preambleSkip; + DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 3, preambleSkip); + // Eliminate leading high signal level by looking for first zero bit in data stream. // Manual loop unrolling and branch hinting to produce faster code. while (p < endP) { @@ -131,13 +142,16 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit) } } - startMargin = p - b; - DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 3, startMargin); + const uint32_t startMargin = p - b; if (p >= endP) { // not returning telemetry is ok if the esc cpu is // overburdened. in that case no edge will be found and // BB_NOEDGE indicates the condition to caller + if (preambleSkip > 0) { + // Increase the start margin + preambleSkip--; + } return DSHOT_TELEMETRY_NOEDGE; } @@ -203,6 +217,7 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit) } } + // length of last sequence has to be inferred since the last bit with inverted dshot is high if (bits < 18) { return DSHOT_TELEMETRY_NOEDGE; } @@ -213,20 +228,34 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit) return DSHOT_TELEMETRY_NOEDGE; } + // Data appears valid + if (startMargin < minMargin) { + minMargin = startMargin; + } + + if (cmpTimeUs(now, nextMarginCheckUs) >= 0) { + nextMarginCheckUs += MARGIN_CHECK_INTERVAL_US; + + // Handle a skipped check + if (nextMarginCheckUs < now) { + nextMarginCheckUs = now + DSHOT_TELEMETRY_START_MARGIN; + } + + if (minMargin > DSHOT_TELEMETRY_START_MARGIN) { + preambleSkip = minMargin - DSHOT_TELEMETRY_START_MARGIN; + } else { + preambleSkip = 0; + } + + minMargin = UINT32_MAX; + } + #ifdef DEBUG_BBDECODE sequence[sequenceIndex] = sequence[sequenceIndex] + (nlen) * 3; sequenceIndex++; #endif // The anticipated edges were observed - - // Attempt to skip the preamble ahead of the telemetry to save CPU - if (startMargin > motorConfig()->dev.telemetryStartMargin) { - preambleSkip = startMargin - motorConfig()->dev.telemetryStartMargin; - } else { - preambleSkip = 0; - } - if (nlen > 0) { value <<= nlen; value |= 1 << (nlen - 1); @@ -235,10 +264,11 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit) return decode_bb_value(value, buffer, count, bit); } +#else // USE_DSHOT_BITBAND + FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit) { - uint8_t startMargin; - + timeUs_t now = micros(); #ifdef DEBUG_BBDECODE memset(sequence, 0, sizeof(sequence)); sequenceIndex = 0; @@ -259,6 +289,8 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit) // Jump forward in the buffer to just before where we anticipate the first zero p += preambleSkip; + DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 3, preambleSkip); + // Eliminate leading high signal level by looking for first zero bit in data stream. // Manual loop unrolling and branch hinting to produce faster code. while (p < endP) { @@ -269,16 +301,17 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit) break; } } - - startMargin = p - buffer; - DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 3, startMargin); + const uint32_t startMargin = p - buffer; if (p >= endP) { // not returning telemetry is ok if the esc cpu is // overburdened. in that case no edge will be found and // BB_NOEDGE indicates the condition to caller // Increase the start margin - preambleSkip--; + if (preambleSkip > 0) { + // Increase the start margin + preambleSkip--; + } return DSHOT_TELEMETRY_NOEDGE; } @@ -317,32 +350,43 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit) // length of last sequence has to be inferred since the last bit with inverted dshot is high if (bits < 18) { - // Increase the start margin - preambleSkip--; return DSHOT_TELEMETRY_NOEDGE; } + // length of last sequence has to be inferred since the last bit with inverted dshot is high const int nlen = 21 - bits; + if (nlen < 0) { + return DSHOT_TELEMETRY_NOEDGE; + } + + // Data appears valid + if (startMargin < minMargin) { + minMargin = startMargin; + } + + if (cmpTimeUs(now, nextMarginCheckUs) >= 0) { + nextMarginCheckUs += MARGIN_CHECK_INTERVAL_US; + + // Handle a skipped check + if (nextMarginCheckUs < now) { + nextMarginCheckUs = now + DSHOT_TELEMETRY_START_MARGIN; + } + + if (minMargin > DSHOT_TELEMETRY_START_MARGIN) { + preambleSkip = minMargin - DSHOT_TELEMETRY_START_MARGIN; + } else { + preambleSkip = 0; + } + + minMargin = UINT32_MAX; + } + #ifdef DEBUG_BBDECODE sequence[sequenceIndex] = sequence[sequenceIndex] + (nlen) * 3; sequenceIndex++; #endif - if (nlen < 0) { - // Increase the start margin - preambleSkip--; - return DSHOT_TELEMETRY_NOEDGE; - } - // The anticipated edges were observed - - // Attempt to skip the preamble ahead of the telemetry to save CPU - if (startMargin > motorConfig()->dev.telemetryStartMargin) { - preambleSkip = startMargin - motorConfig()->dev.telemetryStartMargin; - } else { - preambleSkip = 0; - } - if (nlen > 0) { value <<= nlen; value |= 1 << (nlen - 1); @@ -350,5 +394,6 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit) return decode_bb_value(value, buffer, count, bit); } +#endif // USE_DSHOT_BITBAND #endif diff --git a/src/main/drivers/dshot_bitbang_decode.h b/src/main/drivers/dshot_bitbang_decode.h index cddb5764b8..cb7502e859 100644 --- a/src/main/drivers/dshot_bitbang_decode.h +++ b/src/main/drivers/dshot_bitbang_decode.h @@ -18,14 +18,14 @@ * If not, see . */ - +#include "platform.h" #if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY) - - -uint32_t decode_bb(uint16_t buffer[], uint32_t count, uint32_t mask); +#ifdef USE_DSHOT_BITBAND uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit); - +#else +uint32_t decode_bb(uint16_t buffer[], uint32_t count, uint32_t mask); +#endif #endif diff --git a/src/main/pg/motor.c b/src/main/pg/motor.c index 1707c64d96..a8493f27a2 100644 --- a/src/main/pg/motor.c +++ b/src/main/pg/motor.c @@ -45,10 +45,6 @@ #define DEFAULT_DSHOT_BURST DSHOT_DMAR_OFF #endif -#if !defined(DSHOT_TELEMETRY_START_MARGIN) -#define DSHOT_TELEMETRY_START_MARGIN 10 -#endif - PG_REGISTER_WITH_RESET_FN(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 2); void pgResetFn_motorConfig(motorConfig_t *motorConfig) @@ -97,7 +93,6 @@ void pgResetFn_motorConfig(motorConfig_t *motorConfig) #ifdef USE_DSHOT_BITBANG motorConfig->dev.useDshotBitbang = DSHOT_BITBANG_DEFAULT; motorConfig->dev.useDshotBitbangedTimer = DSHOT_BITBANGED_TIMER_DEFAULT; - motorConfig->dev.telemetryStartMargin = DSHOT_TELEMETRY_START_MARGIN; #endif } diff --git a/src/main/pg/motor.h b/src/main/pg/motor.h index 929b419567..a2ea0fb551 100644 --- a/src/main/pg/motor.h +++ b/src/main/pg/motor.h @@ -50,7 +50,6 @@ typedef struct motorDevConfig_s { uint8_t useDshotBitbang; uint8_t useDshotBitbangedTimer; uint8_t motorOutputReordering[MAX_SUPPORTED_MOTORS]; // Reindexing motors for "remap motors" feature in Configurator - uint8_t telemetryStartMargin; } motorDevConfig_t; typedef struct motorConfig_s { diff --git a/src/main/target/STM32F405/target.h b/src/main/target/STM32F405/target.h index 684c5da42b..7fd5155bbc 100644 --- a/src/main/target/STM32F405/target.h +++ b/src/main/target/STM32F405/target.h @@ -57,6 +57,8 @@ #define USE_I2C #define I2C_FULL_RECONFIGURABILITY +#define USE_DSHOT_BITBAND + #define USE_BEEPER #define USE_SPI diff --git a/src/main/target/STM32F411/target.h b/src/main/target/STM32F411/target.h index 06f7467cac..b349300777 100644 --- a/src/main/target/STM32F411/target.h +++ b/src/main/target/STM32F411/target.h @@ -49,6 +49,8 @@ #define USE_I2C #define I2C_FULL_RECONFIGURABILITY +#define USE_DSHOT_BITBAND + #define USE_BEEPER #ifdef USE_SDCARD