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