src/main/drivers/pwm_output_dshot.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <math.h>
  24
  25 #include "platform.h"
  26
  27 #ifdef USE_DSHOT
  28
  29 #include "build/debug.h"
  30
  31 #include "drivers/dma.h"
  32 #include "drivers/dma_reqmap.h"
  33 #include "drivers/io.h"
  34 #include "drivers/nvic.h"
  35 #include "drivers/rcc.h"
  36 #include "drivers/time.h"
  37 #include "drivers/timer.h"
  38 #include "drivers/system.h"
  39 #if defined(STM32F4)
  40 #include "stm32f4xx.h"
  41 #elif defined(STM32F3)
  42 #include "stm32f30x.h"
  43 #endif
  44
  45 #include "pwm_output.h"
  46 #include "drivers/dshot.h"
  47 #include "drivers/dshot_dpwm.h"
  48 #include "drivers/dshot_command.h"
  49
  50 #include "pwm_output_dshot_shared.h"
  51
  52 #ifdef USE_DSHOT_TELEMETRY
  53
  54 void dshotEnableChannels(uint8_t motorCount)
  55 {
  56     for (int i = 0; i < motorCount; i++) {
  57         if (dmaMotors[i].output & TIMER_OUTPUT_N_CHANNEL) {
  58             TIM_CCxNCmd(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerHardware->channel, TIM_CCxN_Enable);
  59         } else {
  60             TIM_CCxCmd(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerHardware->channel, TIM_CCx_Enable);
  61         }
  62     }
  63 }
  64
  65 #endif
  66
  67 FAST_CODE void pwmDshotSetDirectionOutput(
  68     motorDmaOutput_t * const motor
  69 #ifndef USE_DSHOT_TELEMETRY
  70     ,TIM_OCInitTypeDef *pOcInit, DMA_InitTypeDef* pDmaInit
  71 #endif
  72 )
  73 {
  74 #ifdef USE_DSHOT_TELEMETRY
  75     TIM_OCInitTypeDef* pOcInit = &motor->ocInitStruct;
  76     DMA_InitTypeDef* pDmaInit = &motor->dmaInitStruct;
  77 #endif
  78
  79     const timerHardware_t * const timerHardware = motor->timerHardware;
  80     TIM_TypeDef *timer = timerHardware->tim;
  81
  82     dmaResource_t *dmaRef = motor->dmaRef;
  83
  84 #if defined(USE_DSHOT_DMAR) && !defined(USE_DSHOT_TELEMETRY)
  85     if (useBurstDshot) {
  86         dmaRef = timerHardware->dmaTimUPRef;
  87     }
  88 #endif
  89
  90     xDMA_DeInit(dmaRef);
  91
  92 #ifdef USE_DSHOT_TELEMETRY
  93     motor->isInput = false;
  94 #endif
  95     timerOCPreloadConfig(timer, timerHardware->channel, TIM_OCPreload_Disable);
  96     timerOCInit(timer, timerHardware->channel, pOcInit);
  97     timerOCPreloadConfig(timer, timerHardware->channel, TIM_OCPreload_Enable);
  98
  99 #ifdef USE_DSHOT_DMAR
 100     if (useBurstDshot) {
 101 #if defined(STM32F3)
 102         pDmaInit->DMA_DIR = DMA_DIR_PeripheralDST;
 103 #else
 104         pDmaInit->DMA_DIR = DMA_DIR_MemoryToPeripheral;
 105 #endif
 106     } else
 107 #endif
 108     {
 109 #if defined(STM32F3)
 110         pDmaInit->DMA_DIR = DMA_DIR_PeripheralDST;
 111         pDmaInit->DMA_M2M = DMA_M2M_Disable;
 112 #elif defined(STM32F4)
 113         pDmaInit->DMA_DIR = DMA_DIR_MemoryToPeripheral;
 114 #endif
 115     }
 116
 117     xDMA_Init(dmaRef, pDmaInit);
 118     xDMA_ITConfig(dmaRef, DMA_IT_TC, ENABLE);
 119 }
 120
 121
 122 #ifdef USE_DSHOT_TELEMETRY
 123 FAST_CODE static void pwmDshotSetDirectionInput(
 124     motorDmaOutput_t * const motor
 125 )
 126 {
 127     DMA_InitTypeDef* pDmaInit = &motor->dmaInitStruct;
 128
 129     const timerHardware_t * const timerHardware = motor->timerHardware;
 130     TIM_TypeDef *timer = timerHardware->tim;
 131
 132     dmaResource_t *dmaRef = motor->dmaRef;
 133
 134     xDMA_DeInit(dmaRef);
 135
 136     motor->isInput = true;
 137     if (!inputStampUs) {
 138         inputStampUs = micros();
 139     }
 140     TIM_ARRPreloadConfig(timer, ENABLE);
 141     timer->ARR = 0xffffffff;
 142
 143     TIM_ICInit(timer, &motor->icInitStruct);
 144
 145 #if defined(STM32F3)
 146     motor->dmaInitStruct.DMA_DIR = DMA_DIR_PeripheralSRC;
 147     motor->dmaInitStruct.DMA_M2M = DMA_M2M_Disable;
 148 #elif defined(STM32F4)
 149     motor->dmaInitStruct.DMA_DIR = DMA_DIR_PeripheralToMemory;
 150 #endif
 151
 152     xDMA_Init(dmaRef, pDmaInit);
 153 }
 154 #endif
 155
 156
 157 void pwmCompleteDshotMotorUpdate(void)
 158 {
 159     /* If there is a dshot command loaded up, time it correctly with motor update*/
 160     if (!dshotCommandQueueEmpty()) {
 161         if (!dshotCommandOutputIsEnabled(dshotPwmDevice.count)) {
 162             return;
 163         }
 164     }
 165
 166     for (int i = 0; i < dmaMotorTimerCount; i++) {
 167 #ifdef USE_DSHOT_DMAR
 168         if (useBurstDshot) {
 169             xDMA_SetCurrDataCounter(dmaMotorTimers[i].dmaBurstRef, dmaMotorTimers[i].dmaBurstLength);
 170             xDMA_Cmd(dmaMotorTimers[i].dmaBurstRef, ENABLE);
 171             TIM_DMAConfig(dmaMotorTimers[i].timer, TIM_DMABase_CCR1, TIM_DMABurstLength_4Transfers);
 172             TIM_DMACmd(dmaMotorTimers[i].timer, TIM_DMA_Update, ENABLE);
 173         } else
 174 #endif
 175         {
 176             TIM_ARRPreloadConfig(dmaMotorTimers[i].timer, DISABLE);
 177             dmaMotorTimers[i].timer->ARR = dmaMotorTimers[i].outputPeriod;
 178             TIM_ARRPreloadConfig(dmaMotorTimers[i].timer, ENABLE);
 179             TIM_SetCounter(dmaMotorTimers[i].timer, 0);
 180             TIM_DMACmd(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources, ENABLE);
 181             dmaMotorTimers[i].timerDmaSources = 0;
 182         }
 183     }
 184 }
 185
 186 static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor)
 187 {
 188     if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) {
 189         motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
 190 #ifdef USE_DSHOT_TELEMETRY
 191         dshotDMAHandlerCycleCounters.irqAt = getCycleCounter();
 192 #endif
 193 #ifdef USE_DSHOT_DMAR
 194         if (useBurstDshot) {
 195             xDMA_Cmd(motor->timerHardware->dmaTimUPRef, DISABLE);
 196             TIM_DMACmd(motor->timerHardware->tim, TIM_DMA_Update, DISABLE);
 197         } else
 198 #endif
 199         {
 200             xDMA_Cmd(motor->dmaRef, DISABLE);
 201             TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, DISABLE);
 202         }
 203
 204 #ifdef USE_DSHOT_TELEMETRY
 205         if (useDshotTelemetry) {
 206             pwmDshotSetDirectionInput(motor);
 207             xDMA_SetCurrDataCounter(motor->dmaRef, GCR_TELEMETRY_INPUT_LEN);
 208             xDMA_Cmd(motor->dmaRef, ENABLE);
 209             TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, ENABLE);
 210             dshotDMAHandlerCycleCounters.changeDirectionCompletedAt = getCycleCounter();
 211         }
 212 #endif
 213         DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
 214     }
 215 }
 216
 217 bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
 218 {
 219 #ifdef USE_DSHOT_TELEMETRY
 220 #define OCINIT motor->ocInitStruct
 221 #define DMAINIT motor->dmaInitStruct
 222 #else
 223     TIM_OCInitTypeDef ocInitStruct;
 224     DMA_InitTypeDef   dmaInitStruct;
 225 #define OCINIT ocInitStruct
 226 #define DMAINIT dmaInitStruct
 227 #endif
 228
 229     dmaResource_t *dmaRef = NULL;
 230 #if defined(STM32F4)
 231     uint32_t dmaChannel = 0;
 232 #endif
 233 #if defined(USE_DMA_SPEC)
 234     const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpecByTimer(timerHardware);
 235
 236     if (dmaSpec != NULL) {
 237         dmaRef = dmaSpec->ref;
 238 #if defined(STM32F4)
 239         dmaChannel = dmaSpec->channel;
 240 #endif
 241     }
 242 #else
 243     dmaRef = timerHardware->dmaRef;
 244 #if defined(STM32F4)
 245     dmaChannel = timerHardware->dmaChannel;
 246 #endif
 247 #endif
 248
 249 #ifdef USE_DSHOT_DMAR
 250     if (useBurstDshot) {
 251         dmaRef = timerHardware->dmaTimUPRef;
 252 #if defined(STM32F4)
 253         dmaChannel = timerHardware->dmaTimUPChannel;
 254 #endif
 255     }
 256 #endif
 257
 258     if (dmaRef == NULL) {
 259         return false;
 260     }
 261
 262     motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
 263     TIM_TypeDef *timer = timerHardware->tim;
 264
 265     // Boolean configureTimer is always true when different channels of the same timer are processed in sequence,
 266     // causing the timer and the associated DMA initialized more than once.
 267     // To fix this, getTimerIndex must be expanded to return if a new timer has been requested.
 268     // However, since the initialization is idempotent, it is left as is in a favor of flash space (for now).
 269     const uint8_t timerIndex = getTimerIndex(timer);
 270     const bool configureTimer = (timerIndex == dmaMotorTimerCount-1);
 271
 272     motor->timer = &dmaMotorTimers[timerIndex];
 273     motor->index = motorIndex;
 274     motor->timerHardware = timerHardware;
 275
 276     const IO_t motorIO = IOGetByTag(timerHardware->tag);
 277
 278     uint8_t pupMode = 0;
 279     pupMode = (output & TIMER_OUTPUT_INVERTED) ? GPIO_PuPd_DOWN : GPIO_PuPd_UP;
 280 #ifdef USE_DSHOT_TELEMETRY
 281     if (useDshotTelemetry) {
 282         output ^= TIMER_OUTPUT_INVERTED;
 283     }
 284 #endif
 285
 286     motor->iocfg = IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, pupMode);
 287     IOConfigGPIOAF(motorIO, motor->iocfg, timerHardware->alternateFunction);
 288
 289     if (configureTimer) {
 290         TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
 291         TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
 292
 293         RCC_ClockCmd(timerRCC(timer), ENABLE);
 294         TIM_Cmd(timer, DISABLE);
 295
 296         TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t)(lrintf((float) timerClock(timer) / getDshotHz(pwmProtocolType) + 0.01f) - 1);
 297         TIM_TimeBaseStructure.TIM_Period = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1;
 298         TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
 299         TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
 300         TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
 301         TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure);
 302     }
 303
 304     TIM_OCStructInit(&OCINIT);
 305     OCINIT.TIM_OCMode = TIM_OCMode_PWM1;
 306     if (output & TIMER_OUTPUT_N_CHANNEL) {
 307         OCINIT.TIM_OutputNState = TIM_OutputNState_Enable;
 308         OCINIT.TIM_OCNIdleState = TIM_OCNIdleState_Reset;
 309         OCINIT.TIM_OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_Low : TIM_OCNPolarity_High;
 310     } else {
 311         OCINIT.TIM_OutputState = TIM_OutputState_Enable;
 312         OCINIT.TIM_OCIdleState = TIM_OCIdleState_Set;
 313         OCINIT.TIM_OCPolarity =  (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High;
 314     }
 315     OCINIT.TIM_Pulse = 0;
 316
 317 #ifdef USE_DSHOT_TELEMETRY
 318     TIM_ICStructInit(&motor->icInitStruct);
 319     motor->icInitStruct.TIM_ICSelection = TIM_ICSelection_DirectTI;
 320     motor->icInitStruct.TIM_ICPolarity = TIM_ICPolarity_BothEdge;
 321     motor->icInitStruct.TIM_ICPrescaler = TIM_ICPSC_DIV1;
 322     motor->icInitStruct.TIM_Channel = timerHardware->channel;
 323     motor->icInitStruct.TIM_ICFilter = 2;
 324 #endif
 325
 326
 327 #ifdef USE_DSHOT_DMAR
 328     if (useBurstDshot) {
 329         motor->timer->dmaBurstRef = dmaRef;
 330     } else
 331 #endif
 332     {
 333         motor->timerDmaSource = timerDmaSource(timerHardware->channel);
 334         motor->timer->timerDmaSources &= ~motor->timerDmaSource;
 335     }
 336
 337     xDMA_Cmd(dmaRef, DISABLE);
 338     xDMA_DeInit(dmaRef);
 339     DMA_StructInit(&DMAINIT);
 340
 341 #ifdef USE_DSHOT_DMAR
 342     if (useBurstDshot) {
 343         dmaInit(timerHardware->dmaTimUPIrqHandler, OWNER_TIMUP, timerGetTIMNumber(timerHardware->tim));
 344
 345         motor->timer->dmaBurstBuffer = &dshotBurstDmaBuffer[timerIndex][0];
 346
 347 #if defined(STM32F3)
 348         DMAINIT.DMA_MemoryBaseAddr = (uint32_t)motor->timer->dmaBurstBuffer;
 349         DMAINIT.DMA_DIR = DMA_DIR_PeripheralDST;
 350 #else
 351         DMAINIT.DMA_Channel = timerHardware->dmaTimUPChannel;
 352         DMAINIT.DMA_Memory0BaseAddr = (uint32_t)motor->timer->dmaBurstBuffer;
 353         DMAINIT.DMA_DIR = DMA_DIR_MemoryToPeripheral;
 354         DMAINIT.DMA_FIFOMode = DMA_FIFOMode_Enable;
 355         DMAINIT.DMA_FIFOThreshold = DMA_FIFOThreshold_Full;
 356         DMAINIT.DMA_MemoryBurst = DMA_MemoryBurst_Single;
 357         DMAINIT.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
 358 #endif
 359         DMAINIT.DMA_PeripheralBaseAddr = (uint32_t)&timerHardware->tim->DMAR;
 360         DMAINIT.DMA_BufferSize = (pwmProtocolType == PWM_TYPE_PROSHOT1000) ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE; // XXX
 361         DMAINIT.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
 362         DMAINIT.DMA_MemoryInc = DMA_MemoryInc_Enable;
 363         DMAINIT.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;
 364         DMAINIT.DMA_MemoryDataSize = DMA_MemoryDataSize_Word;
 365         DMAINIT.DMA_Mode = DMA_Mode_Normal;
 366         DMAINIT.DMA_Priority = DMA_Priority_High;
 367     } else
 368 #endif
 369     {
 370         dmaInit(dmaGetIdentifier(dmaRef), OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
 371
 372         motor->dmaBuffer = &dshotDmaBuffer[motorIndex][0];
 373
 374 #if defined(STM32F3)
 375         DMAINIT.DMA_MemoryBaseAddr = (uint32_t)motor->dmaBuffer;
 376         DMAINIT.DMA_DIR = DMA_DIR_PeripheralDST;
 377         DMAINIT.DMA_M2M = DMA_M2M_Disable;
 378 #elif defined(STM32F4)
 379         DMAINIT.DMA_Channel = dmaChannel;
 380         DMAINIT.DMA_Memory0BaseAddr = (uint32_t)motor->dmaBuffer;
 381         DMAINIT.DMA_DIR = DMA_DIR_MemoryToPeripheral;
 382         DMAINIT.DMA_FIFOMode = DMA_FIFOMode_Enable;
 383         DMAINIT.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull;
 384         DMAINIT.DMA_MemoryBurst = DMA_MemoryBurst_Single;
 385         DMAINIT.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
 386 #endif
 387         DMAINIT.DMA_PeripheralBaseAddr = (uint32_t)timerChCCR(timerHardware);
 388         DMAINIT.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
 389         DMAINIT.DMA_MemoryInc = DMA_MemoryInc_Enable;
 390         DMAINIT.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;
 391         DMAINIT.DMA_MemoryDataSize = DMA_MemoryDataSize_Word;
 392         DMAINIT.DMA_Mode = DMA_Mode_Normal;
 393         DMAINIT.DMA_Priority = DMA_Priority_High;
 394     }
 395
 396     // XXX Consolidate common settings in the next refactor
 397
 398     motor->dmaRef = dmaRef;
 399
 400 #ifdef USE_DSHOT_TELEMETRY
 401     motor->dshotTelemetryDeadtimeUs = DSHOT_TELEMETRY_DEADTIME_US + 1000000 *
 402         (16 * MOTOR_BITLENGTH) / getDshotHz(pwmProtocolType);
 403     motor->timer->outputPeriod = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1;
 404     pwmDshotSetDirectionOutput(motor);
 405 #else
 406     pwmDshotSetDirectionOutput(motor, &OCINIT, &DMAINIT);
 407 #endif
 408 #ifdef USE_DSHOT_DMAR
 409     if (useBurstDshot) {
 410         dmaSetHandler(timerHardware->dmaTimUPIrqHandler, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
 411     } else
 412 #endif
 413     {
 414         dmaSetHandler(dmaGetIdentifier(dmaRef), motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
 415     }
 416
 417     TIM_Cmd(timer, ENABLE);
 418     if (output & TIMER_OUTPUT_N_CHANNEL) {
 419         TIM_CCxNCmd(timer, timerHardware->channel, TIM_CCxN_Enable);
 420     } else {
 421         TIM_CCxCmd(timer, timerHardware->channel, TIM_CCx_Enable);
 422     }
 423     if (configureTimer) {
 424         TIM_ARRPreloadConfig(timer, ENABLE);
 425         TIM_CtrlPWMOutputs(timer, ENABLE);
 426         TIM_Cmd(timer, ENABLE);
 427     }
 428 #ifdef USE_DSHOT_TELEMETRY
 429     if (useDshotTelemetry) {
 430         // avoid high line during startup to prevent bootloader activation
 431         *timerChCCR(timerHardware) = 0xffff;
 432     }
 433 #endif
 434     motor->configured = true;
 435
 436     return true;
 437 }
 438
 439 #endif