diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 851bb5463a..b55a75b359 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -137,20 +137,20 @@ static void pwmWriteDshot(uint8_t index, float value) pwmWriteDshotInt(index, lrintf(value)); } -static uint8_t loadDmaBufferDshot(motorDmaOutput_t *const motor, uint16_t packet) +static uint8_t loadDmaBufferDshot(uint32_t *dmaBuffer, int stride, uint16_t packet) { for (int i = 0; i < 16; i++) { - motor->dmaBuffer[i] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first + dmaBuffer[i * stride] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first packet <<= 1; } return DSHOT_DMA_BUFFER_SIZE; } -static uint8_t loadDmaBufferProshot(motorDmaOutput_t *const motor, uint16_t packet) +static uint8_t loadDmaBufferProshot(uint32_t *dmaBuffer, int stride, uint16_t packet) { for (int i = 0; i < 4; i++) { - motor->dmaBuffer[i] = PROSHOT_BASE_SYMBOL + ((packet & 0xF000) >> 12) * PROSHOT_BIT_WIDTH; // Most significant nibble first + dmaBuffer[i * stride] = PROSHOT_BASE_SYMBOL + ((packet & 0xF000) >> 12) * PROSHOT_BIT_WIDTH; // Most significant nibble first packet <<= 4; // Shift 4 bits } diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index d0021d1b75..dff5a5d499 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -185,7 +185,7 @@ void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIn bool isMotorProtocolDshot(void); #ifdef USE_DSHOT -typedef uint8_t loadDmaBufferFn(motorDmaOutput_t *const motor, uint16_t packet); // function pointer used to encode a digital motor value into the DMA buffer representation +typedef uint8_t loadDmaBufferFn(uint32_t *dmaBuffer, int stride, uint16_t packet); // function pointer used to encode a digital motor value into the DMA buffer representation uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, uint16_t value); diff --git a/src/main/drivers/pwm_output_dshot.c b/src/main/drivers/pwm_output_dshot.c index 7922a2d83f..b9b0d57252 100644 --- a/src/main/drivers/pwm_output_dshot.c +++ b/src/main/drivers/pwm_output_dshot.c @@ -71,19 +71,13 @@ void pwmWriteDshotInt(uint8_t index, uint16_t value) #endif uint16_t packet = prepareDshotPacket(motor, value); - - // XXX Replace this with striding loader in the next refactor - uint8_t bufferSize = loadDmaBuffer(motor, packet); + uint8_t bufferSize; #ifdef USE_DSHOT_DMAR - // Load channel data into burst buffer - uint8_t channelIndex = timerLookupChannelIndex(motor->timerHardware->channel); - // XXX This copy will be deleted once the striding loader is available - for (int i = 0; i < bufferSize; i++) { - motor->timer->dmaBurstBuffer[channelIndex + i * 4] = motor->dmaBuffer[i]; - } + bufferSize = loadDmaBuffer(&motor->timer->dmaBurstBuffer[timerLookupChannelIndex(motor->timerHardware->channel)], 4, packet); motor->timer->dmaBurstLength = bufferSize * 4; #else + bufferSize = loadDmaBuffer(motor->dmaBuffer, 1, packet); motor->timer->timerDmaSources |= motor->timerDmaSource; DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, bufferSize); DMA_Cmd(motor->timerHardware->dmaRef, ENABLE); diff --git a/src/main/drivers/pwm_output_dshot_hal.c b/src/main/drivers/pwm_output_dshot_hal.c index cd5dc759cb..ba3fc1af18 100644 --- a/src/main/drivers/pwm_output_dshot_hal.c +++ b/src/main/drivers/pwm_output_dshot_hal.c @@ -64,19 +64,16 @@ void pwmWriteDshotInt(uint8_t index, uint16_t value) uint16_t packet = prepareDshotPacket(motor, value); - uint8_t bufferSize = loadDmaBuffer(motor, packet); + uint8_t bufferSize; #ifdef USE_DSHOT_DMAR - STATIC_ASSERT(TIM_CHANNEL_1==0, tim_channel_0_indexing); - uint8_t channel_index = motor->timerHardware->channel / TIM_CHANNEL_2; - // load channel data into burst buffer - for(int i = 0; i < bufferSize; i++) { - motor->timer->dmaBurstBuffer[channel_index + i * 4] = motor->dmaBuffer[i]; - } + bufferSize = loadDmaBuffer(&motor->timer->dmaBurstBuffer[timerLookupChannelIndex(motor->timerHardware->channel)], 4, packet); if(HAL_DMA_STATE_READY == motor->TimHandle.hdma[motor->timerDmaIndex]->State) { HAL_DMA_Start_IT(motor->TimHandle.hdma[motor->timerDmaIndex], (uint32_t)motor->timer->dmaBurstBuffer, (uint32_t)&motor->TimHandle.Instance->DMAR, bufferSize * 4); } #else + bufferSize = loadDmaBuffer(motor->dmaBuffer, 1, packet); + if (DMA_SetCurrDataCounter(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, bufferSize) != HAL_OK) { /* DMA set error */ return; diff --git a/src/main/drivers/timer_hal.c b/src/main/drivers/timer_hal.c index 3de2e6453b..e1ead7fbbe 100644 --- a/src/main/drivers/timer_hal.c +++ b/src/main/drivers/timer_hal.c @@ -272,6 +272,11 @@ static inline uint8_t lookupChannelIndex(const uint16_t channel) return channel >> 2; } +uint8_t timerLookupChannelIndex(const uint16_t channel) +{ + return lookupChannelIndex(channel); +} + rccPeriphTag_t timerRCC(TIM_TypeDef *tim) { for (int i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) {