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