src/main/drivers/dshot_bitbang_ll.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 <stdint.h>
  22 #include <math.h>
  23 #include <string.h>
  24
  25 #include "platform.h"
  26
  27 #ifdef USE_DSHOT_BITBANG
  28
  29 #include "build/atomic.h"
  30 #include "build/debug.h"
  31
  32 #include "drivers/io.h"
  33 #include "drivers/io_impl.h"
  34 #include "drivers/dma.h"
  35 #include "drivers/dma_reqmap.h"
  36 #include "drivers/dshot.h"
  37 #include "drivers/dshot_bitbang_impl.h"
  38 #include "drivers/dshot_command.h"
  39 #include "drivers/motor.h"
  40 #include "drivers/nvic.h"
  41 #include "drivers/pwm_output.h" // XXX for pwmOutputPort_t motors[]; should go away with refactoring
  42 #include "drivers/time.h"
  43 #include "drivers/timer.h"
  44
  45 #include "pg/motor.h"
  46
  47 // Setup GPIO_MODER and GPIO_ODR register manipulation values
  48
  49 void bbGpioSetup(bbMotor_t *bbMotor)
  50 {
  51     bbPort_t *bbPort = bbMotor->bbPort;
  52     int pinIndex = bbMotor->pinIndex;
  53
  54 #ifdef STM32H7
  55     bbPort->gpioModeMask |= (GPIO_MODER_MODE0 << (pinIndex * 2)); // A minor name change in H7 CMSIS
  56 #else
  57     bbPort->gpioModeMask |= (GPIO_MODER_MODER0 << (pinIndex * 2));
  58 #endif
  59     bbPort->gpioModeInput |= (LL_GPIO_MODE_INPUT << (pinIndex * 2));
  60     bbPort->gpioModeOutput |= (LL_GPIO_MODE_OUTPUT << (pinIndex * 2));
  61
  62 #ifdef USE_DSHOT_TELEMETRY
  63     if (useDshotTelemetry) {
  64         bbPort->gpioIdleBSRR |= (1 << pinIndex);         // BS (lower half)
  65     } else
  66 #endif
  67     {
  68         bbPort->gpioIdleBSRR |= (1 << (pinIndex + 16));  // BR (higher half)
  69     }
  70
  71 #ifdef USE_DSHOT_TELEMETRY
  72     if (useDshotTelemetry) {
  73         IOWrite(bbMotor->io, 1);
  74     } else
  75 #endif
  76     {
  77         IOWrite(bbMotor->io, 0);
  78     }
  79 }
  80
  81 void bbTimerChannelInit(bbPort_t *bbPort)
  82 {
  83     const timerHardware_t *timhw = bbPort->timhw;
  84
  85     switch (bbPort->timhw->channel) {
  86     case TIM_CHANNEL_1: bbPort->llChannel = LL_TIM_CHANNEL_CH1; break;
  87     case TIM_CHANNEL_2: bbPort->llChannel = LL_TIM_CHANNEL_CH2; break;
  88     case TIM_CHANNEL_3: bbPort->llChannel = LL_TIM_CHANNEL_CH3; break;
  89     case TIM_CHANNEL_4: bbPort->llChannel = LL_TIM_CHANNEL_CH4; break;
  90     }
  91
  92     LL_TIM_OC_InitTypeDef ocInit;
  93     LL_TIM_OC_StructInit(&ocInit);
  94     ocInit.OCMode = LL_TIM_OCMODE_PWM1;
  95     ocInit.OCIdleState = LL_TIM_OCIDLESTATE_HIGH;
  96     ocInit.OCState = LL_TIM_OCSTATE_ENABLE;
  97     ocInit.OCPolarity = LL_TIM_OCPOLARITY_LOW;
  98     ocInit.CompareValue = 10; // Duty doesn't matter, but too value small would make monitor output invalid
  99
 100     //TIM_Cmd(bbPort->timhw->tim, DISABLE);
 101     LL_TIM_DisableCounter(bbPort->timhw->tim);
 102
 103     //timerOCInit(timhw->tim, timhw->channel, &TIM_OCStruct);
 104     LL_TIM_OC_Init(timhw->tim, bbPort->llChannel, &ocInit);
 105
 106     //timerOCPreloadConfig(timhw->tim, timhw->channel, TIM_OCPreload_Enable);
 107     LL_TIM_OC_EnablePreload(timhw->tim, bbPort->llChannel);
 108
 109 #ifdef DEBUG_MONITOR_PACER
 110     if (timhw->tag) {
 111         IO_t io = IOGetByTag(timhw->tag);
 112         IOConfigGPIOAF(io, IOCFG_AF_PP, timhw->alternateFunction);
 113         IOInit(io, OWNER_DSHOT_BITBANG, 0);
 114         //TIM_CtrlPWMOutputs(timhw->tim, ENABLE);
 115         LL_TIM_EnableAllOutputs(timhw->tim);
 116     }
 117 #endif
 118
 119     // Enable and keep it running
 120
 121     //TIM_Cmd(bbPort->timhw->tim, ENABLE);
 122     LL_TIM_EnableCounter(bbPort->timhw->tim);
 123 }
 124
 125 #ifdef USE_DMA_REGISTER_CACHE
 126 void bbLoadDMARegs(dmaResource_t *dmaResource, dmaRegCache_t *dmaRegCache)
 127 {
 128 #if defined(STM32F7) || defined(STM32H7)
 129     ((DMA_ARCH_TYPE *)dmaResource)->CR = dmaRegCache->CR;
 130     ((DMA_ARCH_TYPE *)dmaResource)->FCR = dmaRegCache->FCR;
 131     ((DMA_ARCH_TYPE *)dmaResource)->NDTR = dmaRegCache->NDTR;
 132     ((DMA_ARCH_TYPE *)dmaResource)->PAR = dmaRegCache->PAR;
 133     ((DMA_ARCH_TYPE *)dmaResource)->M0AR = dmaRegCache->M0AR;
 134 #elif defined(STM32G4)
 135     ((DMA_ARCH_TYPE *)dmaResource)->CCR = dmaRegCache->CCR;
 136     ((DMA_ARCH_TYPE *)dmaResource)->CNDTR = dmaRegCache->CNDTR;
 137     ((DMA_ARCH_TYPE *)dmaResource)->CPAR = dmaRegCache->CPAR;
 138     ((DMA_ARCH_TYPE *)dmaResource)->CMAR = dmaRegCache->CMAR;
 139 #else
 140 #error MCU dependent code required
 141 #endif
 142 }
 143
 144 static void bbSaveDMARegs(dmaResource_t *dmaResource, dmaRegCache_t *dmaRegCache)
 145 {
 146 #if defined(STM32F7) || defined(STM32H7)
 147     dmaRegCache->CR = ((DMA_ARCH_TYPE *)dmaResource)->CR;
 148     dmaRegCache->FCR = ((DMA_ARCH_TYPE *)dmaResource)->FCR;
 149     dmaRegCache->NDTR = ((DMA_ARCH_TYPE *)dmaResource)->NDTR;
 150     dmaRegCache->PAR = ((DMA_ARCH_TYPE *)dmaResource)->PAR;
 151     dmaRegCache->M0AR = ((DMA_ARCH_TYPE *)dmaResource)->M0AR;
 152 #elif defined(STM32G4)
 153     dmaRegCache->CCR = ((DMA_ARCH_TYPE *)dmaResource)->CCR;
 154     dmaRegCache->CNDTR = ((DMA_ARCH_TYPE *)dmaResource)->CNDTR;
 155     dmaRegCache->CPAR = ((DMA_ARCH_TYPE *)dmaResource)->CPAR;
 156     dmaRegCache->CMAR = ((DMA_ARCH_TYPE *)dmaResource)->CMAR;
 157 #else
 158 #error MCU dependent code required
 159 #endif
 160 }
 161 #endif
 162
 163 void bbSwitchToOutput(bbPort_t * bbPort)
 164 {
 165     // Output idle level before switching to output
 166     // Use BSRR register for this
 167     // Normal: Use BR (higher half)
 168     // Inverted: Use BS (lower half)
 169
 170     WRITE_REG(bbPort->gpio->BSRR, bbPort->gpioIdleBSRR);
 171
 172     // Set GPIO to output
 173
 174     ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
 175         MODIFY_REG(bbPort->gpio->MODER, bbPort->gpioModeMask, bbPort->gpioModeOutput);
 176     }
 177
 178     // Reinitialize port group DMA for output
 179
 180     dmaResource_t *dmaResource = bbPort->dmaResource;
 181 #ifdef USE_DMA_REGISTER_CACHE
 182     bbDMA_Cmd(bbPort, DISABLE);
 183     bbLoadDMARegs(dmaResource, &bbPort->dmaRegOutput);
 184 #else
 185     //xDMA_DeInit(dmaResource);
 186     xLL_EX_DMA_Deinit(dmaResource);
 187     //xDMA_Init(dmaResource, &bbPort->outputDmaInit);
 188     xLL_EX_DMA_Init(dmaResource, &bbPort->outputDmaInit);
 189     // Needs this, as it is DeInit'ed above...
 190     //xDMA_ITConfig(dmaResource, DMA_IT_TC, ENABLE);
 191     xLL_EX_DMA_EnableIT_TC(dmaResource);
 192 #endif
 193
 194     // Reinitialize pacer timer for output
 195
 196     bbPort->timhw->tim->ARR = bbPort->outputARR;
 197
 198     bbPort->direction = DSHOT_BITBANG_DIRECTION_OUTPUT;
 199 }
 200
 201 #ifdef USE_DSHOT_TELEMETRY
 202 void bbSwitchToInput(bbPort_t *bbPort)
 203 {
 204     // Set GPIO to input
 205
 206     ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
 207         MODIFY_REG(bbPort->gpio->MODER, bbPort->gpioModeMask, bbPort->gpioModeInput);
 208     }
 209
 210     // Reinitialize port group DMA for input
 211
 212     dmaResource_t *dmaResource = bbPort->dmaResource;
 213 #ifdef USE_DMA_REGISTER_CACHE
 214     bbLoadDMARegs(dmaResource, &bbPort->dmaRegInput);
 215 #else
 216     //xDMA_DeInit(dmaResource);
 217     xLL_EX_DMA_Deinit(dmaResource);
 218     //xDMA_Init(dmaResource, &bbPort->inputDmaInit);
 219     xLL_EX_DMA_Init(dmaResource, &bbPort->inputDmaInit);
 220
 221     // Needs this, as it is DeInit'ed above...
 222     //xDMA_ITConfig(dmaResource, DMA_IT_TC, ENABLE);
 223     xLL_EX_DMA_EnableIT_TC(dmaResource);
 224 #endif
 225
 226     // Reinitialize pacer timer for input
 227
 228     bbPort->timhw->tim->ARR = bbPort->inputARR;
 229
 230     bbDMA_Cmd(bbPort, ENABLE);
 231
 232     bbPort->direction = DSHOT_BITBANG_DIRECTION_INPUT;
 233 }
 234 #endif
 235
 236 void bbDMAPreconfigure(bbPort_t *bbPort, uint8_t direction)
 237 {
 238     LL_DMA_InitTypeDef *dmainit = (direction == DSHOT_BITBANG_DIRECTION_OUTPUT) ?  &bbPort->outputDmaInit : &bbPort->inputDmaInit;
 239
 240     LL_DMA_StructInit(dmainit);
 241
 242     dmainit->Mode = LL_DMA_MODE_NORMAL;
 243 #if defined(STM32G4) || defined(STM32H7)
 244     dmainit->PeriphRequest = bbPort->dmaChannel;
 245 #else
 246     dmainit->Channel = bbPort->dmaChannel;
 247     dmainit->FIFOMode = LL_DMA_FIFOMODE_ENABLE ;
 248 #endif
 249
 250     dmainit->PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT;
 251     dmainit->MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT;
 252
 253     if (direction == DSHOT_BITBANG_DIRECTION_OUTPUT) {
 254         dmainit->Priority = LL_DMA_PRIORITY_VERYHIGH;
 255         dmainit->Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH;
 256         dmainit->NbData = bbPort->portOutputCount;
 257         dmainit->PeriphOrM2MSrcAddress = (uint32_t)&bbPort->gpio->BSRR;
 258         dmainit->PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_WORD;
 259         dmainit->MemoryOrM2MDstAddress = (uint32_t)bbPort->portOutputBuffer;
 260         dmainit->MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_WORD;
 261
 262 #ifdef USE_DMA_REGISTER_CACHE
 263         xLL_EX_DMA_Init(bbPort->dmaResource, dmainit);
 264         bbSaveDMARegs(bbPort->dmaResource, &bbPort->dmaRegOutput);
 265 #endif
 266     } else {
 267         dmainit->Priority = LL_DMA_PRIORITY_HIGH;
 268         dmainit->Direction = LL_DMA_DIRECTION_PERIPH_TO_MEMORY;
 269         dmainit->NbData = bbPort->portInputCount;
 270
 271         dmainit->PeriphOrM2MSrcAddress = (uint32_t)&bbPort->gpio->IDR;
 272         dmainit->PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_HALFWORD;
 273         dmainit->MemoryOrM2MDstAddress = (uint32_t)bbPort->portInputBuffer;
 274
 275 #ifdef STM32G4
 276         // XXX G4 seems to require 16-bit transfer
 277         dmainit->MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_HALFWORD;
 278 #else
 279         dmainit->MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_WORD;
 280 #endif
 281
 282 #ifdef USE_DMA_REGISTER_CACHE
 283         xLL_EX_DMA_Init(bbPort->dmaResource, dmainit);
 284         bbSaveDMARegs(bbPort->dmaResource, &bbPort->dmaRegInput);
 285 #endif
 286     }
 287 }
 288
 289 void bbTIM_TimeBaseInit(bbPort_t *bbPort, uint16_t period)
 290 {
 291     LL_TIM_InitTypeDef *init = &bbPort->timeBaseInit;
 292
 293     init->Prescaler = 0; // Feed raw timerClock
 294     init->ClockDivision = LL_TIM_CLOCKDIVISION_DIV1;
 295     init->CounterMode = LL_TIM_COUNTERMODE_UP;
 296     init->Autoreload = period;
 297     //TIM_TimeBaseInit(bbPort->timhw->tim, &bbPort->timeBaseInit);
 298     LL_TIM_Init(bbPort->timhw->tim, init);
 299     MODIFY_REG(bbPort->timhw->tim->CR1, TIM_CR1_ARPE, TIM_AUTORELOAD_PRELOAD_ENABLE);
 300 }
 301
 302 void bbTIM_DMACmd(TIM_TypeDef* TIMx, uint16_t TIM_DMASource, FunctionalState NewState)
 303 {
 304     //TIM_DMACmd(TIMx, TIM_DMASource, NewState);
 305     if (NewState == ENABLE) {
 306         SET_BIT(TIMx->DIER, TIM_DMASource);
 307     } else {
 308         CLEAR_BIT(TIMx->DIER, TIM_DMASource);
 309     }
 310 }
 311
 312 void bbDMA_ITConfig(bbPort_t *bbPort)
 313 {
 314     //xDMA_ITConfig(bbPort->dmaResource, DMA_IT_TC, ENABLE);
 315
 316     xLL_EX_DMA_EnableIT_TC(bbPort->dmaResource);
 317
 318 #if defined(STM32G4)
 319     SET_BIT(((DMA_Channel_TypeDef *)(bbPort->dmaResource))->CCR, DMA_CCR_TCIE|DMA_CCR_TEIE);
 320 #else
 321     SET_BIT(((DMA_Stream_TypeDef *)(bbPort->dmaResource))->CR, DMA_SxCR_TCIE|DMA_SxCR_TEIE);
 322 #endif
 323 }
 324
 325 void bbDMA_Cmd(bbPort_t *bbPort, FunctionalState NewState)
 326 {
 327     //xDMA_Cmd(bbPort->dmaResource, NewState);
 328
 329     if (NewState == ENABLE) {
 330         xLL_EX_DMA_EnableResource(bbPort->dmaResource);
 331     } else {
 332         xLL_EX_DMA_DisableResource(bbPort->dmaResource);
 333     }
 334 }
 335
 336 int bbDMA_Count(bbPort_t *bbPort)
 337 {
 338     return xLL_EX_DMA_GetDataLength(bbPort->dmaResource);
 339 }
 340 #endif // USE_DSHOT_BITBANG