src/main/drivers/pwm_output_dshot_hal.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 "common/time.h"
  32
  33 #include "drivers/dma.h"
  34 #include "drivers/dma_reqmap.h"
  35 #include "drivers/dshot.h"
  36 #include "drivers/dshot_dpwm.h"
  37 #include "drivers/dshot_command.h"
  38 #include "drivers/io.h"
  39 #include "drivers/nvic.h"
  40 #include "drivers/motor.h"
  41 #include "drivers/pwm_output.h"
  42 #include "drivers/pwm_output_dshot_shared.h"
  43 #include "drivers/rcc.h"
  44 #include "drivers/time.h"
  45 #include "drivers/timer.h"
  46 #include "drivers/system.h"
  47
  48 #ifdef USE_DSHOT_TELEMETRY
  49
  50 void dshotEnableChannels(uint8_t motorCount)
  51 {
  52     for (int i = 0; i < motorCount; i++) {
  53         if (dmaMotors[i].output & TIMER_OUTPUT_N_CHANNEL) {
  54             LL_EX_TIM_CC_EnableNChannel(dmaMotors[i].timerHardware->tim, dmaMotors[i].llChannel);
  55         } else {
  56             LL_TIM_CC_EnableChannel(dmaMotors[i].timerHardware->tim, dmaMotors[i].llChannel);
  57         }
  58     }
  59 }
  60
  61 #endif
  62
  63 void pwmDshotSetDirectionOutput(
  64     motorDmaOutput_t * const motor
  65 #ifndef USE_DSHOT_TELEMETRY
  66     , LL_TIM_OC_InitTypeDef* pOcInit, LL_DMA_InitTypeDef* pDmaInit
  67 #endif
  68 )
  69 {
  70 #ifdef USE_DSHOT_TELEMETRY
  71     LL_TIM_OC_InitTypeDef* pOcInit = &motor->ocInitStruct;
  72     LL_DMA_InitTypeDef* pDmaInit = &motor->dmaInitStruct;
  73 #endif
  74
  75     const timerHardware_t * const timerHardware = motor->timerHardware;
  76     TIM_TypeDef *timer = timerHardware->tim;
  77
  78     xLL_EX_DMA_DeInit(motor->dmaRef);
  79
  80 #ifdef USE_DSHOT_TELEMETRY
  81     motor->isInput = false;
  82 #endif
  83     LL_TIM_OC_DisablePreload(timer, motor->llChannel);
  84     LL_TIM_OC_Init(timer, motor->llChannel, pOcInit);
  85     LL_TIM_OC_EnablePreload(timer, motor->llChannel);
  86
  87     motor->dmaInitStruct.Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH;
  88
  89     xLL_EX_DMA_Init(motor->dmaRef, pDmaInit);
  90     xLL_EX_DMA_EnableIT_TC(motor->dmaRef);
  91 }
  92
  93 #ifdef USE_DSHOT_TELEMETRY
  94 FAST_CODE static void pwmDshotSetDirectionInput(
  95     motorDmaOutput_t * const motor
  96 )
  97 {
  98     LL_DMA_InitTypeDef* pDmaInit = &motor->dmaInitStruct;
  99
 100     const timerHardware_t * const timerHardware = motor->timerHardware;
 101     TIM_TypeDef *timer = timerHardware->tim;
 102
 103     xLL_EX_DMA_DeInit(motor->dmaRef);
 104
 105     motor->isInput = true;
 106     if (!inputStampUs) {
 107         inputStampUs = micros();
 108     }
 109     LL_TIM_EnableARRPreload(timer); // Only update the period once all channels are done
 110     timer->ARR = 0xffffffff;
 111
 112 #ifdef STM32H7
 113     // Configure pin as GPIO output to avoid glitch during timer configuration
 114     uint32_t pin = IO_Pin(motor->io);
 115     LL_GPIO_SetPinMode(IO_GPIO(motor->io), pin, LL_GPIO_MODE_OUTPUT);
 116     LL_GPIO_SetPinSpeed(IO_GPIO(motor->io), pin, LL_GPIO_SPEED_FREQ_LOW); // Needs to be low
 117     LL_GPIO_SetPinPull(IO_GPIO(motor->io), pin, LL_GPIO_PULL_NO);
 118     LL_GPIO_SetPinOutputType(IO_GPIO(motor->io), pin, LL_GPIO_OUTPUT_PUSHPULL);
 119 #endif
 120
 121     LL_TIM_IC_Init(timer, motor->llChannel, &motor->icInitStruct);
 122
 123 #ifdef STM32H7
 124     // Configure pin back to timer
 125     LL_GPIO_SetPinMode(IO_GPIO(motor->io), IO_Pin(motor->io), LL_GPIO_MODE_ALTERNATE);
 126     if (IO_Pin(motor->io) & 0xFF) {
 127         LL_GPIO_SetAFPin_0_7(IO_GPIO(motor->io), IO_Pin(motor->io), timerHardware->alternateFunction);
 128     } else {
 129         LL_GPIO_SetAFPin_8_15(IO_GPIO(motor->io), IO_Pin(motor->io), timerHardware->alternateFunction);
 130     }
 131 #endif
 132
 133     motor->dmaInitStruct.Direction = LL_DMA_DIRECTION_PERIPH_TO_MEMORY;
 134     xLL_EX_DMA_Init(motor->dmaRef, pDmaInit);
 135 }
 136 #endif
 137
 138
 139 FAST_CODE void pwmCompleteDshotMotorUpdate(void)
 140 {
 141     /* If there is a dshot command loaded up, time it correctly with motor update*/
 142     if (!dshotCommandQueueEmpty() && !dshotCommandOutputIsEnabled(dshotPwmDevice.count)) {
 143         return;
 144     }
 145
 146     for (int i = 0; i < dmaMotorTimerCount; i++) {
 147 #ifdef USE_DSHOT_DMAR
 148         if (useBurstDshot) {
 149             xLL_EX_DMA_SetDataLength(dmaMotorTimers[i].dmaBurstRef, dmaMotorTimers[i].dmaBurstLength);
 150             xLL_EX_DMA_EnableResource(dmaMotorTimers[i].dmaBurstRef);
 151
 152             /* configure the DMA Burst Mode */
 153             LL_TIM_ConfigDMABurst(dmaMotorTimers[i].timer, LL_TIM_DMABURST_BASEADDR_CCR1, LL_TIM_DMABURST_LENGTH_4TRANSFERS);
 154             /* Enable the TIM DMA Request */
 155             LL_TIM_EnableDMAReq_UPDATE(dmaMotorTimers[i].timer);
 156         } else
 157 #endif
 158         {
 159             LL_TIM_DisableARRPreload(dmaMotorTimers[i].timer);
 160             dmaMotorTimers[i].timer->ARR = dmaMotorTimers[i].outputPeriod;
 161
 162             /* Reset timer counter */
 163             LL_TIM_SetCounter(dmaMotorTimers[i].timer, 0);
 164             /* Enable channel DMA requests */
 165             LL_EX_TIM_EnableIT(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources);
 166             dmaMotorTimers[i].timerDmaSources = 0;
 167         }
 168     }
 169 }
 170
 171 FAST_CODE static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
 172 {
 173     if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) {
 174         motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
 175 #ifdef USE_DSHOT_TELEMETRY
 176         if (!motor->isInput) {
 177             dshotDMAHandlerCycleCounters.irqAt = getCycleCounter();
 178 #endif
 179 #ifdef USE_DSHOT_DMAR
 180             if (useBurstDshot) {
 181                 xLL_EX_DMA_DisableResource(motor->timerHardware->dmaTimUPRef);
 182                 LL_TIM_DisableDMAReq_UPDATE(motor->timerHardware->tim);
 183             } else
 184 #endif
 185             {
 186                 xLL_EX_DMA_DisableResource(motor->dmaRef);
 187                 LL_EX_TIM_DisableIT(motor->timerHardware->tim, motor->timerDmaSource);
 188             }
 189
 190 #ifdef USE_DSHOT_TELEMETRY
 191             if (useDshotTelemetry) {
 192                 pwmDshotSetDirectionInput(motor);
 193                 xLL_EX_DMA_SetDataLength(motor->dmaRef, GCR_TELEMETRY_INPUT_LEN);
 194                 xLL_EX_DMA_EnableResource(motor->dmaRef);
 195                 LL_EX_TIM_EnableIT(motor->timerHardware->tim, motor->timerDmaSource);
 196                 dshotDMAHandlerCycleCounters.changeDirectionCompletedAt = getCycleCounter();
 197             }
 198         }
 199 #endif
 200         DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
 201     }
 202 }
 203
 204 bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
 205 {
 206 #ifdef USE_DSHOT_TELEMETRY
 207 #define OCINIT motor->ocInitStruct
 208 #define DMAINIT motor->dmaInitStruct
 209 #else
 210     LL_TIM_OC_InitTypeDef ocInitStruct;
 211     LL_DMA_InitTypeDef   dmaInitStruct;
 212 #define OCINIT ocInitStruct
 213 #define DMAINIT dmaInitStruct
 214 #endif
 215
 216     dmaResource_t *dmaRef = NULL;
 217     uint32_t dmaChannel = 0;
 218 #if defined(USE_DMA_SPEC)
 219     const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpecByTimer(timerHardware);
 220
 221     if (dmaSpec != NULL) {
 222         dmaRef = dmaSpec->ref;
 223         dmaChannel = dmaSpec->channel;
 224     }
 225 #else
 226     dmaRef = timerHardware->dmaRef;
 227     dmaChannel = timerHardware->dmaChannel;
 228 #endif
 229
 230 #ifdef USE_DSHOT_DMAR
 231     if (useBurstDshot) {
 232         dmaRef = timerHardware->dmaTimUPRef;
 233         dmaChannel = timerHardware->dmaTimUPChannel;
 234     }
 235 #endif
 236
 237     if (dmaRef == NULL) {
 238         return false;
 239     }
 240
 241     dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(dmaRef);
 242
 243     bool dmaIsConfigured = false;
 244 #ifdef USE_DSHOT_DMAR
 245     if (useBurstDshot) {
 246         const resourceOwner_t *owner = dmaGetOwner(dmaIdentifier);
 247         if (owner->owner == OWNER_TIMUP && owner->resourceIndex == timerGetTIMNumber(timerHardware->tim)) {
 248             dmaIsConfigured = true;
 249         } else if (!dmaAllocate(dmaIdentifier, OWNER_TIMUP, timerGetTIMNumber(timerHardware->tim))) {
 250             return false;
 251         }
 252     } else
 253 #endif
 254     {
 255         if (!dmaAllocate(dmaIdentifier, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex))) {
 256             return false;
 257         }
 258     }
 259
 260     motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
 261     motor->dmaRef = dmaRef;
 262
 263     TIM_TypeDef *timer = timerHardware->tim;
 264
 265     const uint8_t timerIndex = getTimerIndex(timer);
 266     const bool configureTimer = (timerIndex == dmaMotorTimerCount - 1);
 267
 268     motor->timer = &dmaMotorTimers[timerIndex];
 269     motor->index = motorIndex;
 270
 271     const IO_t motorIO = IOGetByTag(timerHardware->tag);
 272     uint8_t pupMode = (output & TIMER_OUTPUT_INVERTED) ? GPIO_PULLDOWN : GPIO_PULLUP;
 273 #ifdef USE_DSHOT_TELEMETRY
 274     if (useDshotTelemetry) {
 275         output ^= TIMER_OUTPUT_INVERTED;
 276 #ifdef STM32H7
 277         if (output & TIMER_OUTPUT_INVERTED) {
 278             IOHi(motorIO);
 279         } else {
 280             IOLo(motorIO);
 281         }
 282 #endif
 283     }
 284 #endif
 285     motor->timerHardware = timerHardware;
 286
 287     motor->iocfg = IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_LOW, pupMode);
 288 #ifdef STM32H7
 289     motor->io = motorIO;
 290 #endif
 291     IOConfigGPIOAF(motorIO, motor->iocfg, timerHardware->alternateFunction);
 292
 293     if (configureTimer) {
 294         LL_TIM_InitTypeDef init;
 295         LL_TIM_StructInit(&init);
 296
 297         RCC_ClockCmd(timerRCC(timer), ENABLE);
 298         LL_TIM_DisableCounter(timer);
 299
 300         init.Prescaler = (uint16_t)(lrintf((float) timerClock(timer) / getDshotHz(pwmProtocolType) + 0.01f) - 1);
 301         init.Autoreload = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH) - 1;
 302         init.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1;
 303         init.RepetitionCounter = 0;
 304         init.CounterMode = LL_TIM_COUNTERMODE_UP;
 305         LL_TIM_Init(timer, &init);
 306     }
 307
 308     LL_TIM_OC_StructInit(&OCINIT);
 309     OCINIT.OCMode = LL_TIM_OCMODE_PWM1;
 310     if (output & TIMER_OUTPUT_N_CHANNEL) {
 311         OCINIT.OCNState = LL_TIM_OCSTATE_ENABLE;
 312         OCINIT.OCNIdleState = LL_TIM_OCIDLESTATE_LOW;
 313         OCINIT.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? LL_TIM_OCPOLARITY_LOW : LL_TIM_OCPOLARITY_HIGH;
 314     } else {
 315         OCINIT.OCState = LL_TIM_OCSTATE_ENABLE;
 316         OCINIT.OCIdleState = LL_TIM_OCIDLESTATE_HIGH;
 317         OCINIT.OCPolarity =  (output & TIMER_OUTPUT_INVERTED) ? LL_TIM_OCPOLARITY_LOW : LL_TIM_OCPOLARITY_HIGH;
 318     }
 319     OCINIT.CompareValue = 0;
 320
 321 #ifdef USE_DSHOT_TELEMETRY
 322     LL_TIM_IC_StructInit(&motor->icInitStruct);
 323     motor->icInitStruct.ICPolarity = LL_TIM_IC_POLARITY_BOTHEDGE;
 324     motor->icInitStruct.ICPrescaler = LL_TIM_ICPSC_DIV1;
 325     motor->icInitStruct.ICFilter = 2;
 326 #endif
 327
 328     uint32_t channel = 0;
 329     switch (timerHardware->channel) {
 330     case TIM_CHANNEL_1: channel = LL_TIM_CHANNEL_CH1; break;
 331     case TIM_CHANNEL_2: channel = LL_TIM_CHANNEL_CH2; break;
 332     case TIM_CHANNEL_3: channel = LL_TIM_CHANNEL_CH3; break;
 333     case TIM_CHANNEL_4: channel = LL_TIM_CHANNEL_CH4; break;
 334     }
 335     motor->llChannel = channel;
 336
 337 #ifdef USE_DSHOT_DMAR
 338     if (useBurstDshot) {
 339         motor->timer->dmaBurstRef = dmaRef;
 340 #ifdef USE_DSHOT_TELEMETRY
 341         motor->dmaRef = dmaRef;
 342 #endif
 343     } else
 344 #endif
 345     {
 346         motor->timerDmaSource = timerDmaSource(timerHardware->channel);
 347         motor->timer->timerDmaSources &= ~motor->timerDmaSource;
 348     }
 349
 350     if (!dmaIsConfigured) {
 351         xLL_EX_DMA_DisableResource(dmaRef);
 352         xLL_EX_DMA_DeInit(dmaRef);
 353
 354         dmaEnable(dmaIdentifier);
 355     }
 356
 357     LL_DMA_StructInit(&DMAINIT);
 358 #ifdef USE_DSHOT_DMAR
 359     if (useBurstDshot) {
 360         motor->timer->dmaBurstBuffer = &dshotBurstDmaBuffer[timerIndex][0];
 361
 362 #if defined(STM32H7) || defined(STM32G4)
 363         DMAINIT.PeriphRequest = dmaChannel;
 364 #else
 365         DMAINIT.Channel = dmaChannel;
 366 #endif
 367         DMAINIT.MemoryOrM2MDstAddress = (uint32_t)motor->timer->dmaBurstBuffer;
 368 #ifndef STM32G4
 369         DMAINIT.FIFOThreshold = LL_DMA_FIFOTHRESHOLD_FULL;
 370 #endif
 371         DMAINIT.PeriphOrM2MSrcAddress = (uint32_t)&timerHardware->tim->DMAR;
 372     } else
 373 #endif
 374     {
 375         motor->dmaBuffer = &dshotDmaBuffer[motorIndex][0];
 376
 377 #if defined(STM32H7) || defined(STM32G4)
 378         DMAINIT.PeriphRequest = dmaChannel;
 379 #else
 380         DMAINIT.Channel = dmaChannel;
 381 #endif
 382         DMAINIT.MemoryOrM2MDstAddress = (uint32_t)motor->dmaBuffer;
 383 #ifndef STM32G4
 384         DMAINIT.FIFOThreshold = LL_DMA_FIFOTHRESHOLD_1_4;
 385 #endif
 386         DMAINIT.PeriphOrM2MSrcAddress = (uint32_t)timerChCCR(timerHardware);
 387     }
 388
 389     DMAINIT.Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH;
 390 #ifndef STM32G4
 391     DMAINIT.FIFOMode = LL_DMA_FIFOMODE_ENABLE;
 392     DMAINIT.MemBurst = LL_DMA_MBURST_SINGLE;
 393     DMAINIT.PeriphBurst = LL_DMA_PBURST_SINGLE;
 394 #endif
 395     DMAINIT.NbData = pwmProtocolType == PWM_TYPE_PROSHOT1000 ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE;
 396     DMAINIT.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT;
 397     DMAINIT.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT;
 398     DMAINIT.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_WORD;
 399     DMAINIT.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_WORD;
 400     DMAINIT.Mode = LL_DMA_MODE_NORMAL;
 401     DMAINIT.Priority = LL_DMA_PRIORITY_HIGH;
 402
 403     if (!dmaIsConfigured) {
 404         xLL_EX_DMA_Init(dmaRef, &DMAINIT);
 405         xLL_EX_DMA_EnableIT_TC(dmaRef);
 406     }
 407
 408     motor->dmaRef = dmaRef;
 409
 410 #ifdef USE_DSHOT_TELEMETRY
 411     motor->dshotTelemetryDeadtimeUs = DSHOT_TELEMETRY_DEADTIME_US + 1000000 *
 412         ( 16 * MOTOR_BITLENGTH) / getDshotHz(pwmProtocolType);
 413     motor->timer->outputPeriod = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1;
 414     pwmDshotSetDirectionOutput(motor);
 415 #else
 416     pwmDshotSetDirectionOutput(motor, &OCINIT, &DMAINIT);
 417 #endif
 418
 419 #ifdef USE_DSHOT_DMAR
 420     if (useBurstDshot) {
 421         if (!dmaIsConfigured) {
 422             dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
 423         }
 424     } else
 425 #endif
 426     {
 427         dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
 428     }
 429
 430     LL_TIM_OC_Init(timer, channel, &OCINIT);
 431     LL_TIM_OC_EnablePreload(timer, channel);
 432     LL_TIM_OC_DisableFast(timer, channel);
 433
 434     LL_TIM_EnableCounter(timer);
 435     if (output & TIMER_OUTPUT_N_CHANNEL) {
 436         LL_EX_TIM_CC_EnableNChannel(timer, channel);
 437     } else {
 438         LL_TIM_CC_EnableChannel(timer, channel);
 439     }
 440
 441     if (configureTimer) {
 442         LL_TIM_EnableAllOutputs(timer);
 443         LL_TIM_EnableARRPreload(timer);
 444         LL_TIM_EnableCounter(timer);
 445     }
 446 #ifdef USE_DSHOT_TELEMETRY
 447     if (useDshotTelemetry) {
 448         // avoid high line during startup to prevent bootloader activation
 449         *timerChCCR(timerHardware) = 0xffff;
 450     }
 451 #endif
 452     motor->configured = true;
 453
 454     return true;
 455 }
 456 #endif