src/main/drivers/pwm_output_stm32f4xx.c

   1 /*
   2  * This file is part of Betaflight.
   3  *
   4  * Betaflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Betaflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Betaflight. If not, see <http://www.gnu.org/licenses/>.
  16  */
  17 #include <stdbool.h>
  18 #include <stdint.h>
  19 #include <math.h>
  20
  21 #include "platform.h"
  22
  23 #include "io.h"
  24 #include "timer.h"
  25 #include "timer_stm32f4xx.h"
  26 #include "pwm_output.h"
  27 #include "nvic.h"
  28 #include "dma.h"
  29 #include "system.h"
  30 #include "rcc.h"
  31
  32 #ifdef USE_DSHOT
  33
  34 #define MAX_DMA_TIMERS 8
  35
  36 #define MOTOR_DSHOT600_MHZ    12
  37 #define MOTOR_DSHOT300_MHZ    6
  38 #define MOTOR_DSHOT150_MHZ    3
  39
  40 #define MOTOR_BIT_0     7
  41 #define MOTOR_BIT_1     14
  42 #define MOTOR_BITLENGTH 19
  43
  44 static uint8_t dmaMotorTimerCount = 0;
  45 static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
  46 static motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
  47
  48 uint8_t getTimerIndex(TIM_TypeDef *timer)
  49 {
  50     for (int i = 0; i < dmaMotorTimerCount; i++) {
  51         if (dmaMotorTimers[i].timer == timer) {
  52             return i;
  53         }
  54     }
  55     dmaMotorTimers[dmaMotorTimerCount++].timer = timer;
  56     return dmaMotorTimerCount-1;
  57 }
  58
  59 void pwmWriteDigital(uint8_t index, uint16_t value)
  60 {
  61     if (!pwmMotorsEnabled) {
  62         return;
  63     }
  64
  65     motorDmaOutput_t * const motor = &dmaMotors[index];
  66
  67     uint16_t packet = (value << 1) | 0;                            // Here goes telemetry bit (false for now)
  68     // compute checksum
  69     int csum = 0;
  70     int csum_data = packet;
  71     for (int i = 0; i < 3; i++) {
  72         csum ^=  csum_data;   // xor data by nibbles
  73         csum_data >>= 4;
  74     }
  75     csum &= 0xf;
  76     // append checksum
  77     packet = (packet << 4) | csum;
  78     // generate pulses for whole packet
  79     for (int i = 0; i < 16; i++) {
  80         motor->dmaBuffer[i] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0;  // MSB first
  81         packet <<= 1;
  82     }
  83
  84     DMA_SetCurrDataCounter(motor->timerHardware->dmaStream, MOTOR_DMA_BUFFER_SIZE);
  85     DMA_Cmd(motor->timerHardware->dmaStream, ENABLE);
  86 }
  87
  88 void pwmCompleteDigitalMotorUpdate(uint8_t motorCount)
  89 {
  90     UNUSED(motorCount);
  91
  92     if (!pwmMotorsEnabled) {
  93         return;
  94     }
  95
  96     for (int i = 0; i < dmaMotorTimerCount; i++) {
  97         TIM_SetCounter(dmaMotorTimers[i].timer, 0);
  98         TIM_DMACmd(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources, ENABLE);
  99     }
 100 }
 101
 102 static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor)
 103 {
 104     if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) {
 105         motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
 106         DMA_Cmd(descriptor->stream, DISABLE);
 107         TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, DISABLE);
 108         DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
 109     }
 110 }
 111
 112 void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType)
 113 {
 114     TIM_OCInitTypeDef TIM_OCInitStructure;
 115     DMA_InitTypeDef DMA_InitStructure;
 116
 117     motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
 118     motor->timerHardware = timerHardware;
 119
 120     TIM_TypeDef *timer = timerHardware->tim;
 121     const IO_t motorIO = IOGetByTag(timerHardware->tag);
 122
 123     const uint8_t timerIndex = getTimerIndex(timer);
 124     const bool configureTimer = (timerIndex == dmaMotorTimerCount-1);
 125
 126     IOInit(motorIO, OWNER_MOTOR, 0);
 127     IOConfigGPIOAF(motorIO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP), timerHardware->alternateFunction);
 128
 129     if (configureTimer) {
 130         TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
 131         TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
 132
 133         RCC_ClockCmd(timerRCC(timer), ENABLE);
 134         TIM_Cmd(timer, DISABLE);
 135
 136         uint32_t hz;
 137         switch (pwmProtocolType) {
 138             case(PWM_TYPE_DSHOT600):
 139                 hz = MOTOR_DSHOT600_MHZ * 1000000;
 140                 break;
 141             case(PWM_TYPE_DSHOT300):
 142                 hz = MOTOR_DSHOT300_MHZ * 1000000;
 143                 break;
 144             default:
 145             case(PWM_TYPE_DSHOT150):
 146                 hz = MOTOR_DSHOT150_MHZ * 1000000;
 147         }
 148
 149         TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / timerClockDivisor(timer) / hz) - 1;
 150         TIM_TimeBaseStructure.TIM_Period = MOTOR_BITLENGTH;
 151         TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
 152         TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
 153         TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
 154         TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure);
 155     }
 156
 157     TIM_OCStructInit(&TIM_OCInitStructure);
 158     TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
 159     if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
 160         TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
 161         TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset;
 162         TIM_OCInitStructure.TIM_OCNPolarity = (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low;
 163     } else {
 164         TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
 165         TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
 166         TIM_OCInitStructure.TIM_OCPolarity =  (timerHardware->output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High;
 167     }
 168     TIM_OCInitStructure.TIM_Pulse = 0;
 169
 170     timerOCInit(timer, timerHardware->channel, &TIM_OCInitStructure);
 171     timerOCPreloadConfig(timer, timerHardware->channel, TIM_OCPreload_Enable);
 172     motor->timerDmaSource = timerDmaSource(timerHardware->channel);
 173     dmaMotorTimers[timerIndex].timerDmaSources |= motor->timerDmaSource;
 174
 175     TIM_CCxCmd(timer, motor->timerHardware->channel, TIM_CCx_Enable);
 176
 177     if (configureTimer) {
 178         TIM_CtrlPWMOutputs(timer, ENABLE);
 179         TIM_ARRPreloadConfig(timer, ENABLE);
 180         TIM_Cmd(timer, ENABLE);
 181     }
 182
 183     DMA_Stream_TypeDef *stream = timerHardware->dmaStream;
 184
 185     dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
 186     dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex);
 187
 188     DMA_Cmd(stream, DISABLE);
 189     DMA_DeInit(stream);
 190
 191     DMA_StructInit(&DMA_InitStructure);
 192     DMA_InitStructure.DMA_Channel = timerHardware->dmaChannel;
 193     DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)timerChCCR(timerHardware);
 194     DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)motor->dmaBuffer;
 195     DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
 196     DMA_InitStructure.DMA_BufferSize = MOTOR_DMA_BUFFER_SIZE;
 197     DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
 198     DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
 199     DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;
 200     DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word;
 201     DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
 202     DMA_InitStructure.DMA_Priority = DMA_Priority_VeryHigh;
 203     DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Enable;
 204     DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull;
 205     DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
 206     DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
 207
 208     DMA_Init(stream, &DMA_InitStructure);
 209
 210     DMA_ITConfig(stream, DMA_IT_TC, ENABLE);
 211     DMA_ClearITPendingBit(stream, dmaFlag_IT_TCIF(timerHardware->dmaStream));
 212 }
 213
 214 #endif