src/main/drivers/dshot_bitbang_stdperiph.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 #include "build/debug_pin.h"
  32
  33 #include "drivers/io.h"
  34 #include "drivers/io_impl.h"
  35 #include "drivers/dma.h"
  36 #include "drivers/dma_reqmap.h"
  37 #include "drivers/dshot.h"
  38 #include "drivers/dshot_bitbang_impl.h"
  39 #include "drivers/dshot_command.h"
  40 #include "drivers/motor.h"
  41 #include "drivers/nvic.h"
  42 #include "drivers/pwm_output.h" // XXX for pwmOutputPort_t motors[]; should go away with refactoring
  43 #include "drivers/time.h"
  44 #include "drivers/timer.h"
  45
  46 #include "pg/motor.h"
  47
  48 void bbGpioSetup(bbMotor_t *bbMotor)
  49 {
  50     bbPort_t *bbPort = bbMotor->bbPort;
  51     int pinIndex = bbMotor->pinIndex;
  52
  53     bbPort->gpioModeMask |= (GPIO_MODER_MODER0 << (pinIndex * 2));
  54     bbPort->gpioModeInput |= (GPIO_Mode_IN << (pinIndex * 2));
  55     bbPort->gpioModeOutput |= (GPIO_Mode_OUT << (pinIndex * 2));
  56
  57 #ifdef USE_DSHOT_TELEMETRY
  58     if (useDshotTelemetry) {
  59         bbPort->gpioIdleBSRR |= (1 << pinIndex);         // BS (lower half)
  60     } else
  61 #endif
  62     {
  63         bbPort->gpioIdleBSRR |= (1 << (pinIndex + 16));  // BR (higher half)
  64     }
  65
  66 #ifdef USE_DSHOT_TELEMETRY
  67     if (useDshotTelemetry) {
  68         IOWrite(bbMotor->io, 1);
  69     } else
  70 #endif
  71     {
  72         IOWrite(bbMotor->io, 0);
  73     }
  74
  75 #if defined(STM32F4)
  76     IOConfigGPIO(bbMotor->io, IO_CONFIG(GPIO_Mode_OUT, GPIO_Speed_50MHz, GPIO_OType_PP, bbPuPdMode));
  77 #else
  78 #error MCU dependent code required
  79 #endif
  80 }
  81
  82 void bbTimerChannelInit(bbPort_t *bbPort)
  83 {
  84     const timerHardware_t *timhw = bbPort->timhw;
  85
  86     TIM_OCInitTypeDef TIM_OCStruct;
  87
  88     TIM_OCStructInit(&TIM_OCStruct);
  89     TIM_OCStruct.TIM_OCMode = TIM_OCMode_PWM1;
  90     TIM_OCStruct.TIM_OCIdleState = TIM_OCIdleState_Set;
  91     TIM_OCStruct.TIM_OutputState = TIM_OutputState_Enable;
  92     TIM_OCStruct.TIM_OCPolarity = TIM_OCPolarity_Low;
  93
  94     TIM_OCStruct.TIM_Pulse = 10; // Duty doesn't matter, but too value small would make monitor output invalid
  95
  96     TIM_Cmd(bbPort->timhw->tim, DISABLE);
  97
  98     timerOCInit(timhw->tim, timhw->channel, &TIM_OCStruct);
  99     // timerOCPreloadConfig(timhw->tim, timhw->channel, TIM_OCPreload_Enable);
 100
 101 #ifdef DEBUG_MONITOR_PACER
 102     if (timhw->tag) {
 103         IO_t io = IOGetByTag(timhw->tag);
 104         IOConfigGPIOAF(io, IOCFG_AF_PP, timhw->alternateFunction);
 105         IOInit(io, OWNER_DSHOT_BITBANG, 0);
 106         TIM_CtrlPWMOutputs(timhw->tim, ENABLE);
 107     }
 108 #endif
 109
 110     // Enable and keep it running
 111
 112     TIM_Cmd(bbPort->timhw->tim, ENABLE);
 113 }
 114
 115 #ifdef USE_DMA_REGISTER_CACHE
 116
 117 void bbLoadDMARegs(dmaResource_t *dmaResource, dmaRegCache_t *dmaRegCache)
 118 {
 119     ((DMA_Stream_TypeDef *)dmaResource)->CR = dmaRegCache->CR;
 120     ((DMA_Stream_TypeDef *)dmaResource)->FCR = dmaRegCache->FCR;
 121     ((DMA_Stream_TypeDef *)dmaResource)->NDTR = dmaRegCache->NDTR;
 122     ((DMA_Stream_TypeDef *)dmaResource)->PAR = dmaRegCache->PAR;
 123     ((DMA_Stream_TypeDef *)dmaResource)->M0AR = dmaRegCache->M0AR;
 124 }
 125
 126 static void bbSaveDMARegs(dmaResource_t *dmaResource, dmaRegCache_t *dmaRegCache)
 127 {
 128     dmaRegCache->CR = ((DMA_Stream_TypeDef *)dmaResource)->CR;
 129     dmaRegCache->FCR = ((DMA_Stream_TypeDef *)dmaResource)->FCR;
 130     dmaRegCache->NDTR = ((DMA_Stream_TypeDef *)dmaResource)->NDTR;
 131     dmaRegCache->PAR = ((DMA_Stream_TypeDef *)dmaResource)->PAR;
 132     dmaRegCache->M0AR = ((DMA_Stream_TypeDef *)dmaResource)->M0AR;
 133 }
 134 #endif
 135
 136 void bbSwitchToOutput(bbPort_t * bbPort)
 137 {
 138     dbgPinHi(1);
 139     // Output idle level before switching to output
 140     // Use BSRR register for this
 141     // Normal: Use BR (higher half)
 142     // Inverted: Use BS (lower half)
 143
 144     WRITE_REG(bbPort->gpio->BSRRL, bbPort->gpioIdleBSRR);
 145
 146     // Set GPIO to output
 147     ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
 148         MODIFY_REG(bbPort->gpio->MODER, bbPort->gpioModeMask, bbPort->gpioModeOutput);
 149     }
 150
 151     // Reinitialize port group DMA for output
 152
 153     dmaResource_t *dmaResource = bbPort->dmaResource;
 154 #ifdef USE_DMA_REGISTER_CACHE
 155     bbLoadDMARegs(dmaResource, &bbPort->dmaRegOutput);
 156 #else
 157     xDMA_DeInit(dmaResource);
 158     xDMA_Init(dmaResource, &bbPort->outputDmaInit);
 159     // Needs this, as it is DeInit'ed above...
 160     xDMA_ITConfig(dmaResource, DMA_IT_TC, ENABLE);
 161 #endif
 162
 163     // Reinitialize pacer timer for output
 164
 165     bbPort->timhw->tim->ARR = bbPort->outputARR;
 166
 167     bbPort->direction = DSHOT_BITBANG_DIRECTION_OUTPUT;
 168
 169     dbgPinLo(1);
 170 }
 171
 172 #ifdef USE_DSHOT_TELEMETRY
 173 void bbSwitchToInput(bbPort_t *bbPort)
 174 {
 175     dbgPinHi(1);
 176
 177     // Set GPIO to input
 178
 179     ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
 180         MODIFY_REG(bbPort->gpio->MODER, bbPort->gpioModeMask, bbPort->gpioModeInput);
 181     }
 182
 183     // Reinitialize port group DMA for input
 184
 185     dmaResource_t *dmaResource = bbPort->dmaResource;
 186 #ifdef USE_DMA_REGISTER_CACHE
 187     bbLoadDMARegs(dmaResource, &bbPort->dmaRegInput);
 188 #else
 189     xDMA_DeInit(dmaResource);
 190     xDMA_Init(dmaResource, &bbPort->inputDmaInit);
 191     // Needs this, as it is DeInit'ed above...
 192     xDMA_ITConfig(dmaResource, DMA_IT_TC, ENABLE);
 193 #endif
 194
 195     // Reinitialize pacer timer for input
 196
 197     bbPort->timhw->tim->CNT = 0;
 198     bbPort->timhw->tim->ARR = bbPort->inputARR;
 199
 200     bbDMA_Cmd(bbPort, ENABLE);
 201
 202     bbPort->direction = DSHOT_BITBANG_DIRECTION_INPUT;
 203
 204     dbgPinLo(1);
 205 }
 206 #endif
 207
 208 void bbDMAPreconfigure(bbPort_t *bbPort, uint8_t direction)
 209 {
 210     DMA_InitTypeDef *dmainit = (direction == DSHOT_BITBANG_DIRECTION_OUTPUT) ?  &bbPort->outputDmaInit : &bbPort->inputDmaInit;
 211
 212     DMA_StructInit(dmainit);
 213
 214     dmainit->DMA_Mode = DMA_Mode_Normal;
 215     dmainit->DMA_Channel = bbPort->dmaChannel;
 216     dmainit->DMA_PeripheralInc = DMA_PeripheralInc_Disable;
 217     dmainit->DMA_MemoryInc = DMA_MemoryInc_Enable;
 218     dmainit->DMA_FIFOMode = DMA_FIFOMode_Enable ;
 219     dmainit->DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull ;
 220     dmainit->DMA_MemoryBurst = DMA_MemoryBurst_Single ;
 221     dmainit->DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
 222
 223     if (direction == DSHOT_BITBANG_DIRECTION_OUTPUT) {
 224         dmainit->DMA_Priority = DMA_Priority_High;
 225         dmainit->DMA_DIR = DMA_DIR_MemoryToPeripheral;
 226         dmainit->DMA_BufferSize = bbPort->portOutputCount;
 227         dmainit->DMA_PeripheralBaseAddr = (uint32_t)&bbPort->gpio->BSRRL;
 228         dmainit->DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;
 229         dmainit->DMA_Memory0BaseAddr = (uint32_t)bbPort->portOutputBuffer;
 230         dmainit->DMA_MemoryDataSize = DMA_MemoryDataSize_Word;
 231
 232 #ifdef USE_DMA_REGISTER_CACHE
 233         xDMA_Init(bbPort->dmaResource, dmainit);
 234         bbSaveDMARegs(bbPort->dmaResource, &bbPort->dmaRegOutput);
 235 #endif
 236     } else {
 237         dmainit->DMA_Priority = DMA_Priority_VeryHigh;
 238         dmainit->DMA_DIR = DMA_DIR_PeripheralToMemory;
 239         dmainit->DMA_BufferSize = bbPort->portInputCount;
 240
 241         dmainit->DMA_PeripheralBaseAddr = (uint32_t)&bbPort->gpio->IDR;
 242
 243         dmainit->DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
 244         dmainit->DMA_Memory0BaseAddr = (uint32_t)bbPort->portInputBuffer;
 245         dmainit->DMA_MemoryDataSize = DMA_MemoryDataSize_Word;
 246
 247 #ifdef USE_DMA_REGISTER_CACHE
 248         xDMA_Init(bbPort->dmaResource, dmainit);
 249         bbSaveDMARegs(bbPort->dmaResource, &bbPort->dmaRegInput);
 250 #endif
 251     }
 252 }
 253
 254 void bbTIM_TimeBaseInit(bbPort_t *bbPort, uint16_t period)
 255 {
 256     TIM_TimeBaseInitTypeDef *init = &bbPort->timeBaseInit;
 257
 258     init->TIM_Prescaler = 0; // Feed raw timerClock
 259     init->TIM_ClockDivision = TIM_CKD_DIV1;
 260     init->TIM_CounterMode = TIM_CounterMode_Up;
 261     init->TIM_Period = period;
 262     TIM_TimeBaseInit(bbPort->timhw->tim, init);
 263     TIM_ARRPreloadConfig(bbPort->timhw->tim, ENABLE);
 264 }
 265
 266 void bbTIM_DMACmd(TIM_TypeDef* TIMx, uint16_t TIM_DMASource, FunctionalState NewState)
 267 {
 268     TIM_DMACmd(TIMx, TIM_DMASource, NewState);
 269 }
 270
 271 void bbDMA_ITConfig(bbPort_t *bbPort)
 272 {
 273     xDMA_ITConfig(bbPort->dmaResource, DMA_IT_TC, ENABLE);
 274 }
 275
 276 void bbDMA_Cmd(bbPort_t *bbPort, FunctionalState NewState)
 277 {
 278     xDMA_Cmd(bbPort->dmaResource, NewState);
 279 }
 280
 281 int bbDMA_Count(bbPort_t *bbPort)
 282 {
 283     return xDMA_GetCurrDataCounter(bbPort->dmaResource);
 284 }
 285
 286 #endif // USE_DSHOT_BB