diff --git a/src/main/build/debug.c b/src/main/build/debug.c
index 6bfe05a099..deb5a9dbb2 100644
--- a/src/main/build/debug.c
+++ b/src/main/build/debug.c
@@ -108,4 +108,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"VTX_MSP",
"GPS_DOP",
"FAILSAFE",
+ "DSHOT_TELEMETRY_COUNTS"
};
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index f43cebd117..1413badcb4 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -106,6 +106,7 @@ typedef enum {
DEBUG_VTX_MSP,
DEBUG_GPS_DOP,
DEBUG_FAILSAFE,
+ DEBUG_DSHOT_TELEMETRY_COUNTS,
DEBUG_COUNT
} debugType_e;
diff --git a/src/main/config/config.c b/src/main/config/config.c
index eb5931eb71..1aa4e9220e 100644
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -630,8 +630,8 @@ void validateAndFixGyroConfig(void)
// check for looptime restrictions based on motor protocol. Motor times have safety margin
float motorUpdateRestriction;
-#if defined(STM32F411xE)
- /* If bidirectional DSHOT is being used on an F411 then force DSHOT300. The motor update restrictions then applied
+#if defined(STM32F4)|| defined(STM32G4)
+ /* If bidirectional DSHOT is being used on an F4 or G4 then force DSHOT300. The motor update restrictions then applied
* will automatically consider the loop time and adjust pid_process_denom appropriately
*/
if (motorConfig()->dev.useDshotTelemetry && (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_DSHOT600)) {
diff --git a/src/main/drivers/dshot_bitbang.c b/src/main/drivers/dshot_bitbang.c
index 296ddc42c9..52eff23825 100644
--- a/src/main/drivers/dshot_bitbang.c
+++ b/src/main/drivers/dshot_bitbang.c
@@ -46,6 +46,16 @@
#include "drivers/timer.h"
#include "pg/motor.h"
+#include "pg/pinio.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,9 +334,12 @@ 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
+ // Disable DMA as telemetry reception is complete
+ bbDMA_Cmd(bbPort, DISABLE);
} else {
#ifdef DEBUG_COUNT_INTERRUPT
bbPort->outputIrq++;
@@ -335,6 +348,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);
}
@@ -441,7 +455,7 @@ static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorPwmProtocolTypes_e p
if (!bbPort || !dmaAllocate(dmaGetIdentifier(bbPort->dmaResource), bbPort->owner.owner, bbPort->owner.resourceIndex)) {
bbDevice.vTable.write = motorWriteNull;
- bbDevice.vTable.updateStart = motorUpdateStartNull;
+ bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
bbDevice.vTable.updateComplete = motorUpdateCompleteNull;
return false;
@@ -493,49 +507,71 @@ static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorPwmProtocolTypes_e p
return true;
}
-static bool bbUpdateStart(void)
+static bool bbTelemetryWait(void)
+{
+ // Wait for telemetry reception to complete
+ bool telemetryPending;
+ bool telemetryWait = false;
+ const timeUs_t startTimeUs = micros();
+
+ do {
+ telemetryPending = false;
+ for (int i = 0; i < usedMotorPorts; i++) {
+ telemetryPending |= bbPorts[i].telemetryPending;
+ }
+
+ telemetryWait |= telemetryPending;
+
+ if (cmpTimeUs(micros(), startTimeUs) > DSHOT_TELEMETRY_TIMEOUT) {
+ break;
+ }
+ } while (telemetryPending);
+
+ if (telemetryWait) {
+ DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 2, debug[2] + 1);
+ }
+
+ return telemetryWait;
+}
+
+static void bbUpdateInit(void)
+{
+ for (int i = 0; i < usedMotorPorts; i++) {
+ bbOutputDataClear(bbPorts[i].portOutputBuffer);
+ }
+}
+
+static bool bbDecodeTelemetry(void)
{
#ifdef USE_DSHOT_TELEMETRY
if (useDshotTelemetry) {
#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;
- }
-
- 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;
- }
- }
- if (!invalidated) {
- SCB_InvalidateDCache_by_Addr((uint32_t *)bbMotors[motorIndex].bbPort->portInputBuffer,
- DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_BYTES);
- }
+ for (int i = 0; i < usedMotorPorts; i++) {
+ bbPort_t *bbPort = &bbPorts[i];
+ SCB_InvalidateDCache_by_Addr((uint32_t *)bbPort->portInputBuffer, DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_BYTES);
+ }
#endif
-
+ for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
#ifdef STM32F4
uint32_t rawValue = decode_bb_bitband(
bbMotors[motorIndex].bbPort->portInputBuffer,
- bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort),
+ bbMotors[motorIndex].bbPort->portInputCount,
bbMotors[motorIndex].pinIndex);
#else
uint32_t rawValue = decode_bb(
bbMotors[motorIndex].bbPort->portInputBuffer,
- bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort),
+ bbMotors[motorIndex].bbPort->portInputCount,
bbMotors[motorIndex].pinIndex);
#endif
if (rawValue == DSHOT_TELEMETRY_NOEDGE) {
+ DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 1, debug[1] + 1);
continue;
}
+ DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 0, debug[0] + 1);
dshotTelemetryState.readCount++;
if (rawValue != DSHOT_TELEMETRY_INVALID) {
@@ -556,10 +592,6 @@ static bool bbUpdateStart(void)
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);
- }
return true;
}
@@ -616,23 +648,11 @@ static void bbUpdateComplete(void)
}
}
-#ifdef USE_DSHOT_CACHE_MGMT
- for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
- // Only clean each buffer once. If all motors are on a common port they'll share a buffer.
- bool clean = false;
- for (int i = 0; i < motorIndex; i++) {
- if (bbMotors[motorIndex].bbPort->portOutputBuffer == bbMotors[i].bbPort->portOutputBuffer) {
- clean = true;
- }
- }
- if (!clean) {
- SCB_CleanDCache_by_Addr(bbMotors[motorIndex].bbPort->portOutputBuffer, MOTOR_DSHOT_BUF_CACHE_ALIGN_BYTES);
- }
- }
-#endif
-
for (int i = 0; i < usedMotorPorts; i++) {
bbPort_t *bbPort = &bbPorts[i];
+#ifdef USE_DSHOT_CACHE_MGMT
+ SCB_CleanDCache_by_Addr(bbPort->portOutputBuffer, MOTOR_DSHOT_BUF_CACHE_ALIGN_BYTES);
+#endif
#ifdef USE_DSHOT_TELEMETRY
if (useDshotTelemetry) {
@@ -708,7 +728,9 @@ static motorVTable_t bbVTable = {
.enable = bbEnableMotors,
.disable = bbDisableMotors,
.isMotorEnabled = bbIsMotorEnabled,
- .updateStart = bbUpdateStart,
+ .telemetryWait = bbTelemetryWait,
+ .decodeTelemetry = bbDecodeTelemetry,
+ .updateInit = bbUpdateInit,
.write = bbWrite,
.writeInt = bbWriteInt,
.updateComplete = bbUpdateComplete,
@@ -755,7 +777,7 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t
if (!IOIsFreeOrPreinit(io)) {
/* not enough motors initialised for the mixer or a break in the motors */
bbDevice.vTable.write = motorWriteNull;
- bbDevice.vTable.updateStart = motorUpdateStartNull;
+ bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
bbDevice.vTable.updateComplete = motorUpdateCompleteNull;
bbStatus = DSHOT_BITBANG_STATUS_MOTOR_PIN_CONFLICT;
return NULL;
diff --git a/src/main/drivers/dshot_bitbang_decode.c b/src/main/drivers/dshot_bitbang_decode.c
index 388b4dc32c..591436cdee 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)
}
}
- if(*p & mask) {
+ startMargin = p - buffer;
+ 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
// 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/dshot_command.c b/src/main/drivers/dshot_command.c
index ded30ad706..cd04ccc65f 100644
--- a/src/main/drivers/dshot_command.c
+++ b/src/main/drivers/dshot_command.c
@@ -218,7 +218,7 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshot
#ifdef USE_DSHOT_TELEMETRY
timeUs_t timeoutUs = micros() + 1000;
- while (!motorGetVTable().updateStart() &&
+ while (!motorGetVTable().decodeTelemetry() &&
cmpTimeUs(timeoutUs, micros()) > 0);
#endif
for (uint8_t i = 0; i < motorDeviceCount(); i++) {
diff --git a/src/main/drivers/dshot_dpwm.c b/src/main/drivers/dshot_dpwm.c
index de26d589da..a7ab462f51 100644
--- a/src/main/drivers/dshot_dpwm.c
+++ b/src/main/drivers/dshot_dpwm.c
@@ -140,7 +140,7 @@ static motorVTable_t dshotPwmVTable = {
.enable = dshotPwmEnableMotors,
.disable = dshotPwmDisableMotors,
.isMotorEnabled = dshotPwmIsMotorEnabled,
- .updateStart = motorUpdateStartNull, // May be updated after copying
+ .decodeTelemetry = motorDecodeTelemetryNull, // May be updated after copying
.write = dshotWrite,
.writeInt = dshotWriteInt,
.updateComplete = pwmCompleteDshotMotorUpdate,
@@ -160,7 +160,7 @@ motorDevice_t *dshotPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
#ifdef USE_DSHOT_TELEMETRY
useDshotTelemetry = motorConfig->useDshotTelemetry;
- dshotPwmDevice.vTable.updateStart = pwmStartDshotMotorUpdate;
+ dshotPwmDevice.vTable.decodeTelemetry = pwmTelemetryDecode;
#endif
switch (motorConfig->motorPwmProtocol) {
diff --git a/src/main/drivers/dshot_dpwm.h b/src/main/drivers/dshot_dpwm.h
index dc1b190072..f2d963a40a 100644
--- a/src/main/drivers/dshot_dpwm.h
+++ b/src/main/drivers/dshot_dpwm.h
@@ -159,7 +159,7 @@ motorDmaOutput_t *getMotorDmaOutput(uint8_t index);
void pwmWriteDshotInt(uint8_t index, uint16_t value);
bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
#ifdef USE_DSHOT_TELEMETRY
-bool pwmStartDshotMotorUpdate(void);
+bool pwmTelemetryDecode(void);
#endif
void pwmCompleteDshotMotorUpdate(void);
diff --git a/src/main/drivers/motor.c b/src/main/drivers/motor.c
index f6917d0b47..e604303f0d 100644
--- a/src/main/drivers/motor.c
+++ b/src/main/drivers/motor.c
@@ -60,15 +60,49 @@ void motorWriteAll(float *values)
{
#ifdef USE_PWM_OUTPUT
if (motorDevice->enabled) {
+#ifdef USE_DSHOT_BITBANG
+ if (isDshotBitbangActive(&motorConfig()->dev)) {
+ // Initialise the output buffers
+ if (motorDevice->vTable.updateInit) {
+ motorDevice->vTable.updateInit();
+ }
+
+ // Update the motor data
+ for (int i = 0; i < motorDevice->count; i++) {
+ motorDevice->vTable.write(i, values[i]);
+ }
+
+ // Don't attempt to write commands to the motors if telemetry is still being received
+ if (motorDevice->vTable.telemetryWait) {
+ (void)motorDevice->vTable.telemetryWait();
+ }
+
+ // Trigger the transmission of the motor data
+ motorDevice->vTable.updateComplete();
+
+ // Perform the decode of the last data received
+ // New data will be received once the send of motor data, triggered above, completes
#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
- if (!motorDevice->vTable.updateStart()) {
- return;
- }
+ motorDevice->vTable.decodeTelemetry();
#endif
- for (int i = 0; i < motorDevice->count; i++) {
- motorDevice->vTable.write(i, values[i]);
+ } else
+#endif
+ {
+ // Perform the decode of the last data received
+ // New data will be received once the send of motor data, triggered above, completes
+#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
+ motorDevice->vTable.decodeTelemetry();
+#endif
+
+ // Update the motor data
+ for (int i = 0; i < motorDevice->count; i++) {
+ motorDevice->vTable.write(i, values[i]);
+ }
+
+ // Trigger the transmission of the motor data
+ motorDevice->vTable.updateComplete();
+
}
- motorDevice->vTable.updateComplete();
}
#else
UNUSED(values);
@@ -193,7 +227,7 @@ static bool motorIsEnabledNull(uint8_t index)
return false;
}
-bool motorUpdateStartNull(void)
+bool motorDecodeTelemetryNull(void)
{
return true;
}
@@ -235,7 +269,7 @@ static const motorVTable_t motorNullVTable = {
.enable = motorEnableNull,
.disable = motorDisableNull,
.isMotorEnabled = motorIsEnabledNull,
- .updateStart = motorUpdateStartNull,
+ .decodeTelemetry = motorDecodeTelemetryNull,
.write = motorWriteNull,
.writeInt = motorWriteIntNull,
.updateComplete = motorUpdateCompleteNull,
diff --git a/src/main/drivers/motor.h b/src/main/drivers/motor.h
index b8dcabaf39..fb9846aa6f 100644
--- a/src/main/drivers/motor.h
+++ b/src/main/drivers/motor.h
@@ -50,7 +50,9 @@ typedef struct motorVTable_s {
bool (*enable)(void);
void (*disable)(void);
bool (*isMotorEnabled)(uint8_t index);
- bool (*updateStart)(void);
+ bool (*telemetryWait)(void);
+ bool (*decodeTelemetry)(void);
+ void (*updateInit)(void);
void (*write)(uint8_t index, float value);
void (*writeInt)(uint8_t index, uint16_t value);
void (*updateComplete)(void);
@@ -70,7 +72,7 @@ typedef struct motorDevice_s {
void motorPostInitNull();
void motorWriteNull(uint8_t index, float value);
-bool motorUpdateStartNull(void);
+bool motorDecodeTelemetryNull(void);
void motorUpdateCompleteNull(void);
void motorPostInit();
diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index 1a031322f0..5c996b7bbd 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -215,7 +215,7 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
}
motorPwmDevice.vTable.write = pwmWriteStandard;
- motorPwmDevice.vTable.updateStart = motorUpdateStartNull;
+ motorPwmDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
motorPwmDevice.vTable.updateComplete = useUnsyncedPwm ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate;
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
diff --git a/src/main/drivers/pwm_output_dshot_shared.c b/src/main/drivers/pwm_output_dshot_shared.c
index 6d92deb338..c32d3ae366 100644
--- a/src/main/drivers/pwm_output_dshot_shared.c
+++ b/src/main/drivers/pwm_output_dshot_shared.c
@@ -176,7 +176,11 @@ static uint32_t decodeTelemetryPacket(uint32_t buffer[], uint32_t count)
#endif
#ifdef USE_DSHOT_TELEMETRY
-FAST_CODE_NOINLINE bool pwmStartDshotMotorUpdate(void)
+/**
+ * Process dshot telemetry packets before switching the channels back to outputs
+ *
+*/
+FAST_CODE_NOINLINE bool pwmTelemetryDecode(void)
{
if (!useDshotTelemetry) {
return true;
diff --git a/src/main/drivers/pwm_output_dshot_shared.h b/src/main/drivers/pwm_output_dshot_shared.h
index 9e4ef0faf8..7c282d5008 100644
--- a/src/main/drivers/pwm_output_dshot_shared.h
+++ b/src/main/drivers/pwm_output_dshot_shared.h
@@ -59,7 +59,7 @@ void pwmDshotSetDirectionOutput(
#endif
);
-bool pwmStartDshotMotorUpdate(void);
+bool pwmTelemetryDecode(void);
#endif
#endif
diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c
index 0a550430a1..24987e560c 100644
--- a/src/main/target/SITL/target.c
+++ b/src/main/target/SITL/target.c
@@ -520,7 +520,7 @@ static motorDevice_t motorPwmDevice = {
.enable = pwmEnableMotors,
.disable = pwmDisableMotors,
.isMotorEnabled = pwmIsMotorEnabled,
- .updateStart = motorUpdateStartNull,
+ .decodeTelemetry = motorDecodeTelemetryNull,
.write = pwmWriteMotor,
.writeInt = pwmWriteMotorInt,
.updateComplete = pwmCompleteMotorUpdate,