src/main/drivers/pwm_output_dshot_hal_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 #include <string.h>
  25
  26 #include "platform.h"
  27
  28 #ifdef USE_DSHOT
  29
  30 #include "drivers/dma_reqmap.h"
  31 #include "drivers/io.h"
  32 #include "timer.h"
  33 #include "pwm_output.h"
  34 #include "drivers/dshot.h"
  35 #include "drivers/dshot_dpwm.h"
  36 #include "drivers/dshot_command.h"
  37 #include "drivers/nvic.h"
  38 #include "dma.h"
  39 #include "rcc.h"
  40 #include "pg/timerup.h"
  41
  42 static HAL_StatusTypeDef result;
  43
  44 static uint8_t dmaMotorTimerCount = 0;
  45 static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
  46 static motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
  47
  48 motorDmaOutput_t *getMotorDmaOutput(uint8_t index)
  49 {
  50     return &dmaMotors[index];
  51 }
  52
  53 uint8_t getTimerIndex(TIM_TypeDef *timer)
  54 {
  55     for (int i = 0; i < dmaMotorTimerCount; i++) {
  56         if (dmaMotorTimers[i].timer == timer) {
  57             return i;
  58         }
  59     }
  60     dmaMotorTimers[dmaMotorTimerCount++].timer = timer;
  61     return dmaMotorTimerCount - 1;
  62 }
  63
  64 // TIM_CHANNEL_x TIM_CHANNEL_x/4 TIM_DMA_ID_CCx TIM_DMA_CCx
  65 // 0x0           0               1              0x0200
  66 // 0x4           1               2              0x0400
  67 // 0x8           2               3              0x0800
  68 // 0xC           3               4              0x1000
  69 //
  70 // TIM_CHANNEL_TO_TIM_DMA_ID_CC = (TIM_CHANNEL_x / 4) + 1
  71 // TIM_CHANNEL_TO_TIM_DMA_CC = 0x200 << (TIM_CHANNEL_x / 4)
  72
  73 // pwmChannelDMAStart
  74 // A variant of HAL_TIM_PWM_Start_DMA/HAL_TIMEx_PWMN_Start_DMA that only disables DMA on a timer channel:
  75 //   1. Configure and enable DMA Stream
  76 //   2. Enable DMA request on a timer channel
  77
  78 void pwmChannelDMAStart(TIM_HandleTypeDef *htim, uint32_t Channel, uint32_t *pData, uint16_t Length)
  79 {
  80     switch (Channel) {
  81     case TIM_CHANNEL_1:
  82         HAL_DMA_Start_IT(htim->hdma[TIM_DMA_ID_CC1], (uint32_t)pData, (uint32_t)&htim->Instance->CCR1, Length);
  83         __HAL_TIM_ENABLE_DMA(htim, TIM_DMA_CC1);
  84     break;
  85
  86     case TIM_CHANNEL_2:
  87         HAL_DMA_Start_IT(htim->hdma[TIM_DMA_ID_CC2], (uint32_t)pData, (uint32_t)&htim->Instance->CCR2, Length);
  88         __HAL_TIM_ENABLE_DMA(htim, TIM_DMA_CC2);
  89         break;
  90
  91     case TIM_CHANNEL_3:
  92         HAL_DMA_Start_IT(htim->hdma[TIM_DMA_ID_CC3], (uint32_t)pData, (uint32_t)&htim->Instance->CCR3,Length);
  93         __HAL_TIM_ENABLE_DMA(htim, TIM_DMA_CC3);
  94         break;
  95
  96     case TIM_CHANNEL_4:
  97         HAL_DMA_Start_IT(htim->hdma[TIM_DMA_ID_CC4], (uint32_t)pData, (uint32_t)&htim->Instance->CCR4, Length);
  98         __HAL_TIM_ENABLE_DMA(htim, TIM_DMA_CC4);
  99         break;
 100     }
 101 }
 102
 103 // pwmChannelDMAStop
 104 // A variant of HAL_TIM_PWM_Stop_DMA/HAL_TIMEx_PWMN_Stop_DMA that only disables DMA on a timer channel
 105 //   1. Disable the TIM Capture/Compare 1 DMA request
 106
 107 void pwmChannelDMAStop(TIM_HandleTypeDef *htim, uint32_t Channel)
 108 {
 109     switch (Channel) {
 110     case TIM_CHANNEL_1:
 111         __HAL_TIM_DISABLE_DMA(htim, TIM_DMA_CC1);
 112         break;
 113
 114     case TIM_CHANNEL_2:
 115         __HAL_TIM_DISABLE_DMA(htim, TIM_DMA_CC2);
 116         break;
 117
 118     case TIM_CHANNEL_3:
 119         __HAL_TIM_DISABLE_DMA(htim, TIM_DMA_CC3);
 120         break;
 121
 122     case TIM_CHANNEL_4:
 123         __HAL_TIM_DISABLE_DMA(htim, TIM_DMA_CC4);
 124         break;
 125     }
 126 }
 127
 128 // pwmBurstDMAStart
 129 // A variant of HAL_TIM_DMABurst_WriteStart that can handle multiple bursts.
 130 // (HAL_TIM_DMABurst_WriteStart can only handle single burst)
 131
 132 void pwmBurstDMAStart(TIM_HandleTypeDef *htim, uint32_t BurstBaseAddress, uint32_t BurstRequestSrc, uint32_t BurstUnit, uint32_t* BurstBuffer, uint32_t BurstLength)
 133 {
 134     // Setup DMA stream
 135     HAL_DMA_Start_IT(htim->hdma[TIM_DMA_ID_UPDATE], (uint32_t)BurstBuffer, (uint32_t)&htim->Instance->DMAR, BurstLength);
 136
 137     // Configure burst mode DMA */
 138    htim->Instance->DCR = BurstBaseAddress | BurstUnit;
 139
 140     // Enable burst mode DMA
 141     __HAL_TIM_ENABLE_DMA(htim, BurstRequestSrc);
 142 }
 143
 144 FAST_CODE void pwmWriteDshotInt(uint8_t index, uint16_t value)
 145 {
 146     motorDmaOutput_t *const motor = &dmaMotors[index];
 147
 148     if (!motor->configured) {
 149         return;
 150     }
 151
 152     /*If there is a command ready to go overwrite the value and send that instead*/
 153     if (dshotCommandIsProcessing()) {
 154         value = dshotCommandGetCurrent(index);
 155         if (value) {
 156             motor->protocolControl.requestTelemetry = true;
 157         }
 158     }
 159
 160     if (!motor->timerHardware
 161 #ifndef USE_DMA_SPEC
 162         // When USE_DMA_SPEC is in effect, motor->timerHardware remains NULL if valid DMA is not assigned.
 163         || !motor->timerHardware->dmaRef
 164 #endif
 165     )
 166     {
 167         return;
 168     }
 169
 170     motor->protocolControl.value = value;
 171
 172     uint16_t packet = prepareDshotPacket(&motor->protocolControl);
 173     uint8_t bufferSize;
 174
 175 #ifdef USE_DSHOT_DMAR
 176     if (useBurstDshot) {
 177         bufferSize = loadDmaBuffer(&motor->timer->dmaBurstBuffer[timerLookupChannelIndex(motor->timerHardware->channel)], 4, packet);
 178         motor->timer->dmaBurstLength = bufferSize * 4;
 179     } else
 180 #endif
 181     {
 182         bufferSize = loadDmaBuffer(motor->dmaBuffer, 1, packet);
 183
 184         pwmChannelDMAStart(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, bufferSize);
 185     }
 186 }
 187
 188 void pwmCompleteDshotMotorUpdate(void)
 189 {
 190     // If there is a dshot command loaded up, time it correctly with motor update
 191
 192     if (!dshotCommandQueueEmpty() && !dshotCommandOutputIsEnabled(dshotPwmDevice.count)) {
 193         return;
 194     }
 195
 196 #ifdef USE_DSHOT_DMAR
 197     if (useBurstDshot) {
 198         for (int i = 0; i < dmaMotorTimerCount; i++) {
 199             motorDmaTimer_t *burstDmaTimer = &dmaMotorTimers[i];
 200
 201             // Transfer CCR1 through CCR4 for each burst
 202             pwmBurstDMAStart(&burstDmaTimer->timHandle,
 203                     TIM_DMABASE_CCR1, TIM_DMA_UPDATE, TIM_DMABURSTLENGTH_4TRANSFERS,
 204                     (uint32_t*)burstDmaTimer->dmaBurstBuffer, burstDmaTimer->dmaBurstLength);
 205         }
 206     } else
 207 #endif
 208     {
 209         // XXX Empty for non-burst?
 210     }
 211 }
 212
 213 FAST_IRQ_HANDLER static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
 214 {
 215     if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) {
 216
 217 #ifdef USE_DSHOT_DMAR
 218         if (useBurstDshot) {
 219             motorDmaTimer_t *burstDmaTimer = &dmaMotorTimers[descriptor->userParam];
 220
 221             HAL_TIM_DMABurst_WriteStop(&burstDmaTimer->timHandle, TIM_DMA_UPDATE);
 222             HAL_DMA_IRQHandler(&burstDmaTimer->hdma_tim);
 223         } else
 224 #endif
 225         {
 226             motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
 227
 228             pwmChannelDMAStop(&motor->TimHandle,motor->timerHardware->channel);
 229             HAL_DMA_IRQHandler(motor->TimHandle.hdma[motor->timerDmaIndex]);
 230         }
 231
 232         DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
 233     }
 234 }
 235
 236 bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
 237 {
 238     dmaResource_t *dmaRef = NULL;
 239     uint32_t dmaChannel;
 240
 241 #ifdef USE_DMA_SPEC
 242     const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpecByTimer(timerHardware);
 243
 244     if (dmaSpec) {
 245         dmaRef = dmaSpec->ref;
 246         dmaChannel = dmaSpec->channel;
 247     }
 248 #else
 249     dmaRef = timerHardware->dmaRef;
 250     dmaChannel = timerHardware->dmaChannel;
 251 #endif
 252
 253 #ifdef USE_DSHOT_DMAR
 254     if (useBurstDshot) {
 255 #ifdef USE_DMA_SPEC
 256         uint8_t timnum = timerGetTIMNumber(timerHardware->tim);
 257         dmaoptValue_t dmaopt = timerUpConfig(timnum - 1)->dmaopt;
 258         const dmaChannelSpec_t *dmaChannelSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_TIMUP, timnum - 1, dmaopt);
 259         dmaRef = dmaChannelSpec->ref;
 260         dmaChannel = dmaChannelSpec->channel;
 261 #else
 262         dmaRef = timerHardware->dmaTimUPRef;
 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     motor->timerHardware = timerHardware;
 293
 294     TIM_TypeDef *timer = timerHardware->tim; // "timer" is confusing; "tim"?
 295     const IO_t motorIO = IOGetByTag(timerHardware->tag);
 296     uint8_t pupMode = (output & TIMER_OUTPUT_INVERTED) ? GPIO_PULLDOWN : GPIO_PULLUP;
 297 #ifdef USE_DSHOT_TELEMETRY
 298     if (useDshotTelemetry) {
 299         output ^= TIMER_OUTPUT_INVERTED;
 300     }
 301 #endif
 302
 303     motor->iocfg = IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, pupMode);
 304     const uint8_t timerIndex = getTimerIndex(timer);
 305     const bool configureTimer = (timerIndex == dmaMotorTimerCount - 1);
 306
 307     IOInit(motorIO, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
 308     IOConfigGPIOAF(motorIO, motor->iocfg, timerHardware->alternateFunction);
 309
 310     // Configure time base
 311
 312     if (configureTimer) {
 313         RCC_ClockCmd(timerRCC(timer), ENABLE);
 314
 315         motor->TimHandle.Instance = timerHardware->tim; // timer
 316         motor->TimHandle.Init.Prescaler = (uint16_t)(lrintf((float) timerClock(timer) / getDshotHz(pwmProtocolType) + 0.01f) - 1);
 317         motor->TimHandle.Init.Period = pwmProtocolType == PWM_TYPE_PROSHOT1000 ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH;
 318         motor->TimHandle.Init.RepetitionCounter = 0;
 319         motor->TimHandle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
 320         motor->TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP;
 321         motor->TimHandle.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
 322
 323         result = HAL_TIM_PWM_Init(&motor->TimHandle);
 324
 325         if (result != HAL_OK) {
 326             /* Initialization Error */
 327             return false;
 328         }
 329     }
 330
 331 /*
 332 From LL version
 333 chan oinv IDLE NIDLE POL  NPOL
 334 N    I    -    Low   -    Low
 335 N    -    -    Low   -    High
 336 P    I    High -     Low  -
 337 P    -    High -     High -
 338 */
 339
 340     /* PWM mode 1 configuration */
 341
 342     TIM_OC_InitTypeDef TIM_OCInitStructure;
 343     TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1;
 344
 345     if (output & TIMER_OUTPUT_N_CHANNEL) {
 346         TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET;
 347         TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_HIGH : TIM_OCPOLARITY_LOW;
 348         TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
 349         TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_HIGH : TIM_OCNPOLARITY_LOW;
 350     } else {
 351         TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET;
 352         TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_LOW : TIM_OCPOLARITY_HIGH;
 353         TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_SET;
 354         TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_LOW : TIM_OCNPOLARITY_HIGH;
 355     }
 356     TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
 357     TIM_OCInitStructure.Pulse = 0;
 358
 359     result = HAL_TIM_PWM_ConfigChannel(&motor->TimHandle, &TIM_OCInitStructure, motor->timerHardware->channel);
 360
 361     if (result != HAL_OK) {
 362         /* Configuration Error */
 363         return false;
 364     }
 365
 366     // DMA setup
 367
 368     motor->timer = &dmaMotorTimers[timerIndex];
 369
 370 #ifdef USE_DSHOT_DMAR
 371     if (useBurstDshot) {
 372         motor->timer->dmaBurstRef = dmaRef;
 373
 374         if (!configureTimer) {
 375             motor->configured = true;
 376             return false;
 377         }
 378     } else
 379 #endif
 380     {
 381         motor->timerDmaSource = timerDmaSource(timerHardware->channel);
 382         motor->timer->timerDmaSources |= motor->timerDmaSource;
 383         motor->timerDmaIndex = timerDmaIndex(timerHardware->channel);
 384     }
 385
 386     if (!dmaIsConfigured) {
 387         dmaEnable(dmaIdentifier);
 388 #ifdef USE_DSHOT_DMAR
 389         if (useBurstDshot) {
 390             dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, timerIndex);
 391         } else
 392 #endif
 393         {
 394             dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motorIndex);
 395         }
 396     }
 397
 398 #ifdef USE_DSHOT_DMAR
 399     if (useBurstDshot) {
 400         motor->timer->hdma_tim.Init.Direction = DMA_MEMORY_TO_PERIPH;
 401         motor->timer->hdma_tim.Init.PeriphInc = DMA_PINC_DISABLE;
 402         motor->timer->hdma_tim.Init.MemInc = DMA_MINC_ENABLE;
 403         motor->timer->hdma_tim.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD ;
 404         motor->timer->hdma_tim.Init.MemDataAlignment = DMA_MDATAALIGN_WORD ;
 405         motor->timer->hdma_tim.Init.Mode = DMA_NORMAL;
 406         motor->timer->hdma_tim.Init.Priority = DMA_PRIORITY_HIGH;
 407 #if !defined(STM32G4)
 408         motor->timer->hdma_tim.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
 409         motor->timer->hdma_tim.Init.PeriphBurst = DMA_PBURST_SINGLE;
 410         motor->timer->hdma_tim.Init.MemBurst = DMA_MBURST_SINGLE;
 411         motor->timer->hdma_tim.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL;
 412 #endif
 413
 414         motor->timer->dmaBurstBuffer = &dshotBurstDmaBuffer[timerIndex][0];
 415         motor->timer->timHandle = motor->TimHandle;
 416         memset(motor->timer->dmaBurstBuffer, 0, DSHOT_DMA_BUFFER_SIZE * 4 * sizeof(uint32_t));
 417
 418         /* Set hdma_tim instance */
 419         motor->timer->hdma_tim.Instance = (DMA_ARCH_TYPE *)dmaRef;
 420         motor->timer->hdma_tim.Init.Request = dmaChannel;
 421
 422         /* Link hdma_tim to hdma[TIM_DMA_ID_UPDATE] (update) */
 423         __HAL_LINKDMA(&motor->timer->timHandle, hdma[TIM_DMA_ID_UPDATE], motor->timer->hdma_tim);
 424
 425         if (!dmaIsConfigured) {
 426             /* Initialize TIMx DMA handle */
 427             result = HAL_DMA_Init(motor->timer->timHandle.hdma[TIM_DMA_ID_UPDATE]);
 428         } else {
 429             result = HAL_OK;
 430         }
 431     } else
 432 #endif
 433     {
 434         motor->hdma_tim.Init.Direction = DMA_MEMORY_TO_PERIPH;
 435         motor->hdma_tim.Init.PeriphInc = DMA_PINC_DISABLE;
 436         motor->hdma_tim.Init.MemInc = DMA_MINC_ENABLE;
 437         motor->hdma_tim.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD ;
 438         motor->hdma_tim.Init.MemDataAlignment = DMA_MDATAALIGN_WORD ;
 439         motor->hdma_tim.Init.Mode = DMA_NORMAL;
 440         motor->hdma_tim.Init.Priority = DMA_PRIORITY_HIGH;
 441 #if !defined(STM32G4)
 442         motor->hdma_tim.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
 443         motor->hdma_tim.Init.PeriphBurst = DMA_PBURST_SINGLE;
 444         motor->hdma_tim.Init.MemBurst = DMA_MBURST_SINGLE;
 445         motor->hdma_tim.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL;
 446 #endif
 447
 448         motor->dmaBuffer = &dshotDmaBuffer[motorIndex][0];
 449         motor->dmaBuffer[DSHOT_DMA_BUFFER_SIZE-2] = 0; // XXX Is this necessary? -> probably.
 450         motor->dmaBuffer[DSHOT_DMA_BUFFER_SIZE-1] = 0; // XXX Is this necessary?
 451
 452         motor->hdma_tim.Instance = (DMA_ARCH_TYPE *)dmaRef;
 453         motor->hdma_tim.Init.Request = dmaChannel;
 454
 455         /* Link hdma_tim to hdma[x] (channelx) */
 456         __HAL_LINKDMA(&motor->TimHandle, hdma[motor->timerDmaIndex], motor->hdma_tim);
 457
 458         /* Initialize TIMx DMA handle */
 459         result = HAL_DMA_Init(motor->TimHandle.hdma[motor->timerDmaIndex]);
 460
 461     }
 462
 463     if (result != HAL_OK) {
 464         /* Initialization Error */
 465         return false;
 466     }
 467
 468     // Start the timer channel now.
 469     // Enabling/disabling DMA request can restart a new cycle without PWM start/stop.
 470
 471     if (motor->timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
 472         result = HAL_TIMEx_PWMN_Start(&motor->TimHandle, motor->timerHardware->channel);
 473     } else {
 474         result = HAL_TIM_PWM_Start(&motor->TimHandle, motor->timerHardware->channel);
 475     }
 476
 477     if (result != HAL_OK) {
 478         /* Starting PWM generation Error */
 479         return false;
 480     }
 481
 482     motor->configured = true;
 483
 484     return true;
 485 }
 486 #endif