2 * This file is part of Cleanflight and Betaflight.
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)
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.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
29 #include "build/debug.h"
31 #include "drivers/dma.h"
32 #include "drivers/dma_reqmap.h"
33 #include "drivers/io.h"
34 #include "drivers/nvic.h"
35 #include "drivers/rcc.h"
36 #include "drivers/time.h"
37 #include "drivers/timer.h"
38 #include "drivers/system.h"
40 #include "stm32f4xx.h"
41 #elif defined(STM32F3)
42 #include "stm32f30x.h"
45 #include "pwm_output.h"
46 #include "drivers/dshot.h"
47 #include "drivers/dshot_dpwm.h"
48 #include "drivers/dshot_command.h"
50 #include "pwm_output_dshot_shared.h"
52 #ifdef USE_DSHOT_TELEMETRY
54 void dshotEnableChannels(uint8_t motorCount
)
56 for (int i
= 0; i
< motorCount
; i
++) {
57 if (dmaMotors
[i
].output
& TIMER_OUTPUT_N_CHANNEL
) {
58 TIM_CCxNCmd(dmaMotors
[i
].timerHardware
->tim
, dmaMotors
[i
].timerHardware
->channel
, TIM_CCxN_Enable
);
60 TIM_CCxCmd(dmaMotors
[i
].timerHardware
->tim
, dmaMotors
[i
].timerHardware
->channel
, TIM_CCx_Enable
);
67 FAST_CODE
void pwmDshotSetDirectionOutput(
68 motorDmaOutput_t
* const motor
69 #ifndef USE_DSHOT_TELEMETRY
70 ,TIM_OCInitTypeDef
*pOcInit
, DMA_InitTypeDef
* pDmaInit
74 #ifdef USE_DSHOT_TELEMETRY
75 TIM_OCInitTypeDef
* pOcInit
= &motor
->ocInitStruct
;
76 DMA_InitTypeDef
* pDmaInit
= &motor
->dmaInitStruct
;
79 const timerHardware_t
* const timerHardware
= motor
->timerHardware
;
80 TIM_TypeDef
*timer
= timerHardware
->tim
;
82 dmaResource_t
*dmaRef
= motor
->dmaRef
;
84 #if defined(USE_DSHOT_DMAR) && !defined(USE_DSHOT_TELEMETRY)
86 dmaRef
= timerHardware
->dmaTimUPRef
;
92 #ifdef USE_DSHOT_TELEMETRY
93 motor
->isInput
= false;
95 timerOCPreloadConfig(timer
, timerHardware
->channel
, TIM_OCPreload_Disable
);
96 timerOCInit(timer
, timerHardware
->channel
, pOcInit
);
97 timerOCPreloadConfig(timer
, timerHardware
->channel
, TIM_OCPreload_Enable
);
102 pDmaInit
->DMA_DIR
= DMA_DIR_PeripheralDST
;
104 pDmaInit
->DMA_DIR
= DMA_DIR_MemoryToPeripheral
;
110 pDmaInit
->DMA_DIR
= DMA_DIR_PeripheralDST
;
111 pDmaInit
->DMA_M2M
= DMA_M2M_Disable
;
112 #elif defined(STM32F4)
113 pDmaInit
->DMA_DIR
= DMA_DIR_MemoryToPeripheral
;
117 xDMA_Init(dmaRef
, pDmaInit
);
118 xDMA_ITConfig(dmaRef
, DMA_IT_TC
, ENABLE
);
122 #ifdef USE_DSHOT_TELEMETRY
123 FAST_CODE
static void pwmDshotSetDirectionInput(
124 motorDmaOutput_t
* const motor
127 DMA_InitTypeDef
* pDmaInit
= &motor
->dmaInitStruct
;
129 const timerHardware_t
* const timerHardware
= motor
->timerHardware
;
130 TIM_TypeDef
*timer
= timerHardware
->tim
;
132 dmaResource_t
*dmaRef
= motor
->dmaRef
;
136 motor
->isInput
= true;
138 inputStampUs
= micros();
140 TIM_ARRPreloadConfig(timer
, ENABLE
);
141 timer
->ARR
= 0xffffffff;
143 TIM_ICInit(timer
, &motor
->icInitStruct
);
146 motor
->dmaInitStruct
.DMA_DIR
= DMA_DIR_PeripheralSRC
;
147 motor
->dmaInitStruct
.DMA_M2M
= DMA_M2M_Disable
;
148 #elif defined(STM32F4)
149 motor
->dmaInitStruct
.DMA_DIR
= DMA_DIR_PeripheralToMemory
;
152 xDMA_Init(dmaRef
, pDmaInit
);
157 void pwmCompleteDshotMotorUpdate(void)
159 /* If there is a dshot command loaded up, time it correctly with motor update*/
160 if (!dshotCommandQueueEmpty()) {
161 if (!dshotCommandOutputIsEnabled(dshotPwmDevice
.count
)) {
166 for (int i
= 0; i
< dmaMotorTimerCount
; i
++) {
167 #ifdef USE_DSHOT_DMAR
169 xDMA_SetCurrDataCounter(dmaMotorTimers
[i
].dmaBurstRef
, dmaMotorTimers
[i
].dmaBurstLength
);
170 xDMA_Cmd(dmaMotorTimers
[i
].dmaBurstRef
, ENABLE
);
171 TIM_DMAConfig(dmaMotorTimers
[i
].timer
, TIM_DMABase_CCR1
, TIM_DMABurstLength_4Transfers
);
172 TIM_DMACmd(dmaMotorTimers
[i
].timer
, TIM_DMA_Update
, ENABLE
);
176 TIM_ARRPreloadConfig(dmaMotorTimers
[i
].timer
, DISABLE
);
177 dmaMotorTimers
[i
].timer
->ARR
= dmaMotorTimers
[i
].outputPeriod
;
178 TIM_ARRPreloadConfig(dmaMotorTimers
[i
].timer
, ENABLE
);
179 TIM_SetCounter(dmaMotorTimers
[i
].timer
, 0);
180 TIM_DMACmd(dmaMotorTimers
[i
].timer
, dmaMotorTimers
[i
].timerDmaSources
, ENABLE
);
181 dmaMotorTimers
[i
].timerDmaSources
= 0;
186 static void motor_DMA_IRQHandler(dmaChannelDescriptor_t
*descriptor
)
188 if (DMA_GET_FLAG_STATUS(descriptor
, DMA_IT_TCIF
)) {
189 motorDmaOutput_t
* const motor
= &dmaMotors
[descriptor
->userParam
];
190 #ifdef USE_DSHOT_TELEMETRY
191 dshotDMAHandlerCycleCounters
.irqAt
= getCycleCounter();
193 #ifdef USE_DSHOT_DMAR
195 xDMA_Cmd(motor
->timerHardware
->dmaTimUPRef
, DISABLE
);
196 TIM_DMACmd(motor
->timerHardware
->tim
, TIM_DMA_Update
, DISABLE
);
200 xDMA_Cmd(motor
->dmaRef
, DISABLE
);
201 TIM_DMACmd(motor
->timerHardware
->tim
, motor
->timerDmaSource
, DISABLE
);
204 #ifdef USE_DSHOT_TELEMETRY
205 if (useDshotTelemetry
) {
206 pwmDshotSetDirectionInput(motor
);
207 xDMA_SetCurrDataCounter(motor
->dmaRef
, GCR_TELEMETRY_INPUT_LEN
);
208 xDMA_Cmd(motor
->dmaRef
, ENABLE
);
209 TIM_DMACmd(motor
->timerHardware
->tim
, motor
->timerDmaSource
, ENABLE
);
210 dshotDMAHandlerCycleCounters
.changeDirectionCompletedAt
= getCycleCounter();
213 DMA_CLEAR_FLAG(descriptor
, DMA_IT_TCIF
);
217 bool pwmDshotMotorHardwareConfig(const timerHardware_t
*timerHardware
, uint8_t motorIndex
, motorPwmProtocolTypes_e pwmProtocolType
, uint8_t output
)
219 #ifdef USE_DSHOT_TELEMETRY
220 #define OCINIT motor->ocInitStruct
221 #define DMAINIT motor->dmaInitStruct
223 TIM_OCInitTypeDef ocInitStruct
;
224 DMA_InitTypeDef dmaInitStruct
;
225 #define OCINIT ocInitStruct
226 #define DMAINIT dmaInitStruct
229 dmaResource_t
*dmaRef
= NULL
;
231 uint32_t dmaChannel
= 0;
233 #if defined(USE_DMA_SPEC)
234 const dmaChannelSpec_t
*dmaSpec
= dmaGetChannelSpecByTimer(timerHardware
);
236 if (dmaSpec
!= NULL
) {
237 dmaRef
= dmaSpec
->ref
;
239 dmaChannel
= dmaSpec
->channel
;
243 dmaRef
= timerHardware
->dmaRef
;
245 dmaChannel
= timerHardware
->dmaChannel
;
249 #ifdef USE_DSHOT_DMAR
251 dmaRef
= timerHardware
->dmaTimUPRef
;
253 dmaChannel
= timerHardware
->dmaTimUPChannel
;
258 if (dmaRef
== NULL
) {
262 motorDmaOutput_t
* const motor
= &dmaMotors
[motorIndex
];
263 TIM_TypeDef
*timer
= timerHardware
->tim
;
265 // Boolean configureTimer is always true when different channels of the same timer are processed in sequence,
266 // causing the timer and the associated DMA initialized more than once.
267 // To fix this, getTimerIndex must be expanded to return if a new timer has been requested.
268 // However, since the initialization is idempotent, it is left as is in a favor of flash space (for now).
269 const uint8_t timerIndex
= getTimerIndex(timer
);
270 const bool configureTimer
= (timerIndex
== dmaMotorTimerCount
-1);
272 motor
->timer
= &dmaMotorTimers
[timerIndex
];
273 motor
->index
= motorIndex
;
274 motor
->timerHardware
= timerHardware
;
276 const IO_t motorIO
= IOGetByTag(timerHardware
->tag
);
279 pupMode
= (output
& TIMER_OUTPUT_INVERTED
) ? GPIO_PuPd_DOWN
: GPIO_PuPd_UP
;
280 #ifdef USE_DSHOT_TELEMETRY
281 if (useDshotTelemetry
) {
282 output
^= TIMER_OUTPUT_INVERTED
;
286 motor
->iocfg
= IO_CONFIG(GPIO_Mode_AF
, GPIO_Speed_50MHz
, GPIO_OType_PP
, pupMode
);
287 IOConfigGPIOAF(motorIO
, motor
->iocfg
, timerHardware
->alternateFunction
);
289 if (configureTimer
) {
290 TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure
;
291 TIM_TimeBaseStructInit(&TIM_TimeBaseStructure
);
293 RCC_ClockCmd(timerRCC(timer
), ENABLE
);
294 TIM_Cmd(timer
, DISABLE
);
296 TIM_TimeBaseStructure
.TIM_Prescaler
= (uint16_t)(lrintf((float) timerClock(timer
) / getDshotHz(pwmProtocolType
) + 0.01f
) - 1);
297 TIM_TimeBaseStructure
.TIM_Period
= (pwmProtocolType
== PWM_TYPE_PROSHOT1000
? (MOTOR_NIBBLE_LENGTH_PROSHOT
) : MOTOR_BITLENGTH
) - 1;
298 TIM_TimeBaseStructure
.TIM_ClockDivision
= TIM_CKD_DIV1
;
299 TIM_TimeBaseStructure
.TIM_RepetitionCounter
= 0;
300 TIM_TimeBaseStructure
.TIM_CounterMode
= TIM_CounterMode_Up
;
301 TIM_TimeBaseInit(timer
, &TIM_TimeBaseStructure
);
304 TIM_OCStructInit(&OCINIT
);
305 OCINIT
.TIM_OCMode
= TIM_OCMode_PWM1
;
306 if (output
& TIMER_OUTPUT_N_CHANNEL
) {
307 OCINIT
.TIM_OutputNState
= TIM_OutputNState_Enable
;
308 OCINIT
.TIM_OCNIdleState
= TIM_OCNIdleState_Reset
;
309 OCINIT
.TIM_OCNPolarity
= (output
& TIMER_OUTPUT_INVERTED
) ? TIM_OCNPolarity_Low
: TIM_OCNPolarity_High
;
311 OCINIT
.TIM_OutputState
= TIM_OutputState_Enable
;
312 OCINIT
.TIM_OCIdleState
= TIM_OCIdleState_Set
;
313 OCINIT
.TIM_OCPolarity
= (output
& TIMER_OUTPUT_INVERTED
) ? TIM_OCPolarity_Low
: TIM_OCPolarity_High
;
315 OCINIT
.TIM_Pulse
= 0;
317 #ifdef USE_DSHOT_TELEMETRY
318 TIM_ICStructInit(&motor
->icInitStruct
);
319 motor
->icInitStruct
.TIM_ICSelection
= TIM_ICSelection_DirectTI
;
320 motor
->icInitStruct
.TIM_ICPolarity
= TIM_ICPolarity_BothEdge
;
321 motor
->icInitStruct
.TIM_ICPrescaler
= TIM_ICPSC_DIV1
;
322 motor
->icInitStruct
.TIM_Channel
= timerHardware
->channel
;
323 motor
->icInitStruct
.TIM_ICFilter
= 2;
327 #ifdef USE_DSHOT_DMAR
329 motor
->timer
->dmaBurstRef
= dmaRef
;
333 motor
->timerDmaSource
= timerDmaSource(timerHardware
->channel
);
334 motor
->timer
->timerDmaSources
&= ~motor
->timerDmaSource
;
337 xDMA_Cmd(dmaRef
, DISABLE
);
339 DMA_StructInit(&DMAINIT
);
341 #ifdef USE_DSHOT_DMAR
343 dmaInit(timerHardware
->dmaTimUPIrqHandler
, OWNER_TIMUP
, timerGetTIMNumber(timerHardware
->tim
));
345 motor
->timer
->dmaBurstBuffer
= &dshotBurstDmaBuffer
[timerIndex
][0];
348 DMAINIT
.DMA_MemoryBaseAddr
= (uint32_t)motor
->timer
->dmaBurstBuffer
;
349 DMAINIT
.DMA_DIR
= DMA_DIR_PeripheralDST
;
351 DMAINIT
.DMA_Channel
= timerHardware
->dmaTimUPChannel
;
352 DMAINIT
.DMA_Memory0BaseAddr
= (uint32_t)motor
->timer
->dmaBurstBuffer
;
353 DMAINIT
.DMA_DIR
= DMA_DIR_MemoryToPeripheral
;
354 DMAINIT
.DMA_FIFOMode
= DMA_FIFOMode_Enable
;
355 DMAINIT
.DMA_FIFOThreshold
= DMA_FIFOThreshold_Full
;
356 DMAINIT
.DMA_MemoryBurst
= DMA_MemoryBurst_Single
;
357 DMAINIT
.DMA_PeripheralBurst
= DMA_PeripheralBurst_Single
;
359 DMAINIT
.DMA_PeripheralBaseAddr
= (uint32_t)&timerHardware
->tim
->DMAR
;
360 DMAINIT
.DMA_BufferSize
= (pwmProtocolType
== PWM_TYPE_PROSHOT1000
) ? PROSHOT_DMA_BUFFER_SIZE
: DSHOT_DMA_BUFFER_SIZE
; // XXX
361 DMAINIT
.DMA_PeripheralInc
= DMA_PeripheralInc_Disable
;
362 DMAINIT
.DMA_MemoryInc
= DMA_MemoryInc_Enable
;
363 DMAINIT
.DMA_PeripheralDataSize
= DMA_PeripheralDataSize_Word
;
364 DMAINIT
.DMA_MemoryDataSize
= DMA_MemoryDataSize_Word
;
365 DMAINIT
.DMA_Mode
= DMA_Mode_Normal
;
366 DMAINIT
.DMA_Priority
= DMA_Priority_High
;
370 dmaInit(dmaGetIdentifier(dmaRef
), OWNER_MOTOR
, RESOURCE_INDEX(motorIndex
));
372 motor
->dmaBuffer
= &dshotDmaBuffer
[motorIndex
][0];
375 DMAINIT
.DMA_MemoryBaseAddr
= (uint32_t)motor
->dmaBuffer
;
376 DMAINIT
.DMA_DIR
= DMA_DIR_PeripheralDST
;
377 DMAINIT
.DMA_M2M
= DMA_M2M_Disable
;
378 #elif defined(STM32F4)
379 DMAINIT
.DMA_Channel
= dmaChannel
;
380 DMAINIT
.DMA_Memory0BaseAddr
= (uint32_t)motor
->dmaBuffer
;
381 DMAINIT
.DMA_DIR
= DMA_DIR_MemoryToPeripheral
;
382 DMAINIT
.DMA_FIFOMode
= DMA_FIFOMode_Enable
;
383 DMAINIT
.DMA_FIFOThreshold
= DMA_FIFOThreshold_1QuarterFull
;
384 DMAINIT
.DMA_MemoryBurst
= DMA_MemoryBurst_Single
;
385 DMAINIT
.DMA_PeripheralBurst
= DMA_PeripheralBurst_Single
;
387 DMAINIT
.DMA_PeripheralBaseAddr
= (uint32_t)timerChCCR(timerHardware
);
388 DMAINIT
.DMA_PeripheralInc
= DMA_PeripheralInc_Disable
;
389 DMAINIT
.DMA_MemoryInc
= DMA_MemoryInc_Enable
;
390 DMAINIT
.DMA_PeripheralDataSize
= DMA_PeripheralDataSize_Word
;
391 DMAINIT
.DMA_MemoryDataSize
= DMA_MemoryDataSize_Word
;
392 DMAINIT
.DMA_Mode
= DMA_Mode_Normal
;
393 DMAINIT
.DMA_Priority
= DMA_Priority_High
;
396 // XXX Consolidate common settings in the next refactor
398 motor
->dmaRef
= dmaRef
;
400 #ifdef USE_DSHOT_TELEMETRY
401 motor
->dshotTelemetryDeadtimeUs
= DSHOT_TELEMETRY_DEADTIME_US
+ 1000000 *
402 (16 * MOTOR_BITLENGTH
) / getDshotHz(pwmProtocolType
);
403 motor
->timer
->outputPeriod
= (pwmProtocolType
== PWM_TYPE_PROSHOT1000
? (MOTOR_NIBBLE_LENGTH_PROSHOT
) : MOTOR_BITLENGTH
) - 1;
404 pwmDshotSetDirectionOutput(motor
);
406 pwmDshotSetDirectionOutput(motor
, &OCINIT
, &DMAINIT
);
408 #ifdef USE_DSHOT_DMAR
410 dmaSetHandler(timerHardware
->dmaTimUPIrqHandler
, motor_DMA_IRQHandler
, NVIC_PRIO_DSHOT_DMA
, motor
->index
);
414 dmaSetHandler(dmaGetIdentifier(dmaRef
), motor_DMA_IRQHandler
, NVIC_PRIO_DSHOT_DMA
, motor
->index
);
417 TIM_Cmd(timer
, ENABLE
);
418 if (output
& TIMER_OUTPUT_N_CHANNEL
) {
419 TIM_CCxNCmd(timer
, timerHardware
->channel
, TIM_CCxN_Enable
);
421 TIM_CCxCmd(timer
, timerHardware
->channel
, TIM_CCx_Enable
);
423 if (configureTimer
) {
424 TIM_ARRPreloadConfig(timer
, ENABLE
);
425 TIM_CtrlPWMOutputs(timer
, ENABLE
);
426 TIM_Cmd(timer
, ENABLE
);
428 #ifdef USE_DSHOT_TELEMETRY
429 if (useDshotTelemetry
) {
430 // avoid high line during startup to prevent bootloader activation
431 *timerChCCR(timerHardware
) = 0xffff;
434 motor
->configured
= true;