diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 300853513f4..bf38ec93404 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -42,6 +42,9 @@ #include "fc/config.h" #include "fc/runtime_config.h" +#include "drivers/timer_impl.h" +#include "drivers/timer.h" + #define MULTISHOT_5US_PW (MULTISHOT_TIMER_HZ * 5 / 1000000.0f) #define MULTISHOT_20US_MULT (MULTISHOT_TIMER_HZ * 20 / 1000000.0f / 1000.0f) @@ -56,7 +59,9 @@ #define DSHOT_MOTOR_BITLENGTH 20 #define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */ +#define MAX_DMA_TIMERS 8 +#define DSHOT_COMMAND_DELAY_US 1000 #define DSHOT_COMMAND_INTERVAL_US 10000 #define DSHOT_COMMAND_QUEUE_LENGTH 8 #define DHSOT_COMMAND_QUEUE_SIZE DSHOT_COMMAND_QUEUE_LENGTH * sizeof(dshotCommands_e) @@ -64,6 +69,10 @@ typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors +#ifdef USE_DSHOT_DMAR + timerDMASafeType_t dmaBurstBuffer[MAX_DMA_TIMERS][DSHOT_DMA_BUFFER_SIZE * 4]; +#endif + typedef struct { TCH_t * tch; bool configured; @@ -77,6 +86,9 @@ typedef struct { #ifdef USE_DSHOT // DSHOT parameters timerDMASafeType_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE]; +#ifdef USE_DSHOT_DMAR + timerDMASafeType_t *dmaBurstBuffer; +#endif #endif } pwmOutputPort_t; @@ -249,6 +261,22 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) } } +#ifdef USE_DSHOT_DMAR +burstDmaTimer_t burstDmaTimers[MAX_DMA_TIMERS]; +uint8_t burstDmaTimersCount = 0; + +static uint8_t getBurstDmaTimerIndex(TIM_TypeDef *timer) +{ + for (int i = 0; i < burstDmaTimersCount; i++) { + if (burstDmaTimers[i].timer == timer) { + return i; + } + } + burstDmaTimers[burstDmaTimersCount++].timer = timer; + return burstDmaTimersCount - 1; +} +#endif + static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, uint32_t dshotHz, bool enableOutput) { // Try allocating new port @@ -259,15 +287,43 @@ static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, } // Configure timer DMA +#ifdef USE_DSHOT_DMAR + //uint8_t timerIndex = lookupTimerIndex(timerHardware->tim); + uint8_t burstDmaTimerIndex = getBurstDmaTimerIndex(timerHardware->tim); + if (burstDmaTimerIndex >= MAX_DMA_TIMERS) { + return NULL; + } + + port->dmaBurstBuffer = &dmaBurstBuffer[burstDmaTimerIndex][0]; + burstDmaTimer_t *burstDmaTimer = &burstDmaTimers[burstDmaTimerIndex]; + burstDmaTimer->dmaBurstBuffer = port->dmaBurstBuffer; + + if (timerPWMConfigDMABurst(burstDmaTimer, port->tch, port->dmaBurstBuffer, sizeof(port->dmaBurstBuffer[0]), DSHOT_DMA_BUFFER_SIZE)) { + port->configured = true; + } +#else if (timerPWMConfigChannelDMA(port->tch, port->dmaBuffer, sizeof(port->dmaBuffer[0]), DSHOT_DMA_BUFFER_SIZE)) { // Only mark as DSHOT channel if DMA was set successfully ZERO_FARRAY(port->dmaBuffer); port->configured = true; } +#endif return port; } +#ifdef USE_DSHOT_DMAR +static void loadDmaBufferDshotStride(timerDMASafeType_t *dmaBuffer, int stride, uint16_t packet) +{ + int i; + for (i = 0; i < 16; i++) { + dmaBuffer[i * stride] = (packet & 0x8000) ? DSHOT_MOTOR_BIT_1 : DSHOT_MOTOR_BIT_0; // MSB first + packet <<= 1; + } + dmaBuffer[i++ * stride] = 0; + dmaBuffer[i++ * stride] = 0; +} +#else static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet) { for (int i = 0; i < 16; i++) { @@ -275,6 +331,7 @@ static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet) packet <<= 1; } } +#endif static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry) { @@ -378,6 +435,7 @@ static void executeDShotCommands(void){ return; } } + delayMicroseconds(DSHOT_COMMAND_DELAY_US); for (uint8_t i = 0; i < getMotorCount(); i++) { motors[i].requestTelemetry = true; motors[i].value = currentExecutingCommand.cmd; @@ -408,6 +466,20 @@ void pwmCompleteMotorUpdate(void) { executeDShotCommands(); +#ifdef USE_DSHOT_DMAR + for (int index = 0; index < motorCount; index++) { + if (motors[index].pwmPort && motors[index].pwmPort->configured) { + uint16_t packet = prepareDshotPacket(motors[index].value, motors[index].requestTelemetry); + loadDmaBufferDshotStride(&motors[index].pwmPort->dmaBurstBuffer[motors[index].pwmPort->tch->timHw->channelIndex], 4, packet); + motors[index].requestTelemetry = false; + } + } + + for (int burstDmaTimerIndex = 0; burstDmaTimerIndex < burstDmaTimersCount; burstDmaTimerIndex++) { + burstDmaTimer_t *burstDmaTimer = &burstDmaTimers[burstDmaTimerIndex]; + pwmBurstDMAStart(burstDmaTimer, DSHOT_DMA_BUFFER_SIZE * 4); + } +#else // Generate DMA buffers for (int index = 0; index < motorCount; index++) { if (motors[index].pwmPort && motors[index].pwmPort->configured) { @@ -424,6 +496,7 @@ void pwmCompleteMotorUpdate(void) { timerPWMStartDMA(motors[index].pwmPort->tch); } } +#endif } #endif } diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index ecdf1ba48d0..664bae3ca59 100644 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -287,3 +287,15 @@ bool timerPWMDMAInProgress(TCH_t * tch) { return tch->dmaState != TCH_DMA_IDLE; } + +#ifdef USE_DSHOT_DMAR +bool timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount) +{ + return impl_timerPWMConfigDMABurst(burstDmaTimer, tch, dmaBuffer, dmaBufferElementSize, dmaBufferElementCount); +} + +void pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength) +{ + impl_pwmBurstDMAStart(burstDmaTimer, BurstLength); +} +#endif diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 5272c804b8f..b4b3ed8da03 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -158,6 +158,10 @@ typedef struct timHardwareContext_s { TIM_HandleTypeDef * timHandle; #endif TCH_t ch[CC_CHANNELS_PER_TIMER]; +#ifdef USE_DSHOT_DMAR + DMA_t dmaBurstRef; + uint16_t DMASource; +#endif } timHardwareContext_t; // Per MCU timer definitions @@ -168,6 +172,20 @@ extern const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT]; extern timerHardware_t timerHardware[]; extern const int timerHardwareCount; +#ifdef USE_DSHOT_DMAR +typedef struct { + TIM_TypeDef *timer; +#ifdef USE_HAL_DRIVER + DMA_TypeDef *dma; + uint32_t streamLL; +#else + DMA_Stream_TypeDef *dmaBurstStream; +#endif + timerDMASafeType_t *dmaBurstBuffer; + uint16_t burstRequestSource; +} burstDmaTimer_t; +#endif + typedef enum { TYPE_FREE, TYPE_PWMINPUT, @@ -229,3 +247,8 @@ void timerPWMStopDMA(TCH_t * tch); bool timerPWMDMAInProgress(TCH_t * tch); volatile timCCR_t *timerCCR(TCH_t * tch); + +#ifdef USE_DSHOT_DMAR +bool timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount); +void pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength); +#endif \ No newline at end of file diff --git a/src/main/drivers/timer_impl.h b/src/main/drivers/timer_impl.h index 82c09837219..4d0a87f9aa5 100644 --- a/src/main/drivers/timer_impl.h +++ b/src/main/drivers/timer_impl.h @@ -84,3 +84,8 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount); void impl_timerPWMStartDMA(TCH_t * tch); void impl_timerPWMStopDMA(TCH_t * tch); + +#ifdef USE_DSHOT_DMAR +bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount); +void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength); +#endif \ No newline at end of file diff --git a/src/main/drivers/timer_impl_hal.c b/src/main/drivers/timer_impl_hal.c index 0649513f210..e800a942353 100644 --- a/src/main/drivers/timer_impl_hal.c +++ b/src/main/drivers/timer_impl_hal.c @@ -408,6 +408,109 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf return true; } +#ifdef USE_DSHOT_DMAR +bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount) +{ + tch->dma = dmaGetByTag(tch->timHw->dmaTag); + tch->dmaBuffer = dmaBuffer; + if (tch->dma == NULL) { + return false; + } + + // If DMA is already in use - abort + if (dmaGetOwner(tch->dma) != OWNER_FREE) { + return false; + } + + if (!tch->timCtx->dmaBurstRef) { + // We assume that timer channels are already initialized by calls to: + // timerConfigBase + // timerPWMConfigChannel + const uint32_t streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)]; + + LL_DMA_DeInit(tch->dma->dma, streamLL); + + LL_DMA_InitTypeDef init; + LL_DMA_StructInit(&init); + +#if defined(STM32H7) || defined(STM32G4) + // For H7 the DMA periphRequest is encoded in the DMA tag + init.PeriphRequest = DMATAG_GET_CHANNEL(tch->timHw->dmaTag); +#else + init.Channel = lookupDMALLChannelTable[DMATAG_GET_CHANNEL(tch->timHw->dmaTag)]; +#endif + + init.PeriphOrM2MSrcAddress = (uint32_t)&tch->timHw->tim->DMAR; + init.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; + + switch (dmaBufferElementSize) { + case 1: + init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_BYTE; + init.PeriphOrM2MSrcDataSize = LL_DMA_MDATAALIGN_BYTE; + break; + case 2: + init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_HALFWORD; + init.PeriphOrM2MSrcDataSize = LL_DMA_MDATAALIGN_HALFWORD; + break; + case 4: + init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_WORD; + init.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_WORD; + break; + default: + // Programmer error + while(1) { + + } + } + + init.MemoryOrM2MDstAddress = (uint32_t)dmaBuffer; + init.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT; + init.NbData = dmaBufferElementCount; + init.Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH; + init.Mode = LL_DMA_MODE_NORMAL; + init.Priority = LL_DMA_PRIORITY_HIGH; + init.FIFOMode = LL_DMA_FIFOMODE_ENABLE; + init.FIFOThreshold = LL_DMA_FIFOTHRESHOLD_FULL; + init.MemBurst = LL_DMA_MBURST_SINGLE; + init.PeriphBurst = LL_DMA_PBURST_SINGLE; + + dmaInit(tch->dma, OWNER_TIMER, 0); + dmaSetHandler(tch->dma, impl_timerDMA_IRQHandler, NVIC_PRIO_TIMER_DMA, (uint32_t)tch); + + LL_DMA_Init(tch->dma->dma, streamLL, &init); + + tch->timCtx->dmaBurstRef = tch->dma; + burstDmaTimer->burstRequestSource = lookupDMASourceTable[tch->timHw->channelIndex]; + burstDmaTimer->streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)]; + burstDmaTimer->dma = tch->dma->dma; + + tch->dmaState = TCH_DMA_READY; + } + + // Start PWM generation + if (tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) { + HAL_TIMEx_PWMN_Start(tch->timCtx->timHandle, lookupTIMChannelTable[tch->timHw->channelIndex]); + } + else { + HAL_TIM_PWM_Start(tch->timCtx->timHandle, lookupTIMChannelTable[tch->timHw->channelIndex]); + } + + return true; +} + +void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength) +{ + LL_DMA_SetDataLength(burstDmaTimer->dma, burstDmaTimer->streamLL, BurstLength); + LL_DMA_EnableIT_TC(burstDmaTimer->dma, burstDmaTimer->streamLL); + LL_DMA_EnableStream(burstDmaTimer->dma, burstDmaTimer->streamLL); + /* configure the DMA Burst Mode */ + LL_TIM_ConfigDMABurst(burstDmaTimer->timer, LL_TIM_DMABURST_BASEADDR_CCR1, LL_TIM_DMABURST_LENGTH_4TRANSFERS); + /* Enable the TIM DMA Request */ + //LL_TIM_EnableDMAReq_UPDATE(burstDmaTimer->timer); + LL_TIM_EnableDMAReq_CCx(burstDmaTimer->timer, burstDmaTimer->burstRequestSource); +} +#endif + void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount) { const uint32_t streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)]; diff --git a/src/main/drivers/timer_impl_stdperiph.c b/src/main/drivers/timer_impl_stdperiph.c index 599b8b5cb3f..5cb3457c77b 100644 --- a/src/main/drivers/timer_impl_stdperiph.c +++ b/src/main/drivers/timer_impl_stdperiph.c @@ -357,6 +357,104 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf return true; } +#ifdef USE_DSHOT_DMAR +bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount) +{ + DMA_InitTypeDef DMA_InitStructure; + TIM_TypeDef * timer = tch->timHw->tim; + + if (!tch->timCtx->dmaBurstRef) { + tch->dma = dmaGetByTag(tch->timHw->dmaTag); + if (tch->dma == NULL) { + return false; + } + + // If DMA is already in use - abort + if (tch->dma->owner != OWNER_FREE) { + return false; + } + } + + // We assume that timer channels are already initialized by calls to: + // timerConfigBase + // timerPWMConfigChannel + + TIM_CtrlPWMOutputs(timer, ENABLE); + TIM_ARRPreloadConfig(timer, ENABLE); + + TIM_CCxCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCx_Enable); + TIM_Cmd(timer, ENABLE); + + if (!tch->timCtx->dmaBurstRef) { + dmaInit(tch->dma, OWNER_TIMER, 0); + dmaSetHandler(tch->dma, impl_timerDMA_IRQHandler, NVIC_PRIO_TIMER_DMA, (uint32_t)tch); + + DMA_DeInit(tch->dma->ref); + DMA_Cmd(tch->dma->ref, DISABLE); + + DMA_StructInit(&DMA_InitStructure); + + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&tch->timHw->tim->DMAR; + DMA_InitStructure.DMA_BufferSize = dmaBufferElementCount; + DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; + DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; + + switch (dmaBufferElementSize) { + case 1: + DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; + break; + case 2: + DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; + break; + case 4: + DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; + break; + default: + // Programmer error + while(1) { + + } + } + +#ifdef STM32F4 + DMA_InitStructure.DMA_Channel = dmaGetChannelByTag(tch->timHw->dmaTag); + DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)dmaBuffer; + DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; + DMA_InitStructure.DMA_Priority = DMA_Priority_High; +#else // F3 + DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dmaBuffer; + DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; + DMA_InitStructure.DMA_Priority = DMA_Priority_High; + DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; +#endif + + DMA_Init(tch->dma->ref, &DMA_InitStructure); + DMA_ITConfig(tch->dma->ref, DMA_IT_TC, ENABLE); + + tch->timCtx->dmaBurstRef = tch->dma; + tch->timCtx->DMASource = lookupDMASourceTable[tch->timHw->channelIndex]; + burstDmaTimer->dmaBurstStream = tch->timCtx->dmaBurstRef->ref; + burstDmaTimer->burstRequestSource = tch->timCtx->DMASource; + + tch->dmaState = TCH_DMA_READY; + } + + return true; +} + +void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength) +{ + DMA_SetCurrDataCounter(burstDmaTimer->dmaBurstStream, BurstLength); + DMA_Cmd(burstDmaTimer->dmaBurstStream, ENABLE); + TIM_DMAConfig(burstDmaTimer->timer, TIM_DMABase_CCR1, TIM_DMABurstLength_4Transfers); + TIM_DMACmd(burstDmaTimer->timer, burstDmaTimer->burstRequestSource, ENABLE); +} +#endif + void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount) { // Make sure we terminate any DMA transaction currently in progress diff --git a/src/main/target/SPEEDYBEEF405V3/target.h b/src/main/target/SPEEDYBEEF405V3/target.h index d7bb949b2e6..99711f82deb 100644 --- a/src/main/target/SPEEDYBEEF405V3/target.h +++ b/src/main/target/SPEEDYBEEF405V3/target.h @@ -152,7 +152,8 @@ #define RANGEFINDER_I2C_BUS BUS_I2C2 #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) -// #define USE_DSHOT +#define USE_DSHOT +#define USE_DSHOT_DMAR // #define USE_ESC_SENSOR #define USE_SERIAL_4WAY_BLHELI_INTERFACE