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 #if defined(STM32F3)
 124 CCM_CODE
 125 #else
 126 FAST_CODE
 127 #endif
 128 static void pwmDshotSetDirectionInput(
 129     motorDmaOutput_t * const motor
 130 )
 131 {
 132     DMA_InitTypeDef* pDmaInit = &motor->dmaInitStruct;
 133
 134     const timerHardware_t * const timerHardware = motor->timerHardware;
 135     TIM_TypeDef *timer = timerHardware->tim;
 136
 137     dmaResource_t *dmaRef = motor->dmaRef;
 138
 139     xDMA_DeInit(dmaRef);
 140
 141     motor->isInput = true;
 142     if (!inputStampUs) {
 143         inputStampUs = micros();
 144     }
 145     TIM_ARRPreloadConfig(timer, ENABLE);
 146     timer->ARR = 0xffffffff;
 147
 148     TIM_ICInit(timer, &motor->icInitStruct);
 149
 150 #if defined(STM32F3)
 151     motor->dmaInitStruct.DMA_DIR = DMA_DIR_PeripheralSRC;
 152     motor->dmaInitStruct.DMA_M2M = DMA_M2M_Disable;
 153 #elif defined(STM32F4)
 154     motor->dmaInitStruct.DMA_DIR = DMA_DIR_PeripheralToMemory;
 155 #endif
 156
 157     xDMA_Init(dmaRef, pDmaInit);
 158 }
 159 #endif
 160
 161
 162 void pwmCompleteDshotMotorUpdate(void)
 163 {
 164     /* If there is a dshot command loaded up, time it correctly with motor update*/
 165     if (!dshotCommandQueueEmpty()) {
 166         if (!dshotCommandOutputIsEnabled(dshotPwmDevice.count)) {
 167             return;
 168         }
 169     }
 170
 171     for (int i = 0; i < dmaMotorTimerCount; i++) {
 172 #ifdef USE_DSHOT_DMAR
 173         if (useBurstDshot) {
 174             xDMA_SetCurrDataCounter(dmaMotorTimers[i].dmaBurstRef, dmaMotorTimers[i].dmaBurstLength);
 175             xDMA_Cmd(dmaMotorTimers[i].dmaBurstRef, ENABLE);
 176             TIM_DMAConfig(dmaMotorTimers[i].timer, TIM_DMABase_CCR1, TIM_DMABurstLength_4Transfers);
 177             TIM_DMACmd(dmaMotorTimers[i].timer, TIM_DMA_Update, ENABLE);
 178         } else
 179 #endif
 180         {
 181             TIM_ARRPreloadConfig(dmaMotorTimers[i].timer, DISABLE);
 182             dmaMotorTimers[i].timer->ARR = dmaMotorTimers[i].outputPeriod;
 183             TIM_ARRPreloadConfig(dmaMotorTimers[i].timer, ENABLE);
 184             TIM_SetCounter(dmaMotorTimers[i].timer, 0);
 185             TIM_DMACmd(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources, ENABLE);
 186             dmaMotorTimers[i].timerDmaSources = 0;
 187         }
 188     }
 189 }
 190
 191 #if defined(STM32F3)
 192 CCM_CODE
 193 #else
 194 FAST_CODE
 195 #endif
 196 static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor)
 197 {
 198     if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) {
 199         motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
 200 #ifdef USE_DSHOT_TELEMETRY
 201         dshotDMAHandlerCycleCounters.irqAt = getCycleCounter();
 202 #endif
 203 #ifdef USE_DSHOT_DMAR
 204         if (useBurstDshot) {
 205             xDMA_Cmd(motor->timerHardware->dmaTimUPRef, DISABLE);
 206             TIM_DMACmd(motor->timerHardware->tim, TIM_DMA_Update, DISABLE);
 207         } else
 208 #endif
 209         {
 210             xDMA_Cmd(motor->dmaRef, DISABLE);
 211             TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, DISABLE);
 212         }
 213
 214 #ifdef USE_DSHOT_TELEMETRY
 215         if (useDshotTelemetry) {
 216             pwmDshotSetDirectionInput(motor);
 217             xDMA_SetCurrDataCounter(motor->dmaRef, GCR_TELEMETRY_INPUT_LEN);
 218             xDMA_Cmd(motor->dmaRef, ENABLE);
 219             TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, ENABLE);
 220             dshotDMAHandlerCycleCounters.changeDirectionCompletedAt = getCycleCounter();
 221         }
 222 #endif
 223         DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
 224     }
 225 }
 226
 227 bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
 228 {
 229 #ifdef USE_DSHOT_TELEMETRY
 230 #define OCINIT motor->ocInitStruct
 231 #define DMAINIT motor->dmaInitStruct
 232 #else
 233     TIM_OCInitTypeDef ocInitStruct;
 234     DMA_InitTypeDef   dmaInitStruct;
 235 #define OCINIT ocInitStruct
 236 #define DMAINIT dmaInitStruct
 237 #endif
 238
 239     dmaResource_t *dmaRef = NULL;
 240 #if defined(STM32F4)
 241     uint32_t dmaChannel = 0;
 242 #endif
 243 #if defined(USE_DMA_SPEC)
 244     const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpecByTimer(timerHardware);
 245
 246     if (dmaSpec != NULL) {
 247         dmaRef = dmaSpec->ref;
 248 #if defined(STM32F4)
 249         dmaChannel = dmaSpec->channel;
 250 #endif
 251     }
 252 #else
 253     dmaRef = timerHardware->dmaRef;
 254 #if defined(STM32F4)
 255     dmaChannel = timerHardware->dmaChannel;
 256 #endif
 257 #endif
 258
 259 #ifdef USE_DSHOT_DMAR
 260     if (useBurstDshot) {
 261         dmaRef = timerHardware->dmaTimUPRef;
 262 #if defined(STM32F4)
 263         dmaChannel = timerHardware->dmaTimUPChannel;
 264 #endif
 265     }
 266 #endif
 267
 268     if (dmaRef == NULL) {
 269         return false;
 270     }
 271
 272     dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(dmaRef);
 273
 274     bool dmaIsConfigured = false;
 275 #ifdef USE_DSHOT_DMAR
 276     if (useBurstDshot) {
 277         const resourceOwner_t *owner = dmaGetOwner(dmaIdentifier);
 278         if (owner->owner == OWNER_TIMUP && owner->resourceIndex == timerGetTIMNumber(timerHardware->tim)) {
 279             dmaIsConfigured = true;
 280         } else if (!dmaAllocate(dmaIdentifier, OWNER_TIMUP, timerGetTIMNumber(timerHardware->tim))) {
 281             return false;
 282         }
 283     } else
 284 #endif
 285     {
 286         if (!dmaAllocate(dmaIdentifier, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex))) {
 287             return false;
 288         }
 289     }
 290
 291     motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
 292     TIM_TypeDef *timer = timerHardware->tim;
 293
 294     // Boolean configureTimer is always true when different channels of the same timer are processed in sequence,
 295     // causing the timer and the associated DMA initialized more than once.
 296     // To fix this, getTimerIndex must be expanded to return if a new timer has been requested.
 297     // However, since the initialization is idempotent, it is left as is in a favor of flash space (for now).
 298     const uint8_t timerIndex = getTimerIndex(timer);
 299     const bool configureTimer = (timerIndex == dmaMotorTimerCount-1);
 300
 301     motor->timer = &dmaMotorTimers[timerIndex];
 302     motor->index = motorIndex;
 303     motor->timerHardware = timerHardware;
 304
 305     const IO_t motorIO = IOGetByTag(timerHardware->tag);
 306
 307     uint8_t pupMode = 0;
 308     pupMode = (output & TIMER_OUTPUT_INVERTED) ? GPIO_PuPd_DOWN : GPIO_PuPd_UP;
 309 #ifdef USE_DSHOT_TELEMETRY
 310     if (useDshotTelemetry) {
 311         output ^= TIMER_OUTPUT_INVERTED;
 312     }
 313 #endif
 314
 315     motor->iocfg = IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, pupMode);
 316     IOConfigGPIOAF(motorIO, motor->iocfg, timerHardware->alternateFunction);
 317
 318     if (configureTimer) {
 319         TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
 320         TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
 321
 322         RCC_ClockCmd(timerRCC(timer), ENABLE);
 323         TIM_Cmd(timer, DISABLE);
 324
 325         TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t)(lrintf((float) timerClock(timer) / getDshotHz(pwmProtocolType) + 0.01f) - 1);
 326         TIM_TimeBaseStructure.TIM_Period = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1;
 327         TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
 328         TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
 329         TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
 330         TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure);
 331     }
 332
 333     TIM_OCStructInit(&OCINIT);
 334     OCINIT.TIM_OCMode = TIM_OCMode_PWM1;
 335     if (output & TIMER_OUTPUT_N_CHANNEL) {
 336         OCINIT.TIM_OutputNState = TIM_OutputNState_Enable;
 337         OCINIT.TIM_OCNIdleState = TIM_OCNIdleState_Reset;
 338         OCINIT.TIM_OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_Low : TIM_OCNPolarity_High;
 339     } else {
 340         OCINIT.TIM_OutputState = TIM_OutputState_Enable;
 341         OCINIT.TIM_OCIdleState = TIM_OCIdleState_Set;
 342         OCINIT.TIM_OCPolarity =  (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High;
 343     }
 344     OCINIT.TIM_Pulse = 0;
 345
 346 #ifdef USE_DSHOT_TELEMETRY
 347     TIM_ICStructInit(&motor->icInitStruct);
 348     motor->icInitStruct.TIM_ICSelection = TIM_ICSelection_DirectTI;
 349     motor->icInitStruct.TIM_ICPolarity = TIM_ICPolarity_BothEdge;
 350     motor->icInitStruct.TIM_ICPrescaler = TIM_ICPSC_DIV1;
 351     motor->icInitStruct.TIM_Channel = timerHardware->channel;
 352     motor->icInitStruct.TIM_ICFilter = 2;
 353 #endif
 354
 355
 356 #ifdef USE_DSHOT_DMAR
 357     if (useBurstDshot) {
 358         motor->timer->dmaBurstRef = dmaRef;
 359     } else
 360 #endif
 361     {
 362         motor->timerDmaSource = timerDmaSource(timerHardware->channel);
 363         motor->timer->timerDmaSources &= ~motor->timerDmaSource;
 364     }
 365
 366     xDMA_Cmd(dmaRef, DISABLE);
 367     xDMA_DeInit(dmaRef);
 368
 369     if (!dmaIsConfigured) {
 370         dmaEnable(dmaIdentifier);
 371     }
 372
 373     DMA_StructInit(&DMAINIT);
 374 #ifdef USE_DSHOT_DMAR
 375     if (useBurstDshot) {
 376         motor->timer->dmaBurstBuffer = &dshotBurstDmaBuffer[timerIndex][0];
 377
 378 #if defined(STM32F3)
 379         DMAINIT.DMA_MemoryBaseAddr = (uint32_t)motor->timer->dmaBurstBuffer;
 380         DMAINIT.DMA_DIR = DMA_DIR_PeripheralDST;
 381 #else
 382         DMAINIT.DMA_Channel = timerHardware->dmaTimUPChannel;
 383         DMAINIT.DMA_Memory0BaseAddr = (uint32_t)motor->timer->dmaBurstBuffer;
 384         DMAINIT.DMA_DIR = DMA_DIR_MemoryToPeripheral;
 385         DMAINIT.DMA_FIFOMode = DMA_FIFOMode_Enable;
 386         DMAINIT.DMA_FIFOThreshold = DMA_FIFOThreshold_Full;
 387         DMAINIT.DMA_MemoryBurst = DMA_MemoryBurst_Single;
 388         DMAINIT.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
 389 #endif
 390         DMAINIT.DMA_PeripheralBaseAddr = (uint32_t)&timerHardware->tim->DMAR;
 391         DMAINIT.DMA_BufferSize = (pwmProtocolType == PWM_TYPE_PROSHOT1000) ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE; // XXX
 392         DMAINIT.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
 393         DMAINIT.DMA_MemoryInc = DMA_MemoryInc_Enable;
 394         DMAINIT.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;
 395         DMAINIT.DMA_MemoryDataSize = DMA_MemoryDataSize_Word;
 396         DMAINIT.DMA_Mode = DMA_Mode_Normal;
 397         DMAINIT.DMA_Priority = DMA_Priority_High;
 398     } else
 399 #endif
 400     {
 401         motor->dmaBuffer = &dshotDmaBuffer[motorIndex][0];
 402
 403 #if defined(STM32F3)
 404         DMAINIT.DMA_MemoryBaseAddr = (uint32_t)motor->dmaBuffer;
 405         DMAINIT.DMA_DIR = DMA_DIR_PeripheralDST;
 406         DMAINIT.DMA_M2M = DMA_M2M_Disable;
 407 #elif defined(STM32F4)
 408         DMAINIT.DMA_Channel = dmaChannel;
 409         DMAINIT.DMA_Memory0BaseAddr = (uint32_t)motor->dmaBuffer;
 410         DMAINIT.DMA_DIR = DMA_DIR_MemoryToPeripheral;
 411         DMAINIT.DMA_FIFOMode = DMA_FIFOMode_Enable;
 412         DMAINIT.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull;
 413         DMAINIT.DMA_MemoryBurst = DMA_MemoryBurst_Single;
 414         DMAINIT.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
 415 #endif
 416         DMAINIT.DMA_PeripheralBaseAddr = (uint32_t)timerChCCR(timerHardware);
 417         DMAINIT.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
 418         DMAINIT.DMA_MemoryInc = DMA_MemoryInc_Enable;
 419         DMAINIT.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;
 420         DMAINIT.DMA_MemoryDataSize = DMA_MemoryDataSize_Word;
 421         DMAINIT.DMA_Mode = DMA_Mode_Normal;
 422         DMAINIT.DMA_Priority = DMA_Priority_High;
 423     }
 424
 425     // XXX Consolidate common settings in the next refactor
 426
 427     motor->dmaRef = dmaRef;
 428
 429 #ifdef USE_DSHOT_TELEMETRY
 430     motor->dshotTelemetryDeadtimeUs = DSHOT_TELEMETRY_DEADTIME_US + 1000000 *
 431         (16 * MOTOR_BITLENGTH) / getDshotHz(pwmProtocolType);
 432     motor->timer->outputPeriod = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1;
 433     pwmDshotSetDirectionOutput(motor);
 434 #else
 435     pwmDshotSetDirectionOutput(motor, &OCINIT, &DMAINIT);
 436 #endif
 437
 438 #ifdef USE_DSHOT_DMAR
 439     if (useBurstDshot) {
 440         if (!dmaIsConfigured) {
 441             dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
 442         }
 443     } else
 444 #endif
 445     {
 446         dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
 447     }
 448
 449     TIM_Cmd(timer, ENABLE);
 450     if (output & TIMER_OUTPUT_N_CHANNEL) {
 451         TIM_CCxNCmd(timer, timerHardware->channel, TIM_CCxN_Enable);
 452     } else {
 453         TIM_CCxCmd(timer, timerHardware->channel, TIM_CCx_Enable);
 454     }
 455     if (configureTimer) {
 456         TIM_ARRPreloadConfig(timer, ENABLE);
 457         TIM_CtrlPWMOutputs(timer, ENABLE);
 458         TIM_Cmd(timer, ENABLE);
 459     }
 460 #ifdef USE_DSHOT_TELEMETRY
 461     if (useDshotTelemetry) {
 462         // avoid high line during startup to prevent bootloader activation
 463         *timerChCCR(timerHardware) = 0xffff;
 464     }
 465 #endif
 466     motor->configured = true;
 467
 468     return true;
 469 }
 470
 471 #endif