diff --git a/make/mcu/STM32F3.mk b/make/mcu/STM32F3.mk index 6e197da33..7022deba5 100644 --- a/make/mcu/STM32F3.mk +++ b/make/mcu/STM32F3.mk @@ -83,6 +83,7 @@ MCU_COMMON_SRC = \ drivers/light_ws2811strip_stdperiph.c \ drivers/transponder_ir_io_stdperiph.c \ drivers/pwm_output_dshot.c \ + drivers/pwm_output_dshot_shared.c \ drivers/serial_uart_init.c \ drivers/serial_uart_stm32f30x.c \ drivers/system_stm32f30x.c \ diff --git a/make/mcu/STM32F4.mk b/make/mcu/STM32F4.mk index 36008efcd..5dcf4b765 100644 --- a/make/mcu/STM32F4.mk +++ b/make/mcu/STM32F4.mk @@ -179,6 +179,7 @@ MCU_COMMON_SRC = \ drivers/light_ws2811strip_stdperiph.c \ drivers/transponder_ir_io_stdperiph.c \ drivers/pwm_output_dshot.c \ + drivers/pwm_output_dshot_shared.c \ drivers/serial_uart_init.c \ drivers/serial_uart_stm32f4xx.c \ drivers/system_stm32f4xx.c \ diff --git a/make/mcu/STM32F7.mk b/make/mcu/STM32F7.mk index 84d866a8a..437bf67fb 100644 --- a/make/mcu/STM32F7.mk +++ b/make/mcu/STM32F7.mk @@ -179,6 +179,7 @@ MCU_COMMON_SRC = \ drivers/bus_spi_ll.c \ drivers/persistent.c \ drivers/pwm_output_dshot_hal.c \ + drivers/pwm_output_dshot_shared.c \ drivers/timer_hal.c \ drivers/timer_stm32f7xx.c \ drivers/system_stm32f7xx.c \ diff --git a/make/source.mk b/make/source.mk index b284cc495..3c24eae4a 100644 --- a/make/source.mk +++ b/make/source.mk @@ -328,6 +328,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ drivers/bus_spi_ll.c \ drivers/max7456.c \ drivers/pwm_output_dshot.c \ + drivers/pwm_output_dshot_shared.c \ drivers/pwm_output_dshot_hal.c endif #!F3 endif #!F1 diff --git a/src/main/drivers/pwm_output_dshot.c b/src/main/drivers/pwm_output_dshot.c index 3e2197838..de99f9470 100644 --- a/src/main/drivers/pwm_output_dshot.c +++ b/src/main/drivers/pwm_output_dshot.c @@ -43,11 +43,6 @@ #include "pwm_output.h" -// TODO remove once debugging no longer needed -#ifdef USE_DSHOT_TELEMETRY -#include -#endif - #include "pwm_output_dshot_shared.h" #ifdef USE_DSHOT_TELEMETRY @@ -60,7 +55,7 @@ static void processInputIrq(motorDmaOutput_t * const motor) readDoneCount++; } -static void enableChannels(uint8_t motorCount) +void dshotEnableChannels(uint8_t motorCount) { for (int i = 0; i < motorCount; i++) { if (dmaMotors[i].output & TIMER_OUTPUT_N_CHANNEL) { @@ -75,7 +70,7 @@ static void enableChannels(uint8_t motorCount) static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor); -inline FAST_CODE static void pwmDshotSetDirectionOutput( +FAST_CODE void pwmDshotSetDirectionOutput( motorDmaOutput_t * const motor, bool output #ifndef USE_DSHOT_TELEMETRY ,TIM_OCInitTypeDef *pOcInit, DMA_InitTypeDef* pDmaInit diff --git a/src/main/drivers/pwm_output_dshot_hal.c b/src/main/drivers/pwm_output_dshot_hal.c index a4aa3bd7a..1938278f9 100644 --- a/src/main/drivers/pwm_output_dshot_hal.c +++ b/src/main/drivers/pwm_output_dshot_hal.c @@ -64,7 +64,7 @@ static void processInputIrq(motorDmaOutput_t * const motor) readDoneCount++; } -static void enableChannels(uint8_t motorCount) +void dshotEnableChannels(uint8_t motorCount) { for (int i = 0; i < motorCount; i++) { if (dmaMotors[i].output & TIMER_OUTPUT_N_CHANNEL) { @@ -80,7 +80,7 @@ static void enableChannels(uint8_t motorCount) static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor); -inline static void pwmDshotSetDirectionOutput( +void pwmDshotSetDirectionOutput( motorDmaOutput_t * const motor, bool output #ifndef USE_DSHOT_TELEMETRY , LL_TIM_OC_InitTypeDef* pOcInit, LL_DMA_InitTypeDef* pDmaInit) diff --git a/src/main/drivers/pwm_output_dshot_shared.h b/src/main/drivers/pwm_output_dshot_shared.h index e316f083d..997871800 100644 --- a/src/main/drivers/pwm_output_dshot_shared.h +++ b/src/main/drivers/pwm_output_dshot_shared.h @@ -20,159 +20,42 @@ #ifdef USE_DSHOT +// TODO remove once debugging no longer needed +#ifdef USE_DSHOT_TELEMETRY +#include +#endif + #if defined(STM32F4) || defined(STM32F7) typedef DMA_Stream_TypeDef dmaStream_t; #else typedef DMA_Channel_TypeDef dmaStream_t; #endif -FAST_RAM_ZERO_INIT static uint8_t dmaMotorTimerCount = 0; +extern FAST_RAM_ZERO_INIT uint8_t dmaMotorTimerCount; #ifdef STM32F7 -FAST_RAM_ZERO_INIT static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS]; -FAST_RAM_ZERO_INIT static motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS]; +extern FAST_RAM_ZERO_INIT motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS]; +extern FAST_RAM_ZERO_INIT motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS]; #else -static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS]; -static motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS]; +extern motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS]; +extern motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS]; #endif #ifdef USE_DSHOT_TELEMETRY -uint32_t readDoneCount; +extern uint32_t readDoneCount; // TODO remove once debugging no longer needed -FAST_RAM_ZERO_INIT uint32_t dshotInvalidPacketCount; -FAST_RAM_ZERO_INIT uint32_t inputBuffer[DSHOT_TELEMETRY_INPUT_LEN]; -FAST_RAM_ZERO_INIT uint32_t setDirectionMicros; +FAST_RAM_ZERO_INIT extern uint32_t dshotInvalidPacketCount; +FAST_RAM_ZERO_INIT extern uint32_t inputBuffer[DSHOT_TELEMETRY_INPUT_LEN]; +FAST_RAM_ZERO_INIT extern uint32_t setDirectionMicros; #endif -motorDmaOutput_t *getMotorDmaOutput(uint8_t index) -{ - return &dmaMotors[index]; -} - -uint8_t getTimerIndex(TIM_TypeDef *timer) -{ - for (int i = 0; i < dmaMotorTimerCount; i++) { - if (dmaMotorTimers[i].timer == timer) { - return i; - } - } - dmaMotorTimers[dmaMotorTimerCount++].timer = timer; - return dmaMotorTimerCount - 1; -} - - -FAST_CODE void pwmWriteDshotInt(uint8_t index, uint16_t value) -{ - motorDmaOutput_t *const motor = &dmaMotors[index]; - - if (!motor->configured) { - return; - } - - /*If there is a command ready to go overwrite the value and send that instead*/ - if (pwmDshotCommandIsProcessing()) { - value = pwmGetDshotCommand(index); -#ifdef USE_DSHOT_TELEMETRY - // reset telemetry debug statistics every time telemetry is enabled - if (value == DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY) { - dshotInvalidPacketCount = 0; - readDoneCount = 0; - } -#endif - if (value) { - motor->requestTelemetry = true; - } - } - - motor->value = value; - - uint16_t packet = prepareDshotPacket(motor); - uint8_t bufferSize; - -#ifdef USE_DSHOT_DMAR - if (useBurstDshot) { - bufferSize = loadDmaBuffer(&motor->timer->dmaBurstBuffer[timerLookupChannelIndex(motor->timerHardware->channel)], 4, packet); - motor->timer->dmaBurstLength = bufferSize * 4; - } else -#endif - { - bufferSize = loadDmaBuffer(motor->dmaBuffer, 1, packet); - motor->timer->timerDmaSources |= motor->timerDmaSource; -#ifdef STM32F7 - LL_EX_DMA_SetDataLength(motor->dmaRef, bufferSize); - LL_EX_DMA_EnableStream(motor->dmaRef); -#else - DMA_SetCurrDataCounter(motor->dmaRef, bufferSize); - DMA_Cmd(motor->dmaRef, ENABLE); -#endif - } -} - +uint8_t getTimerIndex(TIM_TypeDef *timer); +motorDmaOutput_t *getMotorDmaOutput(uint8_t index); +void dshotEnableChannels(uint8_t motorCount); #ifdef USE_DSHOT_TELEMETRY -static void enableChannels(uint8_t motorCount); - -static uint16_t decodeDshotPacket(uint32_t buffer[]) -{ - uint32_t value = 0; - for (int i = 1; i < DSHOT_TELEMETRY_INPUT_LEN; i += 2) { - int diff = buffer[i] - buffer[i-1]; - value <<= 1; - if (diff > 0) { - if (diff >= 11) value |= 1; - } else { - if (diff >= -9) value |= 1; - } - } - - uint32_t csum = value; - csum = csum ^ (csum >> 8); // xor bytes - csum = csum ^ (csum >> 4); // xor nibbles - - if (csum & 0xf) { - return 0xffff; - } - return value >> 4; -} - -static uint16_t decodeProshotPacket(uint32_t buffer[]) -{ - uint32_t value = 0; - for (int i = 1; i < PROSHOT_TELEMETRY_INPUT_LEN; i += 2) { - const int proshotModulo = MOTOR_NIBBLE_LENGTH_PROSHOT; - int diff = ((buffer[i] + proshotModulo - buffer[i-1]) % proshotModulo) - PROSHOT_BASE_SYMBOL; - int nibble; - if (diff < 0) { - nibble = 0; - } else { - nibble = (diff + PROSHOT_BIT_WIDTH / 2) / PROSHOT_BIT_WIDTH; - } - value <<= 4; - value |= (nibble & 0xf); - } - - uint32_t csum = value; - csum = csum ^ (csum >> 8); // xor bytes - csum = csum ^ (csum >> 4); // xor nibbles - - if (csum & 0xf) { - return 0xffff; - } - return value >> 4; -} - -#endif - - -#ifdef USE_DSHOT_TELEMETRY - -uint16_t getDshotTelemetry(uint8_t index) -{ - return dmaMotors[index].dshotTelemetryValue; -} - -inline FAST_CODE static void pwmDshotSetDirectionOutput( +FAST_CODE void pwmDshotSetDirectionOutput( motorDmaOutput_t * const motor, bool output #ifndef USE_DSHOT_TELEMETRY #ifdef STM32F7 @@ -183,48 +66,7 @@ inline FAST_CODE static void pwmDshotSetDirectionOutput( #endif ); -void 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; - 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); - } - enableChannels(motorCount); - } -} +void pwmStartDshotMotorUpdate(uint8_t motorCount); #endif #endif diff --git a/src/main/target/MATEKF722/target.c b/src/main/target/MATEKF722/target.c index 069aff7c9..829d5117c 100644 --- a/src/main/target/MATEKF722/target.c +++ b/src/main/target/MATEKF722/target.c @@ -30,7 +30,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MOTOR, 0, 1), // S1 DMA2_ST2 (XXX was DMA1_ST4) + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MOTOR, 0, 0), // S1 DMA2_ST2 (XXX was DMA1_ST4) DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, 0, 1), // S2 DMA2_ST3 DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 1), // S3 DMA2_ST4 DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0), // S4 DMA2_ST7