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