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 #endif
  42
  43 #include "pwm_output.h"
  44 #include "drivers/dshot.h"
  45 #include "drivers/dshot_dpwm.h"
  46 #include "drivers/dshot_command.h"
  47
  48 #include "pwm_output_dshot_shared.h"
  49
  50 #ifdef USE_DSHOT_TELEMETRY
  51
  52 void dshotEnableChannels(uint8_t motorCount)
  53 {
  54     for (int i = 0; i < motorCount; i++) {
  55         if (dmaMotors[i].output & TIMER_OUTPUT_N_CHANNEL) {
  56             TIM_CCxNCmd(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerHardware->channel, TIM_CCxN_Enable);
  57         } else {
  58             TIM_CCxCmd(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerHardware->channel, TIM_CCx_Enable);
  59         }
  60     }
  61 }
  62
  63 #endif
  64
  65 FAST_CODE void pwmDshotSetDirectionOutput(
  66     motorDmaOutput_t * const motor
  67 #ifndef USE_DSHOT_TELEMETRY
  68     ,TIM_OCInitTypeDef *pOcInit, DMA_InitTypeDef* pDmaInit
  69 #endif
  70 )
  71 {
  72 #ifdef USE_DSHOT_TELEMETRY
  73     TIM_OCInitTypeDef* pOcInit = &motor->ocInitStruct;
  74     DMA_InitTypeDef* pDmaInit = &motor->dmaInitStruct;
  75 #endif
  76
  77     const timerHardware_t * const timerHardware = motor->timerHardware;
  78     TIM_TypeDef *timer = timerHardware->tim;
  79
  80     dmaResource_t *dmaRef = motor->dmaRef;
  81
  82 #if defined(USE_DSHOT_DMAR) && !defined(USE_DSHOT_TELEMETRY)
  83     if (useBurstDshot) {
  84         dmaRef = timerHardware->dmaTimUPRef;
  85     }
  86 #endif
  87
  88     xDMA_DeInit(dmaRef);
  89
  90 #ifdef USE_DSHOT_TELEMETRY
  91     motor->isInput = false;
  92 #endif
  93     timerOCPreloadConfig(timer, timerHardware->channel, TIM_OCPreload_Disable);
  94     timerOCInit(timer, timerHardware->channel, pOcInit);
  95     timerOCPreloadConfig(timer, timerHardware->channel, TIM_OCPreload_Enable);
  96
  97 #ifdef USE_DSHOT_DMAR
  98     if (useBurstDshot) {
  99         pDmaInit->DMA_DIR = DMA_DIR_MemoryToPeripheral;
 100     } else
 101 #endif
 102     {
 103 #if defined(STM32F4)
 104         pDmaInit->DMA_DIR = DMA_DIR_MemoryToPeripheral;
 105 #endif
 106     }
 107
 108     xDMA_Init(dmaRef, pDmaInit);
 109     xDMA_ITConfig(dmaRef, DMA_IT_TC, ENABLE);
 110 }
 111
 112
 113 #ifdef USE_DSHOT_TELEMETRY
 114 FAST_CODE
 115 static void pwmDshotSetDirectionInput(
 116     motorDmaOutput_t * const motor
 117 )
 118 {
 119     DMA_InitTypeDef* pDmaInit = &motor->dmaInitStruct;
 120
 121     const timerHardware_t * const timerHardware = motor->timerHardware;
 122     TIM_TypeDef *timer = timerHardware->tim;
 123
 124     dmaResource_t *dmaRef = motor->dmaRef;
 125
 126     xDMA_DeInit(dmaRef);
 127
 128     motor->isInput = true;
 129     if (!inputStampUs) {
 130         inputStampUs = micros();
 131     }
 132     TIM_ARRPreloadConfig(timer, ENABLE);
 133     timer->ARR = 0xffffffff;
 134
 135     TIM_ICInit(timer, &motor->icInitStruct);
 136
 137 #if defined(STM32F4)
 138     motor->dmaInitStruct.DMA_DIR = DMA_DIR_PeripheralToMemory;
 139 #endif
 140
 141     xDMA_Init(dmaRef, pDmaInit);
 142 }
 143 #endif
 144
 145
 146 void pwmCompleteDshotMotorUpdate(void)
 147 {
 148     /* If there is a dshot command loaded up, time it correctly with motor update*/
 149     if (!dshotCommandQueueEmpty()) {
 150         if (!dshotCommandOutputIsEnabled(dshotPwmDevice.count)) {
 151             return;
 152         }
 153     }
 154
 155     for (int i = 0; i < dmaMotorTimerCount; i++) {
 156 #ifdef USE_DSHOT_DMAR
 157         if (useBurstDshot) {
 158             xDMA_SetCurrDataCounter(dmaMotorTimers[i].dmaBurstRef, dmaMotorTimers[i].dmaBurstLength);
 159             xDMA_Cmd(dmaMotorTimers[i].dmaBurstRef, ENABLE);
 160             TIM_DMAConfig(dmaMotorTimers[i].timer, TIM_DMABase_CCR1, TIM_DMABurstLength_4Transfers);
 161             TIM_DMACmd(dmaMotorTimers[i].timer, TIM_DMA_Update, ENABLE);
 162         } else
 163 #endif
 164         {
 165             TIM_ARRPreloadConfig(dmaMotorTimers[i].timer, DISABLE);
 166             dmaMotorTimers[i].timer->ARR = dmaMotorTimers[i].outputPeriod;
 167             TIM_ARRPreloadConfig(dmaMotorTimers[i].timer, ENABLE);
 168             TIM_SetCounter(dmaMotorTimers[i].timer, 0);
 169             TIM_DMACmd(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources, ENABLE);
 170             dmaMotorTimers[i].timerDmaSources = 0;
 171         }
 172     }
 173 }
 174
 175 FAST_CODE static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor)
 176 {
 177     if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) {
 178         motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
 179 #ifdef USE_DSHOT_TELEMETRY
 180         dshotDMAHandlerCycleCounters.irqAt = getCycleCounter();
 181 #endif
 182 #ifdef USE_DSHOT_DMAR
 183         if (useBurstDshot) {
 184             xDMA_Cmd(motor->timerHardware->dmaTimUPRef, DISABLE);
 185             TIM_DMACmd(motor->timerHardware->tim, TIM_DMA_Update, DISABLE);
 186         } else
 187 #endif
 188         {
 189             xDMA_Cmd(motor->dmaRef, DISABLE);
 190             TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, DISABLE);
 191         }
 192
 193 #ifdef USE_DSHOT_TELEMETRY
 194         if (useDshotTelemetry) {
 195             pwmDshotSetDirectionInput(motor);
 196             xDMA_SetCurrDataCounter(motor->dmaRef, GCR_TELEMETRY_INPUT_LEN);
 197             xDMA_Cmd(motor->dmaRef, ENABLE);
 198             TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, ENABLE);
 199             dshotDMAHandlerCycleCounters.changeDirectionCompletedAt = getCycleCounter();
 200         }
 201 #endif
 202         DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
 203     }
 204 }
 205
 206 bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
 207 {
 208 #ifdef USE_DSHOT_TELEMETRY
 209 #define OCINIT motor->ocInitStruct
 210 #define DMAINIT motor->dmaInitStruct
 211 #else
 212     TIM_OCInitTypeDef ocInitStruct;
 213     DMA_InitTypeDef   dmaInitStruct;
 214 #define OCINIT ocInitStruct
 215 #define DMAINIT dmaInitStruct
 216 #endif
 217
 218     dmaResource_t *dmaRef = NULL;
 219 #if defined(STM32F4)
 220     uint32_t dmaChannel = 0;
 221 #endif
 222 #if defined(USE_DMA_SPEC)
 223     const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpecByTimer(timerHardware);
 224
 225     if (dmaSpec != NULL) {
 226         dmaRef = dmaSpec->ref;
 227 #if defined(STM32F4)
 228         dmaChannel = dmaSpec->channel;
 229 #endif
 230     }
 231 #else
 232     dmaRef = timerHardware->dmaRef;
 233 #if defined(STM32F4)
 234     dmaChannel = timerHardware->dmaChannel;
 235 #endif
 236 #endif
 237
 238 #ifdef USE_DSHOT_DMAR
 239     if (useBurstDshot) {
 240         dmaRef = timerHardware->dmaTimUPRef;
 241 #if defined(STM32F4)
 242         dmaChannel = timerHardware->dmaTimUPChannel;
 243 #endif
 244     }
 245 #endif
 246
 247     if (dmaRef == NULL) {
 248         return false;
 249     }
 250
 251     dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(dmaRef);
 252
 253     bool dmaIsConfigured = false;
 254 #ifdef USE_DSHOT_DMAR
 255     if (useBurstDshot) {
 256         const resourceOwner_t *owner = dmaGetOwner(dmaIdentifier);
 257         if (owner->owner == OWNER_TIMUP && owner->resourceIndex == timerGetTIMNumber(timerHardware->tim)) {
 258             dmaIsConfigured = true;
 259         } else if (!dmaAllocate(dmaIdentifier, OWNER_TIMUP, timerGetTIMNumber(timerHardware->tim))) {
 260             return false;
 261         }
 262     } else
 263 #endif
 264     {
 265         if (!dmaAllocate(dmaIdentifier, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex))) {
 266             return false;
 267         }
 268     }
 269
 270     motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
 271     TIM_TypeDef *timer = timerHardware->tim;
 272
 273     // Boolean configureTimer is always true when different channels of the same timer are processed in sequence,
 274     // causing the timer and the associated DMA initialized more than once.
 275     // To fix this, getTimerIndex must be expanded to return if a new timer has been requested.
 276     // However, since the initialization is idempotent, it is left as is in a favor of flash space (for now).
 277     const uint8_t timerIndex = getTimerIndex(timer);
 278     const bool configureTimer = (timerIndex == dmaMotorTimerCount-1);
 279
 280     motor->timer = &dmaMotorTimers[timerIndex];
 281     motor->index = motorIndex;
 282     motor->timerHardware = timerHardware;
 283
 284     const IO_t motorIO = IOGetByTag(timerHardware->tag);
 285
 286     uint8_t pupMode = 0;
 287     pupMode = (output & TIMER_OUTPUT_INVERTED) ? GPIO_PuPd_DOWN : GPIO_PuPd_UP;
 288 #ifdef USE_DSHOT_TELEMETRY
 289     if (useDshotTelemetry) {
 290         output ^= TIMER_OUTPUT_INVERTED;
 291     }
 292 #endif
 293
 294     motor->iocfg = IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, pupMode);
 295     IOConfigGPIOAF(motorIO, motor->iocfg, timerHardware->alternateFunction);
 296
 297     if (configureTimer) {
 298         TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
 299         TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
 300
 301         RCC_ClockCmd(timerRCC(timer), ENABLE);
 302         TIM_Cmd(timer, DISABLE);
 303
 304         TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t)(lrintf((float) timerClock(timer) / getDshotHz(pwmProtocolType) + 0.01f) - 1);
 305         TIM_TimeBaseStructure.TIM_Period = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1;
 306         TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
 307         TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
 308         TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
 309         TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure);
 310     }
 311
 312     TIM_OCStructInit(&OCINIT);
 313     OCINIT.TIM_OCMode = TIM_OCMode_PWM1;
 314     if (output & TIMER_OUTPUT_N_CHANNEL) {
 315         OCINIT.TIM_OutputNState = TIM_OutputNState_Enable;
 316         OCINIT.TIM_OCNIdleState = TIM_OCNIdleState_Reset;
 317         OCINIT.TIM_OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_Low : TIM_OCNPolarity_High;
 318     } else {
 319         OCINIT.TIM_OutputState = TIM_OutputState_Enable;
 320         OCINIT.TIM_OCIdleState = TIM_OCIdleState_Set;
 321         OCINIT.TIM_OCPolarity =  (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High;
 322     }
 323     OCINIT.TIM_Pulse = 0;
 324
 325 #ifdef USE_DSHOT_TELEMETRY
 326     TIM_ICStructInit(&motor->icInitStruct);
 327     motor->icInitStruct.TIM_ICSelection = TIM_ICSelection_DirectTI;
 328     motor->icInitStruct.TIM_ICPolarity = TIM_ICPolarity_BothEdge;
 329     motor->icInitStruct.TIM_ICPrescaler = TIM_ICPSC_DIV1;
 330     motor->icInitStruct.TIM_Channel = timerHardware->channel;
 331     motor->icInitStruct.TIM_ICFilter = 2;
 332 #endif
 333
 334
 335 #ifdef USE_DSHOT_DMAR
 336     if (useBurstDshot) {
 337         motor->timer->dmaBurstRef = dmaRef;
 338     } else
 339 #endif
 340     {
 341         motor->timerDmaSource = timerDmaSource(timerHardware->channel);
 342         motor->timer->timerDmaSources &= ~motor->timerDmaSource;
 343     }
 344
 345     xDMA_Cmd(dmaRef, DISABLE);
 346     xDMA_DeInit(dmaRef);
 347
 348     if (!dmaIsConfigured) {
 349         dmaEnable(dmaIdentifier);
 350     }
 351
 352     DMA_StructInit(&DMAINIT);
 353 #ifdef USE_DSHOT_DMAR
 354     if (useBurstDshot) {
 355         motor->timer->dmaBurstBuffer = &dshotBurstDmaBuffer[timerIndex][0];
 356
 357         DMAINIT.DMA_Channel = timerHardware->dmaTimUPChannel;
 358         DMAINIT.DMA_Memory0BaseAddr = (uint32_t)motor->timer->dmaBurstBuffer;
 359         DMAINIT.DMA_DIR = DMA_DIR_MemoryToPeripheral;
 360         DMAINIT.DMA_FIFOMode = DMA_FIFOMode_Enable;
 361         DMAINIT.DMA_FIFOThreshold = DMA_FIFOThreshold_Full;
 362         DMAINIT.DMA_MemoryBurst = DMA_MemoryBurst_Single;
 363         DMAINIT.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
 364
 365         DMAINIT.DMA_PeripheralBaseAddr = (uint32_t)&timerHardware->tim->DMAR;
 366         DMAINIT.DMA_BufferSize = (pwmProtocolType == PWM_TYPE_PROSHOT1000) ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE; // XXX
 367         DMAINIT.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
 368         DMAINIT.DMA_MemoryInc = DMA_MemoryInc_Enable;
 369         DMAINIT.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;
 370         DMAINIT.DMA_MemoryDataSize = DMA_MemoryDataSize_Word;
 371         DMAINIT.DMA_Mode = DMA_Mode_Normal;
 372         DMAINIT.DMA_Priority = DMA_Priority_High;
 373     } else
 374 #endif
 375     {
 376         motor->dmaBuffer = &dshotDmaBuffer[motorIndex][0];
 377
 378 #if 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
 409 #ifdef USE_DSHOT_DMAR
 410     if (useBurstDshot) {
 411         if (!dmaIsConfigured) {
 412             dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
 413         }
 414     } else
 415 #endif
 416     {
 417         dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
 418     }
 419
 420     TIM_Cmd(timer, ENABLE);
 421     if (output & TIMER_OUTPUT_N_CHANNEL) {
 422         TIM_CCxNCmd(timer, timerHardware->channel, TIM_CCxN_Enable);
 423     } else {
 424         TIM_CCxCmd(timer, timerHardware->channel, TIM_CCx_Enable);
 425     }
 426     if (configureTimer) {
 427         TIM_ARRPreloadConfig(timer, ENABLE);
 428         TIM_CtrlPWMOutputs(timer, ENABLE);
 429         TIM_Cmd(timer, ENABLE);
 430     }
 431 #ifdef USE_DSHOT_TELEMETRY
 432     if (useDshotTelemetry) {
 433         // avoid high line during startup to prevent bootloader activation
 434         *timerChCCR(timerHardware) = 0xffff;
 435     }
 436 #endif
 437     motor->configured = true;
 438
 439     return true;
 440 }
 441
 442 #endif