diff --git a/src/main/build/debug.c b/src/main/build/debug.c
index fc4a34ea2d..f6aba1db23 100644
--- a/src/main/build/debug.c
+++ b/src/main/build/debug.c
@@ -112,4 +112,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"ANGLE_MODE",
"ANGLE_TARGET",
"CURRENT_ANGLE",
+ "DSHOT_TELEMETRY_COUNTS",
};
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index b85a3c7c00..e01dd1366a 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -110,6 +110,7 @@ typedef enum {
DEBUG_ANGLE_MODE,
DEBUG_ANGLE_TARGET,
DEBUG_CURRENT_ANGLE,
+ DEBUG_DSHOT_TELEMETRY_COUNTS,
DEBUG_COUNT
} debugType_e;
diff --git a/src/main/drivers/dshot_bitbang_decode.c b/src/main/drivers/dshot_bitbang_decode.c
index 388b4dc32c..65d8689520 100644
--- a/src/main/drivers/dshot_bitbang_decode.c
+++ b/src/main/drivers/dshot_bitbang_decode.c
@@ -1,3 +1,23 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
#include
#include
#include
@@ -8,6 +28,7 @@
#include "common/maths.h"
#include "common/utils.h"
+#include "build/debug.h"
#include "drivers/dshot.h"
#include "drivers/dshot_bitbang_decode.h"
@@ -28,7 +49,8 @@ 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
-
+#define DSHOT_TELEMETRY_START_MARGIN 10
+static uint8_t preambleSkip = 0;
typedef struct bitBandWord_s {
uint32_t value;
@@ -84,6 +106,8 @@ static uint32_t decode_bb_value(uint32_t value, uint16_t buffer[], uint32_t coun
uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
{
+ uint8_t startMargin;
+
#ifdef DEBUG_BBDECODE
memset(sequence, 0, sizeof(sequence));
sequenceIndex = 0;
@@ -94,6 +118,9 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
bitBandWord_t* b = p;
bitBandWord_t* endP = p + (count - MIN_VALID_BBSAMPLES);
+ // Jump forward in the buffer to just before where we anticipate the first zero
+ p += 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) {
@@ -105,6 +132,9 @@ 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);
+
if (p >= endP) {
// not returning telemetry is ok if the esc cpu is
// overburdened. in that case no edge will be found and
@@ -188,6 +218,10 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
sequence[sequenceIndex] = sequence[sequenceIndex] + (nlen) * 3;
sequenceIndex++;
#endif
+
+ // The anticipated edges were observed
+ preambleSkip = startMargin - DSHOT_TELEMETRY_START_MARGIN;
+
if (nlen > 0) {
value <<= nlen;
value |= 1 << (nlen - 1);
@@ -198,6 +232,8 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
{
+ uint8_t startMargin;
+
#ifdef DEBUG_BBDECODE
memset(sequence, 0, sizeof(sequence));
sequenceIndex = 0;
@@ -209,12 +245,15 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
memset(sequence, 0, sizeof(sequence));
int sequenceIndex = 0;
#endif
-
uint16_t lastValue = 0;
uint32_t value = 0;
uint16_t* p = buffer;
uint16_t* endP = p + count - MIN_VALID_BBSAMPLES;
+
+ // Jump forward in the buffer to just before where we anticipate the first zero
+ p += 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) {
@@ -226,10 +265,15 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
}
}
+ startMargin = p - buffer;
+ DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 3, startMargin);
+
if(*p & mask) {
// 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--;
return DSHOT_TELEMETRY_NOEDGE;
}
@@ -268,6 +312,8 @@ 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;
}
@@ -278,9 +324,14 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
#endif
if (nlen < 0) {
+ // Increase the start margin
+ preambleSkip--;
return DSHOT_TELEMETRY_NOEDGE;
}
+ // The anticipated edges were observed
+ preambleSkip = startMargin - DSHOT_TELEMETRY_START_MARGIN;
+
if (nlen > 0) {
value <<= nlen;
value |= 1 << (nlen - 1);
diff --git a/src/main/drivers/dshot_bitbang_impl.h b/src/main/drivers/dshot_bitbang_impl.h
index f81fcc46db..928fbf4fc1 100644
--- a/src/main/drivers/dshot_bitbang_impl.h
+++ b/src/main/drivers/dshot_bitbang_impl.h
@@ -167,6 +167,7 @@ typedef struct bbPort_s {
uint16_t *portInputBuffer;
uint32_t portInputCount;
bool inputActive;
+ volatile bool telemetryPending;
// Misc
#ifdef DEBUG_COUNT_INTERRUPT
diff --git a/src/main/drivers/stm32/dshot_bitbang.c b/src/main/drivers/stm32/dshot_bitbang.c
index 296ddc42c9..04d824ab6e 100644
--- a/src/main/drivers/stm32/dshot_bitbang.c
+++ b/src/main/drivers/stm32/dshot_bitbang.c
@@ -47,6 +47,15 @@
#include "pg/motor.h"
+// DEBUG_DSHOT_TELEMETRY_COUNTS
+// 0 - Count of telemetry packets read
+// 1 - Count of missing edge
+// 2 - Count of reception not complete in time
+// 3 - Number of high bits before telemetry start
+
+// Maximum time to wait for telemetry reception to complete
+#define DSHOT_TELEMETRY_TIMEOUT 2000
+
FAST_DATA_ZERO_INIT bbPacer_t bbPacers[MAX_MOTOR_PACERS]; // TIM1 or TIM8
FAST_DATA_ZERO_INIT int usedMotorPacers = 0;
@@ -324,6 +333,7 @@ FAST_IRQ_HANDLER void bbDMAIrqHandler(dmaChannelDescriptor_t *descriptor)
#ifdef USE_DSHOT_TELEMETRY
if (useDshotTelemetry) {
if (bbPort->direction == DSHOT_BITBANG_DIRECTION_INPUT) {
+ bbPort->telemetryPending = false;
#ifdef DEBUG_COUNT_INTERRUPT
bbPort->inputIrq++;
#endif
@@ -335,6 +345,7 @@ FAST_IRQ_HANDLER void bbDMAIrqHandler(dmaChannelDescriptor_t *descriptor)
// Switch to input
bbSwitchToInput(bbPort);
+ bbPort->telemetryPending = true;
bbTIM_DMACmd(bbPort->timhw->tim, bbPort->dmaSource, ENABLE);
}
@@ -500,62 +511,81 @@ static bool bbUpdateStart(void)
#ifdef USE_DSHOT_TELEMETRY_STATS
const timeMs_t currentTimeMs = millis();
#endif
- timeUs_t currentUs = micros();
- // don't send while telemetry frames might still be incoming
- if (cmpTimeUs(currentUs, lastSendUs) < (timeDelta_t)(40 + 2 * dshotFrameUs)) {
- return false;
- }
+ // Wait for telemetry reception to complete before decode
+ bool telemetryPending;
+ bool telemetryWait = false;
+ const timeUs_t startTimeUs = micros();
- for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
+ do {
+ telemetryPending = false;
+ for (int i = 0; i < usedMotorPorts; i++) {
+ telemetryPending |= bbPorts[i].telemetryPending;
+ }
+
+ telemetryWait |= telemetryPending;
+
+ if (cmpTimeUs(micros(), startTimeUs) > DSHOT_TELEMETRY_TIMEOUT) {
+ return false;
+ }
+ } while (telemetryPending);
+
+ if (telemetryWait) {
+ DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 2, debug[2] + 1);
+ } else {
+ for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
#ifdef USE_DSHOT_CACHE_MGMT
- // Only invalidate the buffer once. If all motors are on a common port they'll share a buffer.
- bool invalidated = false;
- for (int i = 0; i < motorIndex; i++) {
- if (bbMotors[motorIndex].bbPort->portInputBuffer == bbMotors[i].bbPort->portInputBuffer) {
- invalidated = true;
+ // Only invalidate the buffer once. If all motors are on a common port they'll share a buffer.
+ bool invalidated = false;
+ for (int i = 0; i < motorIndex; i++) {
+ if (bbMotors[motorIndex].bbPort->portInputBuffer == bbMotors[i].bbPort->portInputBuffer) {
+ invalidated = true;
+ }
+ }
+ if (!invalidated) {
+ SCB_InvalidateDCache_by_Addr((uint32_t *)bbMotors[motorIndex].bbPort->portInputBuffer,
+ DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_BYTES);
}
- }
- if (!invalidated) {
- SCB_InvalidateDCache_by_Addr((uint32_t *)bbMotors[motorIndex].bbPort->portInputBuffer,
- DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_BYTES);
- }
#endif
#ifdef STM32F4
- uint32_t rawValue = decode_bb_bitband(
- bbMotors[motorIndex].bbPort->portInputBuffer,
- bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort),
- bbMotors[motorIndex].pinIndex);
+ uint32_t rawValue = decode_bb_bitband(
+ bbMotors[motorIndex].bbPort->portInputBuffer,
+ bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort),
+ bbMotors[motorIndex].pinIndex);
#else
- uint32_t rawValue = decode_bb(
- bbMotors[motorIndex].bbPort->portInputBuffer,
- bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort),
- bbMotors[motorIndex].pinIndex);
+ uint32_t rawValue = decode_bb(
+ bbMotors[motorIndex].bbPort->portInputBuffer,
+ bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort),
+ bbMotors[motorIndex].pinIndex);
#endif
- if (rawValue == DSHOT_TELEMETRY_NOEDGE) {
- continue;
- }
- dshotTelemetryState.readCount++;
-
- if (rawValue != DSHOT_TELEMETRY_INVALID) {
- // Check EDT enable or store raw value
- if ((rawValue == 0x0E00) && (dshotCommandGetCurrent(motorIndex) == DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE)) {
- dshotTelemetryState.motorState[motorIndex].telemetryTypes = 1 << DSHOT_TELEMETRY_TYPE_STATE_EVENTS;
- } else {
- dshotTelemetryState.motorState[motorIndex].rawValue = rawValue;
+ if (rawValue == DSHOT_TELEMETRY_NOEDGE) {
+ DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 1, debug[1] + 1);
+ continue;
}
- } else {
- dshotTelemetryState.invalidPacketCount++;
- }
-#ifdef USE_DSHOT_TELEMETRY_STATS
- updateDshotTelemetryQuality(&dshotTelemetryQuality[motorIndex], rawValue != DSHOT_TELEMETRY_INVALID, currentTimeMs);
-#endif
- }
+ DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 0, debug[0] + 1);
+ dshotTelemetryState.readCount++;
- dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_NOT_PROCESSED;
- }
+ if (rawValue != DSHOT_TELEMETRY_INVALID) {
+ // Check EDT enable or store raw value
+ if ((rawValue == 0x0E00) && (dshotCommandGetCurrent(motorIndex) == DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE)) {
+ dshotTelemetryState.motorState[motorIndex].telemetryTypes = 1 << DSHOT_TELEMETRY_TYPE_STATE_EVENTS;
+ } else {
+ dshotTelemetryState.motorState[motorIndex].rawValue = rawValue;
+ }
+ } else {
+ dshotTelemetryState.invalidPacketCount++;
+ }
+#ifdef USE_DSHOT_TELEMETRY_STATS
+ updateDshotTelemetryQuality(&dshotTelemetryQuality[motorIndex], rawValue != DSHOT_TELEMETRY_INVALID, currentTimeMs);
#endif
+ }
+
+ dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_NOT_PROCESSED;
+ }
+#endif
+ }
+
for (int i = 0; i < usedMotorPorts; i++) {
bbDMA_Cmd(&bbPorts[i], DISABLE);
bbOutputDataClear(bbPorts[i].portOutputBuffer);