From 5a759c56efd0d3fee9a35db1ca02f3a92ec4b581 Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Tue, 19 Mar 2019 13:02:44 +0100 Subject: [PATCH] avoid dshot telemetry collisions actually use calculated deadtime fix whitespace and return value fix ws Address review feedback fix ws --- src/main/drivers/pwm_output.c | 11 ++- src/main/drivers/pwm_output.h | 12 ++- src/main/drivers/pwm_output_dshot.c | 9 ++- src/main/drivers/pwm_output_dshot_hal.c | 9 ++- src/main/drivers/pwm_output_dshot_shared.c | 86 ++++++++++++---------- src/main/drivers/pwm_output_dshot_shared.h | 2 +- src/main/flight/mixer.c | 4 +- 7 files changed, 82 insertions(+), 51 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 506a89dbe4..5a5af9e12b 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -224,9 +224,10 @@ bool pwmAreMotorsEnabled(void) } #ifdef USE_DSHOT_TELEMETRY -static void pwmStartWriteUnused(uint8_t motorCount) +static bool pwmStartWriteUnused(uint8_t motorCount) { UNUSED(motorCount); + return true; } #endif @@ -253,9 +254,9 @@ void pwmCompleteMotorUpdate(uint8_t motorCount) } #ifdef USE_DSHOT_TELEMETRY -void pwmStartMotorUpdate(uint8_t motorCount) +bool pwmStartMotorUpdate(uint8_t motorCount) { - pwmStartWrite(motorCount); + return pwmStartWrite(motorCount); } #endif @@ -521,7 +522,9 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo delayMicroseconds(DSHOT_COMMAND_DELAY_US); #ifdef USE_DSHOT_TELEMETRY - pwmStartDshotMotorUpdate(motorCount); + timeUs_t currentTimeUs = micros(); + while (!pwmStartDshotMotorUpdate(motorCount) && + cmpTimeUs(micros(), currentTimeUs) < 1000); #endif for (uint8_t i = 0; i < motorCount; i++) { if ((i == index) || (index == ALL_MOTORS)) { diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 62bd4b487a..87244aad0b 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -22,6 +22,8 @@ #include "platform.h" +#include "common/time.h" + #include "drivers/io_types.h" #include "drivers/pwm_output_counts.h" #include "drivers/timer.h" @@ -112,6 +114,8 @@ typedef enum { #define PROSHOT_BASE_SYMBOL 24 // 1uS #define PROSHOT_BIT_WIDTH 3 #define MOTOR_NIBBLE_LENGTH_PROSHOT 96 // 4uS + +#define DSHOT_TELEMETRY_DEADTIME_US (2 * 30 + 10) // 2 * 30uS to switch lines plus 10us grace period #endif @@ -128,6 +132,7 @@ typedef struct { #endif uint16_t dmaBurstLength; uint32_t dmaBurstBuffer[DSHOT_DMA_BUFFER_SIZE * 4]; + timeUs_t inputDirectionStampUs; #endif uint16_t timerDmaSources; } motorDmaTimer_t; @@ -146,6 +151,7 @@ typedef struct { volatile bool isInput; volatile bool hasTelemetry; uint16_t dshotTelemetryValue; + timeDelta_t dshotTelemetryDeadtimeUs; bool dshotTelemetryActive; #ifdef USE_HAL_DRIVER LL_TIM_OC_InitTypeDef ocInitStruct; @@ -183,7 +189,7 @@ motorDmaOutput_t *getMotorDmaOutput(uint8_t index); struct timerHardware_s; typedef void pwmWriteFn(uint8_t index, float value); // function pointer used to write motors typedef void pwmCompleteWriteFn(uint8_t motorCount); // function pointer used after motors are written -typedef void pwmStartWriteFn(uint8_t motorCount); // function pointer used before motors are written +typedef bool pwmStartWriteFn(uint8_t motorCount); // function pointer used before motors are written typedef struct { volatile timCCR_t *ccr; @@ -244,7 +250,7 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo void pwmWriteDshotInt(uint8_t index, uint16_t value); void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output); #ifdef USE_DSHOT_TELEMETRY -void pwmStartDshotMotorUpdate(uint8_t motorCount); +bool pwmStartDshotMotorUpdate(uint8_t motorCount); #endif void pwmCompleteDshotMotorUpdate(uint8_t motorCount); @@ -268,7 +274,7 @@ void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, void pwmWriteMotor(uint8_t index, float value); void pwmShutdownPulsesForAllMotors(uint8_t motorCount); void pwmCompleteMotorUpdate(uint8_t motorCount); -void pwmStartMotorUpdate(uint8_t motorCount); +bool pwmStartMotorUpdate(uint8_t motorCount); void pwmWriteServo(uint8_t index, float value); diff --git a/src/main/drivers/pwm_output_dshot.c b/src/main/drivers/pwm_output_dshot.c index 50bee313e8..43f78bfb20 100644 --- a/src/main/drivers/pwm_output_dshot.c +++ b/src/main/drivers/pwm_output_dshot.c @@ -96,8 +96,9 @@ FAST_CODE void pwmDshotSetDirectionOutput( DMA_DeInit(dmaRef); #ifdef USE_DSHOT_TELEMETRY - motor->isInput = !output; if (!output) { + motor->isInput = true; + motor->timer->inputDirectionStampUs = micros(); TIM_ICInit(timer, &motor->icInitStruct); #if defined(STM32F3) @@ -111,6 +112,9 @@ FAST_CODE void pwmDshotSetDirectionOutput( UNUSED(output); #endif { +#ifdef USE_DSHOT_TELEMETRY + motor->isInput = false; +#endif timerOCPreloadConfig(timer, timerHardware->channel, TIM_OCPreload_Disable); timerOCInit(timer, timerHardware->channel, pOcInit); timerOCPreloadConfig(timer, timerHardware->channel, TIM_OCPreload_Enable); @@ -388,6 +392,9 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m #ifdef USE_DSHOT_TELEMETRY motor->dmaInputLen = motor->useProshot ? PROSHOT_TELEMETRY_INPUT_LEN : DSHOT_TELEMETRY_INPUT_LEN; + motor->dshotTelemetryDeadtimeUs = DSHOT_TELEMETRY_DEADTIME_US + 1000000 * + ( 2 + (motor->useProshot ? 4 * MOTOR_NIBBLE_LENGTH_PROSHOT : 16 * MOTOR_BITLENGTH)) + / getDshotHz(pwmProtocolType); pwmDshotSetDirectionOutput(motor, true); #else pwmDshotSetDirectionOutput(motor, true, &OCINIT, &DMAINIT); diff --git a/src/main/drivers/pwm_output_dshot_hal.c b/src/main/drivers/pwm_output_dshot_hal.c index 7c79f13084..3541922219 100644 --- a/src/main/drivers/pwm_output_dshot_hal.c +++ b/src/main/drivers/pwm_output_dshot_hal.c @@ -98,8 +98,9 @@ void pwmDshotSetDirectionOutput( LL_EX_DMA_DeInit(motor->dmaRef); #ifdef USE_DSHOT_TELEMETRY - motor->isInput = !output; if (!output) { + motor->isInput = true; + motor->timer->inputDirectionStampUs = micros(); LL_TIM_IC_Init(timer, motor->llChannel, &motor->icInitStruct); motor->dmaInitStruct.Direction = LL_DMA_DIRECTION_PERIPH_TO_MEMORY; } else @@ -107,6 +108,9 @@ void pwmDshotSetDirectionOutput( UNUSED(output); #endif { +#ifdef USE_DSHOT_TELEMETRY + motor->isInput = false; +#endif LL_TIM_OC_DisablePreload(timer, motor->llChannel); LL_TIM_OC_Init(timer, motor->llChannel, pOcInit); LL_TIM_OC_EnablePreload(timer, motor->llChannel); @@ -351,6 +355,9 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m #ifdef USE_DSHOT_TELEMETRY motor->dmaInputLen = motor->useProshot ? PROSHOT_TELEMETRY_INPUT_LEN : DSHOT_TELEMETRY_INPUT_LEN; + motor->dshotTelemetryDeadtimeUs = DSHOT_TELEMETRY_DEADTIME_US + 1000000 * + ( 2 + (motor->useProshot ? 4 * MOTOR_NIBBLE_LENGTH_PROSHOT : 16 * MOTOR_BITLENGTH)) + / getDshotHz(pwmProtocolType); pwmDshotSetDirectionOutput(motor, true); #else pwmDshotSetDirectionOutput(motor, true, &OCINIT, &DMAINIT); diff --git a/src/main/drivers/pwm_output_dshot_shared.c b/src/main/drivers/pwm_output_dshot_shared.c index c2c250554f..83f45daa1f 100644 --- a/src/main/drivers/pwm_output_dshot_shared.c +++ b/src/main/drivers/pwm_output_dshot_shared.c @@ -203,48 +203,54 @@ FAST_CODE void pwmDshotSetDirectionOutput( #ifdef USE_DSHOT_TELEMETRY -void pwmStartDshotMotorUpdate(uint8_t motorCount) +bool pwmStartDshotMotorUpdate(uint8_t motorCount) { - if (useDshotTelemetry) { - for (int i = 0; i < motorCount; i++) { - if (dmaMotors[i].hasTelemetry) { -#ifdef STM32F7 - uint32_t edges = LL_EX_DMA_GetDataLength(dmaMotors[i].dmaRef); -#else - uint32_t edges = DMA_GetCurrDataCounter(dmaMotors[i].dmaRef); -#endif - uint16_t value = 0xffff; - if (edges == 0) { - if (dmaMotors[i].useProshot) { - value = decodeProshotPacket(dmaMotors[i].dmaBuffer); - } else { - value = decodeDshotPacket(dmaMotors[i].dmaBuffer); - } - } - if (value != 0xffff) { - dmaMotors[i].dshotTelemetryValue = value; - dmaMotors[i].dshotTelemetryActive = true; - if (i < 4) { - DEBUG_SET(DEBUG_DSHOT_RPM_TELEMETRY, i, value); - } - } else { - dshotInvalidPacketCount++; - if (i == 0) { - memcpy(inputBuffer,dmaMotors[i].dmaBuffer,sizeof(inputBuffer)); - } - } - dmaMotors[i].hasTelemetry = false; - } else { -#ifdef STM32F7 - LL_EX_TIM_DisableIT(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource); -#else - TIM_DMACmd(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource, DISABLE); -#endif - } - pwmDshotSetDirectionOutput(&dmaMotors[i], true); - } - dshotEnableChannels(motorCount); + if (!useDshotTelemetry) { + return true; } + for (int i = 0; i < motorCount; i++) { + if (dmaMotors[i].hasTelemetry) { +#ifdef STM32F7 + uint32_t edges = LL_EX_DMA_GetDataLength(dmaMotors[i].dmaRef); +#else + uint32_t edges = DMA_GetCurrDataCounter(dmaMotors[i].dmaRef); +#endif + uint16_t value = 0xffff; + if (edges == 0) { + if (dmaMotors[i].useProshot) { + value = decodeProshotPacket(dmaMotors[i].dmaBuffer); + } else { + value = decodeDshotPacket(dmaMotors[i].dmaBuffer); + } + } + if (value != 0xffff) { + dmaMotors[i].dshotTelemetryValue = value; + dmaMotors[i].dshotTelemetryActive = true; + if (i < 4) { + DEBUG_SET(DEBUG_DSHOT_RPM_TELEMETRY, i, value); + } + } else { + dshotInvalidPacketCount++; + if (i == 0) { + memcpy(inputBuffer,dmaMotors[i].dmaBuffer,sizeof(inputBuffer)); + } + } + dmaMotors[i].hasTelemetry = false; + } else { + timeDelta_t usSinceInput = cmpTimeUs(micros(), dmaMotors[i].timer->inputDirectionStampUs); + if (usSinceInput >= 0 && usSinceInput < dmaMotors[i].dshotTelemetryDeadtimeUs) { + return false; + } +#ifdef STM32F7 + LL_EX_TIM_DisableIT(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource); +#else + TIM_DMACmd(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource, DISABLE); +#endif + } + pwmDshotSetDirectionOutput(&dmaMotors[i], true); + } + dshotEnableChannels(motorCount); + return true; } bool isDshotMotorTelemetryActive(uint8_t motorIndex) diff --git a/src/main/drivers/pwm_output_dshot_shared.h b/src/main/drivers/pwm_output_dshot_shared.h index 9978718005..a2018e1eb5 100644 --- a/src/main/drivers/pwm_output_dshot_shared.h +++ b/src/main/drivers/pwm_output_dshot_shared.h @@ -66,7 +66,7 @@ FAST_CODE void pwmDshotSetDirectionOutput( #endif ); -void pwmStartDshotMotorUpdate(uint8_t motorCount); +bool pwmStartDshotMotorUpdate(uint8_t motorCount); #endif #endif diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index b498440a9d..5762cd3b5c 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -518,7 +518,9 @@ void writeMotors(void) { if (pwmAreMotorsEnabled()) { #if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY) - pwmStartMotorUpdate(motorCount); + if (!pwmStartMotorUpdate(motorCount)) { + return; + } #endif for (int i = 0; i < motorCount; i++) { pwmWriteMotor(i, motor[i]);