src/platform/APM32/pwm_output_dshot_apm32.c

   1 /*
   2  * This file is part of Betaflight.
   3  *
   4  * Betaflight is free software. You can redistribute this software
   5  * and/or modify this software under the terms of the GNU General
   6  * Public License as published by the Free Software Foundation,
   7  * either version 3 of the License, or (at your option) any later
   8  * version.
   9  *
  10  * Betaflight is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  *
  14  * See the GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public
  17  * License along with this software.
  18  *
  19  * If not, see <http://www.gnu.org/licenses/>.
  20  */
  21
  22 #include <stdbool.h>
  23 #include <stdint.h>
  24 #include <math.h>
  25
  26 #include "platform.h"
  27
  28 #ifdef USE_DSHOT
  29
  30 #include "build/debug.h"
  31
  32 #include "common/time.h"
  33
  34 #include "drivers/dma.h"
  35 #include "drivers/dma_reqmap.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/motor.h"
  42 #include "drivers/pwm_output.h"
  43 #include "drivers/pwm_output_dshot_shared.h"
  44 #include "drivers/rcc.h"
  45 #include "drivers/time.h"
  46 #include "drivers/timer.h"
  47 #include "drivers/system.h"
  48
  49 #ifdef USE_DSHOT_TELEMETRY
  50
  51 void dshotEnableChannels(uint8_t motorCount)
  52 {
  53     for (int i = 0; i < motorCount; i++) {
  54         if (dmaMotors[i].output & TIMER_OUTPUT_N_CHANNEL) {
  55             DDL_TMR_CC_EnableChannel(dmaMotors[i].timerHardware->tim, dmaMotors[i].llChannel << 4);
  56         } else {
  57             DDL_TMR_CC_EnableChannel(dmaMotors[i].timerHardware->tim, dmaMotors[i].llChannel);
  58         }
  59     }
  60 }
  61
  62 #endif
  63
  64 void pwmDshotSetDirectionOutput(
  65     motorDmaOutput_t * const motor
  66 #ifndef USE_DSHOT_TELEMETRY
  67     , DDL_TMR_OC_InitTypeDef* pOcInit, DDL_DMA_InitTypeDef* pDmaInit
  68 #endif
  69 )
  70 {
  71 #ifdef USE_DSHOT_TELEMETRY
  72     DDL_TMR_OC_InitTypeDef* pOcInit = &motor->ocInitStruct;
  73     DDL_DMA_InitTypeDef* pDmaInit = &motor->dmaInitStruct;
  74 #endif
  75
  76     const timerHardware_t * const timerHardware = motor->timerHardware;
  77     TMR_TypeDef *timer = timerHardware->tim;
  78
  79 //     dmaResource_t *dmaRef = motor->dmaRef;
  80
  81 // #if defined(USE_DSHOT_DMAR) && !defined(USE_DSHOT_TELEMETRY)
  82 //     if (useBurstDshot) {
  83 //         dmaRef = timerHardware->dmaTimUPRef;
  84 //     }
  85 // #endif
  86
  87     xDDL_EX_DMA_DeInit(motor->dmaRef);
  88
  89 #ifdef USE_DSHOT_TELEMETRY
  90     motor->isInput = false;
  91 #endif
  92     DDL_TMR_OC_DisablePreload(timer, motor->llChannel);
  93     DDL_TMR_OC_Init(timer, motor->llChannel, pOcInit);
  94     DDL_TMR_OC_EnablePreload(timer, motor->llChannel);
  95
  96     motor->dmaInitStruct.Direction = DDL_DMA_DIRECTION_MEMORY_TO_PERIPH;
  97
  98     xDDL_EX_DMA_Init(motor->dmaRef, pDmaInit);
  99     xDDL_EX_DMA_EnableIT_TC(motor->dmaRef);
 100 }
 101
 102 #ifdef USE_DSHOT_TELEMETRY
 103 FAST_CODE static void pwmDshotSetDirectionInput(
 104     motorDmaOutput_t * const motor
 105 )
 106 {
 107     DDL_DMA_InitTypeDef* pDmaInit = &motor->dmaInitStruct;
 108
 109     const timerHardware_t * const timerHardware = motor->timerHardware;
 110     TMR_TypeDef *timer = timerHardware->tim;
 111
 112     xDDL_EX_DMA_DeInit(motor->dmaRef);
 113
 114     motor->isInput = true;
 115     if (!inputStampUs) {
 116         inputStampUs = micros();
 117     }
 118     DDL_TMR_EnableARRPreload(timer); // Only update the period once all channels are done
 119     timer->AUTORLD = 0xffffffff;
 120
 121     DDL_TMR_IC_Init(timer, motor->llChannel, &motor->icInitStruct);
 122
 123     motor->dmaInitStruct.Direction = DDL_DMA_DIRECTION_PERIPH_TO_MEMORY;
 124     xDDL_EX_DMA_Init(motor->dmaRef, pDmaInit);
 125 }
 126 #endif
 127
 128 FAST_CODE void pwmCompleteDshotMotorUpdate(void)
 129 {
 130     /* If there is a dshot command loaded up, time it correctly with motor update*/
 131     if (!dshotCommandQueueEmpty() && !dshotCommandOutputIsEnabled(dshotPwmDevice.count)) {
 132         return;
 133     }
 134
 135     for (int i = 0; i < dmaMotorTimerCount; i++) {
 136 #ifdef USE_DSHOT_DMAR
 137         if (useBurstDshot) {
 138             xDDL_EX_DMA_SetDataLength(dmaMotorTimers[i].dmaBurstRef, dmaMotorTimers[i].dmaBurstLength);
 139             xDDL_EX_DMA_EnableResource(dmaMotorTimers[i].dmaBurstRef);
 140
 141             /* configure the DMA Burst Mode */
 142             DDL_TMR_ConfigDMABurst(dmaMotorTimers[i].timer, DDL_TMR_DMABURST_BASEADDR_CC1, DDL_TMR_DMABURST_LENGTH_4TRANSFERS);
 143             /* Enable the TIM DMA Request */
 144             DDL_TMR_EnableDMAReq_UPDATE(dmaMotorTimers[i].timer);
 145         } else
 146 #endif
 147         {
 148             DDL_TMR_DisableARRPreload(dmaMotorTimers[i].timer);
 149             dmaMotorTimers[i].timer->AUTORLD = dmaMotorTimers[i].outputPeriod;
 150
 151             /* Reset timer counter */
 152             DDL_TMR_SetCounter(dmaMotorTimers[i].timer, 0);
 153             /* Enable channel DMA requests */
 154             DDL_EX_TMR_EnableIT(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources);
 155             dmaMotorTimers[i].timerDmaSources = 0;
 156         }
 157     }
 158 }
 159
 160 FAST_CODE static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
 161 {
 162     if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) {
 163         motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam];
 164 #ifdef USE_DSHOT_TELEMETRY
 165         dshotDMAHandlerCycleCounters.irqAt = getCycleCounter();
 166 #endif
 167 #ifdef USE_DSHOT_DMAR
 168         if (useBurstDshot) {
 169             xDDL_EX_DMA_DisableResource(motor->timerHardware->dmaTimUPRef);
 170             DDL_TMR_DisableDMAReq_UPDATE(motor->timerHardware->tim);
 171         } else
 172 #endif
 173         {
 174             xDDL_EX_DMA_DisableResource(motor->dmaRef);
 175             DDL_EX_TMR_DisableIT(motor->timerHardware->tim, motor->timerDmaSource);
 176         }
 177
 178 #ifdef USE_DSHOT_TELEMETRY
 179         if (useDshotTelemetry) {
 180             pwmDshotSetDirectionInput(motor);
 181             xDDL_EX_DMA_SetDataLength(motor->dmaRef, GCR_TELEMETRY_INPUT_LEN);
 182             xDDL_EX_DMA_EnableResource(motor->dmaRef);
 183             DDL_EX_TMR_EnableIT(motor->timerHardware->tim, motor->timerDmaSource);
 184             dshotDMAHandlerCycleCounters.changeDirectionCompletedAt = getCycleCounter();
 185         }
 186 #endif
 187         DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
 188     }
 189 }
 190
 191 bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, 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     DDL_TMR_OC_InitTypeDef ocInitStruct;
 198     DDL_DMA_InitTypeDef   dmaInitStruct;
 199 #define OCINIT ocInitStruct
 200 #define DMAINIT dmaInitStruct
 201 #endif
 202
 203     dmaResource_t *dmaRef = NULL;
 204     uint32_t dmaChannel = 0;
 205 #if defined(USE_DMA_SPEC)
 206     const dmaChannelSpec_t *dmaSpec = dmaGetChannelSpecByTimer(timerHardware);
 207
 208     if (dmaSpec != NULL) {
 209         dmaRef = dmaSpec->ref;
 210         dmaChannel = dmaSpec->channel;
 211     }
 212 #else
 213     dmaRef = timerHardware->dmaRef;
 214     dmaChannel = timerHardware->dmaChannel;
 215 #endif
 216
 217 #ifdef USE_DSHOT_DMAR
 218     if (useBurstDshot) {
 219         dmaRef = timerHardware->dmaTimUPRef;
 220         dmaChannel = timerHardware->dmaTimUPChannel;
 221     }
 222 #endif
 223
 224     if (dmaRef == NULL) {
 225         return false;
 226     }
 227
 228     dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(dmaRef);
 229
 230     bool dmaIsConfigured = false;
 231 #ifdef USE_DSHOT_DMAR
 232     if (useBurstDshot) {
 233         const resourceOwner_t *owner = dmaGetOwner(dmaIdentifier);
 234         if (owner->owner == OWNER_TIMUP && owner->resourceIndex == timerGetTIMNumber(timerHardware->tim)) {
 235             dmaIsConfigured = true;
 236         } else if (!dmaAllocate(dmaIdentifier, OWNER_TIMUP, timerGetTIMNumber(timerHardware->tim))) {
 237             return false;
 238         }
 239     } else
 240 #endif
 241     {
 242         if (!dmaAllocate(dmaIdentifier, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex))) {
 243             return false;
 244         }
 245     }
 246
 247     motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
 248     motor->dmaRef = dmaRef;
 249
 250     TMR_TypeDef *timer = timerHardware->tim;
 251
 252     const uint8_t timerIndex = getTimerIndex(timer);
 253     const bool configureTimer = (timerIndex == dmaMotorTimerCount - 1);
 254
 255     motor->timer = &dmaMotorTimers[timerIndex];
 256     motor->index = motorIndex;
 257
 258     const IO_t motorIO = IOGetByTag(timerHardware->tag);
 259     uint8_t pupMode = (output & TIMER_OUTPUT_INVERTED) ? GPIO_PULLDOWN : GPIO_PULLUP;
 260 #ifdef USE_DSHOT_TELEMETRY
 261     if (useDshotTelemetry) {
 262         output ^= TIMER_OUTPUT_INVERTED;
 263     }
 264 #endif
 265     motor->timerHardware = timerHardware;
 266
 267     motor->iocfg = IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_LOW, pupMode);
 268
 269     IOConfigGPIOAF(motorIO, motor->iocfg, timerHardware->alternateFunction);
 270
 271     if (configureTimer) {
 272         DDL_TMR_InitTypeDef init;
 273         DDL_TMR_StructInit(&init);
 274
 275         RCC_ClockCmd(timerRCC(timer), ENABLE);
 276         DDL_TMR_DisableCounter(timer);
 277
 278         init.Prescaler = (uint16_t)(lrintf((float) timerClock(timer) / getDshotHz(pwmProtocolType) + 0.01f) - 1);
 279         init.Autoreload = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH) - 1;
 280         init.ClockDivision = DDL_TMR_CLOCKDIVISION_DIV1;
 281         init.RepetitionCounter = 0;
 282         init.CounterMode = DDL_TMR_COUNTERMODE_UP;
 283         DDL_TMR_Init(timer, &init);
 284     }
 285
 286     DDL_TMR_OC_StructInit(&OCINIT);
 287     OCINIT.OCMode = DDL_TMR_OCMODE_PWM1;
 288     if (output & TIMER_OUTPUT_N_CHANNEL) {
 289         OCINIT.OCNState = DDL_TMR_OCSTATE_ENABLE;
 290         OCINIT.OCNIdleState = DDL_TMR_OCIDLESTATE_LOW;
 291         OCINIT.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? DDL_TMR_OCPOLARITY_LOW : DDL_TMR_OCPOLARITY_HIGH;
 292     } else {
 293         OCINIT.OCState = DDL_TMR_OCSTATE_ENABLE;
 294         OCINIT.OCIdleState = DDL_TMR_OCIDLESTATE_HIGH;
 295         OCINIT.OCPolarity =  (output & TIMER_OUTPUT_INVERTED) ? DDL_TMR_OCPOLARITY_LOW : DDL_TMR_OCPOLARITY_HIGH;
 296     }
 297     OCINIT.CompareValue = 0;
 298
 299 #ifdef USE_DSHOT_TELEMETRY
 300     DDL_TMR_IC_StructInit(&motor->icInitStruct);
 301     motor->icInitStruct.ICPolarity = DDL_TMR_IC_POLARITY_BOTHEDGE;
 302     motor->icInitStruct.ICPrescaler = DDL_TMR_ICPSC_DIV1;
 303     motor->icInitStruct.ICFilter = 2;
 304 #endif
 305
 306     uint32_t channel = 0;
 307     switch (timerHardware->channel) {
 308     case TMR_CHANNEL_1: channel = DDL_TMR_CHANNEL_CH1; break;
 309     case TMR_CHANNEL_2: channel = DDL_TMR_CHANNEL_CH2; break;
 310     case TMR_CHANNEL_3: channel = DDL_TMR_CHANNEL_CH3; break;
 311     case TMR_CHANNEL_4: channel = DDL_TMR_CHANNEL_CH4; break;
 312     }
 313     motor->llChannel = channel;
 314
 315 #ifdef USE_DSHOT_DMAR
 316     if (useBurstDshot) {
 317         motor->timer->dmaBurstRef = dmaRef;
 318 #ifdef USE_DSHOT_TELEMETRY
 319         motor->dmaRef = dmaRef;
 320 #endif
 321     } else
 322 #endif
 323     {
 324         motor->timerDmaSource = timerDmaSource(timerHardware->channel);
 325         motor->timer->timerDmaSources &= ~motor->timerDmaSource;
 326     }
 327
 328     if (!dmaIsConfigured) {
 329         xDDL_EX_DMA_DisableResource(dmaRef);
 330         xDDL_EX_DMA_DeInit(dmaRef);
 331
 332         dmaEnable(dmaIdentifier);
 333     }
 334
 335     DDL_DMA_StructInit(&DMAINIT);
 336 #ifdef USE_DSHOT_DMAR
 337     if (useBurstDshot) {
 338         motor->timer->dmaBurstBuffer = &dshotBurstDmaBuffer[timerIndex][0];
 339
 340         DMAINIT.Channel = dmaChannel;
 341         DMAINIT.MemoryOrM2MDstAddress = (uint32_t)motor->timer->dmaBurstBuffer;
 342         DMAINIT.FIFOThreshold = DDL_DMA_FIFOTHRESHOLD_FULL;
 343         DMAINIT.PeriphOrM2MSrcAddress = (uint32_t)&timerHardware->tim->DMAR;
 344     } else
 345 #endif
 346     {
 347         motor->dmaBuffer = &dshotDmaBuffer[motorIndex][0];
 348
 349         DMAINIT.Channel = dmaChannel;
 350         DMAINIT.MemoryOrM2MDstAddress = (uint32_t)motor->dmaBuffer;
 351         DMAINIT.FIFOThreshold = DDL_DMA_FIFOTHRESHOLD_1_4;
 352         DMAINIT.PeriphOrM2MSrcAddress = (uint32_t)timerChCCR(timerHardware);
 353     }
 354
 355     DMAINIT.Direction = DDL_DMA_DIRECTION_MEMORY_TO_PERIPH;
 356     DMAINIT.FIFOMode = DDL_DMA_FIFOMODE_ENABLE;
 357     DMAINIT.MemBurst = DDL_DMA_MBURST_SINGLE;
 358     DMAINIT.PeriphBurst = DDL_DMA_PBURST_SINGLE;
 359     DMAINIT.NbData = pwmProtocolType == PWM_TYPE_PROSHOT1000 ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE;
 360     DMAINIT.PeriphOrM2MSrcIncMode = DDL_DMA_PERIPH_NOINCREMENT;
 361     DMAINIT.MemoryOrM2MDstIncMode = DDL_DMA_MEMORY_INCREMENT;
 362     DMAINIT.PeriphOrM2MSrcDataSize = DDL_DMA_PDATAALIGN_WORD;
 363     DMAINIT.MemoryOrM2MDstDataSize = DDL_DMA_MDATAALIGN_WORD;
 364     DMAINIT.Mode = DDL_DMA_MODE_NORMAL;
 365     DMAINIT.Priority = DDL_DMA_PRIORITY_HIGH;
 366
 367     if (!dmaIsConfigured) {
 368         xDDL_EX_DMA_Init(dmaRef, &DMAINIT);
 369         xDDL_EX_DMA_EnableIT_TC(dmaRef);
 370     }
 371
 372     motor->dmaRef = dmaRef;
 373
 374 #ifdef USE_DSHOT_TELEMETRY
 375     motor->dshotTelemetryDeadtimeUs = DSHOT_TELEMETRY_DEADTIME_US + 1000000 *
 376         ( 16 * MOTOR_BITLENGTH) / getDshotHz(pwmProtocolType);
 377     motor->timer->outputPeriod = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1;
 378     pwmDshotSetDirectionOutput(motor);
 379 #else
 380     pwmDshotSetDirectionOutput(motor, &OCINIT, &DMAINIT);
 381 #endif
 382
 383 #ifdef USE_DSHOT_DMAR
 384     if (useBurstDshot) {
 385         if (!dmaIsConfigured) {
 386             dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
 387         }
 388     } else
 389 #endif
 390     {
 391         dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
 392     }
 393
 394     DDL_TMR_OC_Init(timer, channel, &OCINIT);
 395     DDL_TMR_OC_EnablePreload(timer, channel);
 396     DDL_TMR_OC_DisableFast(timer, channel);
 397
 398     DDL_TMR_EnableCounter(timer);
 399     if (output & TIMER_OUTPUT_N_CHANNEL) {
 400         DDL_EX_TMR_CC_EnableNChannel(timer, channel);
 401     } else {
 402         DDL_TMR_CC_EnableChannel(timer, channel);
 403     }
 404
 405     if (configureTimer) {
 406         DDL_TMR_EnableAllOutputs(timer);
 407         DDL_TMR_EnableARRPreload(timer);
 408         DDL_TMR_EnableCounter(timer);
 409     }
 410 #ifdef USE_DSHOT_TELEMETRY
 411     if (useDshotTelemetry) {
 412         // avoid high line during startup to prevent bootloader activation
 413         *timerChCCR(timerHardware) = 0xffff;
 414     }
 415 #endif
 416     motor->configured = true;
 417
 418     return true;
 419 }
 420 #endif