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 "common/time.h"
33 #include "drivers/dma.h"
34 #include "drivers/dma_reqmap.h"
35 #include "drivers/dshot.h"
36 #include "drivers/dshot_dpwm.h"
37 #include "drivers/dshot_command.h"
38 #include "drivers/io.h"
39 #include "drivers/nvic.h"
40 #include "drivers/motor.h"
41 #include "drivers/pwm_output.h"
42 #include "drivers/pwm_output_dshot_shared.h"
43 #include "drivers/rcc.h"
44 #include "drivers/time.h"
45 #include "drivers/timer.h"
46 #include "drivers/system.h"
48 #ifdef USE_DSHOT_TELEMETRY
50 void dshotEnableChannels(uint8_t motorCount
)
52 for (int i
= 0; i
< motorCount
; i
++) {
53 if (dmaMotors
[i
].output
& TIMER_OUTPUT_N_CHANNEL
) {
54 LL_EX_TIM_CC_EnableNChannel(dmaMotors
[i
].timerHardware
->tim
, dmaMotors
[i
].llChannel
);
56 LL_TIM_CC_EnableChannel(dmaMotors
[i
].timerHardware
->tim
, dmaMotors
[i
].llChannel
);
63 void pwmDshotSetDirectionOutput(
64 motorDmaOutput_t
* const motor
65 #ifndef USE_DSHOT_TELEMETRY
66 , LL_TIM_OC_InitTypeDef
* pOcInit
, LL_DMA_InitTypeDef
* pDmaInit
70 #ifdef USE_DSHOT_TELEMETRY
71 LL_TIM_OC_InitTypeDef
* pOcInit
= &motor
->ocInitStruct
;
72 LL_DMA_InitTypeDef
* pDmaInit
= &motor
->dmaInitStruct
;
75 const timerHardware_t
* const timerHardware
= motor
->timerHardware
;
76 TIM_TypeDef
*timer
= timerHardware
->tim
;
78 xLL_EX_DMA_DeInit(motor
->dmaRef
);
80 #ifdef USE_DSHOT_TELEMETRY
81 motor
->isInput
= false;
83 LL_TIM_OC_DisablePreload(timer
, motor
->llChannel
);
84 LL_TIM_OC_Init(timer
, motor
->llChannel
, pOcInit
);
85 LL_TIM_OC_EnablePreload(timer
, motor
->llChannel
);
87 motor
->dmaInitStruct
.Direction
= LL_DMA_DIRECTION_MEMORY_TO_PERIPH
;
89 xLL_EX_DMA_Init(motor
->dmaRef
, pDmaInit
);
90 xLL_EX_DMA_EnableIT_TC(motor
->dmaRef
);
93 #ifdef USE_DSHOT_TELEMETRY
94 FAST_CODE
static void pwmDshotSetDirectionInput(
95 motorDmaOutput_t
* const motor
98 LL_DMA_InitTypeDef
* pDmaInit
= &motor
->dmaInitStruct
;
100 const timerHardware_t
* const timerHardware
= motor
->timerHardware
;
101 TIM_TypeDef
*timer
= timerHardware
->tim
;
103 xLL_EX_DMA_DeInit(motor
->dmaRef
);
105 motor
->isInput
= true;
107 inputStampUs
= micros();
109 LL_TIM_EnableARRPreload(timer
); // Only update the period once all channels are done
110 timer
->ARR
= 0xffffffff;
113 // Configure pin as GPIO output to avoid glitch during timer configuration
114 uint32_t pin
= IO_Pin(motor
->io
);
115 LL_GPIO_SetPinMode(IO_GPIO(motor
->io
), pin
, LL_GPIO_MODE_OUTPUT
);
116 LL_GPIO_SetPinSpeed(IO_GPIO(motor
->io
), pin
, LL_GPIO_SPEED_FREQ_LOW
); // Needs to be low
117 LL_GPIO_SetPinPull(IO_GPIO(motor
->io
), pin
, LL_GPIO_PULL_NO
);
118 LL_GPIO_SetPinOutputType(IO_GPIO(motor
->io
), pin
, LL_GPIO_OUTPUT_PUSHPULL
);
121 LL_TIM_IC_Init(timer
, motor
->llChannel
, &motor
->icInitStruct
);
124 // Configure pin back to timer
125 LL_GPIO_SetPinMode(IO_GPIO(motor
->io
), IO_Pin(motor
->io
), LL_GPIO_MODE_ALTERNATE
);
126 if (IO_Pin(motor
->io
) & 0xFF) {
127 LL_GPIO_SetAFPin_0_7(IO_GPIO(motor
->io
), IO_Pin(motor
->io
), timerHardware
->alternateFunction
);
129 LL_GPIO_SetAFPin_8_15(IO_GPIO(motor
->io
), IO_Pin(motor
->io
), timerHardware
->alternateFunction
);
133 motor
->dmaInitStruct
.Direction
= LL_DMA_DIRECTION_PERIPH_TO_MEMORY
;
134 xLL_EX_DMA_Init(motor
->dmaRef
, pDmaInit
);
139 FAST_CODE
void pwmCompleteDshotMotorUpdate(void)
141 /* If there is a dshot command loaded up, time it correctly with motor update*/
142 if (!dshotCommandQueueEmpty() && !dshotCommandOutputIsEnabled(dshotPwmDevice
.count
)) {
146 for (int i
= 0; i
< dmaMotorTimerCount
; i
++) {
147 #ifdef USE_DSHOT_DMAR
149 xLL_EX_DMA_SetDataLength(dmaMotorTimers
[i
].dmaBurstRef
, dmaMotorTimers
[i
].dmaBurstLength
);
150 xLL_EX_DMA_EnableResource(dmaMotorTimers
[i
].dmaBurstRef
);
152 /* configure the DMA Burst Mode */
153 LL_TIM_ConfigDMABurst(dmaMotorTimers
[i
].timer
, LL_TIM_DMABURST_BASEADDR_CCR1
, LL_TIM_DMABURST_LENGTH_4TRANSFERS
);
154 /* Enable the TIM DMA Request */
155 LL_TIM_EnableDMAReq_UPDATE(dmaMotorTimers
[i
].timer
);
159 LL_TIM_DisableARRPreload(dmaMotorTimers
[i
].timer
);
160 dmaMotorTimers
[i
].timer
->ARR
= dmaMotorTimers
[i
].outputPeriod
;
162 /* Reset timer counter */
163 LL_TIM_SetCounter(dmaMotorTimers
[i
].timer
, 0);
164 /* Enable channel DMA requests */
165 LL_EX_TIM_EnableIT(dmaMotorTimers
[i
].timer
, dmaMotorTimers
[i
].timerDmaSources
);
166 dmaMotorTimers
[i
].timerDmaSources
= 0;
171 FAST_CODE
static void motor_DMA_IRQHandler(dmaChannelDescriptor_t
* descriptor
)
173 if (DMA_GET_FLAG_STATUS(descriptor
, DMA_IT_TCIF
)) {
174 motorDmaOutput_t
* const motor
= &dmaMotors
[descriptor
->userParam
];
175 if (!motor
->isInput
) {
176 #ifdef USE_DSHOT_TELEMETRY
177 dshotDMAHandlerCycleCounters
.irqAt
= getCycleCounter();
179 #ifdef USE_DSHOT_DMAR
181 xLL_EX_DMA_DisableResource(motor
->timerHardware
->dmaTimUPRef
);
182 LL_TIM_DisableDMAReq_UPDATE(motor
->timerHardware
->tim
);
186 xLL_EX_DMA_DisableResource(motor
->dmaRef
);
187 LL_EX_TIM_DisableIT(motor
->timerHardware
->tim
, motor
->timerDmaSource
);
190 #ifdef USE_DSHOT_TELEMETRY
191 if (useDshotTelemetry
) {
192 pwmDshotSetDirectionInput(motor
);
193 xLL_EX_DMA_SetDataLength(motor
->dmaRef
, GCR_TELEMETRY_INPUT_LEN
);
194 xLL_EX_DMA_EnableResource(motor
->dmaRef
);
195 LL_EX_TIM_EnableIT(motor
->timerHardware
->tim
, motor
->timerDmaSource
);
196 dshotDMAHandlerCycleCounters
.changeDirectionCompletedAt
= getCycleCounter();
200 DMA_CLEAR_FLAG(descriptor
, DMA_IT_TCIF
);
204 bool pwmDshotMotorHardwareConfig(const timerHardware_t
*timerHardware
, uint8_t motorIndex
, uint8_t reorderedMotorIndex
, motorPwmProtocolTypes_e pwmProtocolType
, uint8_t output
)
206 #ifdef USE_DSHOT_TELEMETRY
207 #define OCINIT motor->ocInitStruct
208 #define DMAINIT motor->dmaInitStruct
210 LL_TIM_OC_InitTypeDef ocInitStruct
;
211 LL_DMA_InitTypeDef dmaInitStruct
;
212 #define OCINIT ocInitStruct
213 #define DMAINIT dmaInitStruct
216 dmaResource_t
*dmaRef
= NULL
;
217 uint32_t dmaChannel
= 0;
218 #if defined(USE_DMA_SPEC)
219 const dmaChannelSpec_t
*dmaSpec
= dmaGetChannelSpecByTimer(timerHardware
);
221 if (dmaSpec
!= NULL
) {
222 dmaRef
= dmaSpec
->ref
;
223 dmaChannel
= dmaSpec
->channel
;
226 dmaRef
= timerHardware
->dmaRef
;
227 dmaChannel
= timerHardware
->dmaChannel
;
230 #ifdef USE_DSHOT_DMAR
232 dmaRef
= timerHardware
->dmaTimUPRef
;
233 dmaChannel
= timerHardware
->dmaTimUPChannel
;
237 if (dmaRef
== NULL
) {
241 dmaIdentifier_e dmaIdentifier
= dmaGetIdentifier(dmaRef
);
243 bool dmaIsConfigured
= false;
244 #ifdef USE_DSHOT_DMAR
246 const resourceOwner_t
*owner
= dmaGetOwner(dmaIdentifier
);
247 if (owner
->owner
== OWNER_TIMUP
&& owner
->resourceIndex
== timerGetTIMNumber(timerHardware
->tim
)) {
248 dmaIsConfigured
= true;
249 } else if (!dmaAllocate(dmaIdentifier
, OWNER_TIMUP
, timerGetTIMNumber(timerHardware
->tim
))) {
255 if (!dmaAllocate(dmaIdentifier
, OWNER_MOTOR
, RESOURCE_INDEX(reorderedMotorIndex
))) {
260 motorDmaOutput_t
* const motor
= &dmaMotors
[motorIndex
];
261 motor
->dmaRef
= dmaRef
;
263 TIM_TypeDef
*timer
= timerHardware
->tim
;
265 const uint8_t timerIndex
= getTimerIndex(timer
);
266 const bool configureTimer
= (timerIndex
== dmaMotorTimerCount
- 1);
268 motor
->timer
= &dmaMotorTimers
[timerIndex
];
269 motor
->index
= motorIndex
;
271 const IO_t motorIO
= IOGetByTag(timerHardware
->tag
);
272 uint8_t pupMode
= (output
& TIMER_OUTPUT_INVERTED
) ? GPIO_PULLDOWN
: GPIO_PULLUP
;
273 #ifdef USE_DSHOT_TELEMETRY
274 if (useDshotTelemetry
) {
275 output
^= TIMER_OUTPUT_INVERTED
;
277 if (output
& TIMER_OUTPUT_INVERTED
) {
285 motor
->timerHardware
= timerHardware
;
287 motor
->iocfg
= IO_CONFIG(GPIO_MODE_AF_PP
, GPIO_SPEED_FREQ_VERY_HIGH
, pupMode
);
291 IOConfigGPIOAF(motorIO
, motor
->iocfg
, timerHardware
->alternateFunction
);
293 if (configureTimer
) {
294 LL_TIM_InitTypeDef init
;
295 LL_TIM_StructInit(&init
);
297 RCC_ClockCmd(timerRCC(timer
), ENABLE
);
298 LL_TIM_DisableCounter(timer
);
300 init
.Prescaler
= (uint16_t)(lrintf((float) timerClock(timer
) / getDshotHz(pwmProtocolType
) + 0.01f
) - 1);
301 init
.Autoreload
= (pwmProtocolType
== PWM_TYPE_PROSHOT1000
? MOTOR_NIBBLE_LENGTH_PROSHOT
: MOTOR_BITLENGTH
) - 1;
302 init
.ClockDivision
= LL_TIM_CLOCKDIVISION_DIV1
;
303 init
.RepetitionCounter
= 0;
304 init
.CounterMode
= LL_TIM_COUNTERMODE_UP
;
305 LL_TIM_Init(timer
, &init
);
308 LL_TIM_OC_StructInit(&OCINIT
);
309 OCINIT
.OCMode
= LL_TIM_OCMODE_PWM1
;
310 if (output
& TIMER_OUTPUT_N_CHANNEL
) {
311 OCINIT
.OCNState
= LL_TIM_OCSTATE_ENABLE
;
312 OCINIT
.OCNIdleState
= LL_TIM_OCIDLESTATE_LOW
;
313 OCINIT
.OCNPolarity
= (output
& TIMER_OUTPUT_INVERTED
) ? LL_TIM_OCPOLARITY_LOW
: LL_TIM_OCPOLARITY_HIGH
;
315 OCINIT
.OCState
= LL_TIM_OCSTATE_ENABLE
;
316 OCINIT
.OCIdleState
= LL_TIM_OCIDLESTATE_HIGH
;
317 OCINIT
.OCPolarity
= (output
& TIMER_OUTPUT_INVERTED
) ? LL_TIM_OCPOLARITY_LOW
: LL_TIM_OCPOLARITY_HIGH
;
319 OCINIT
.CompareValue
= 0;
321 #ifdef USE_DSHOT_TELEMETRY
322 LL_TIM_IC_StructInit(&motor
->icInitStruct
);
323 motor
->icInitStruct
.ICPolarity
= LL_TIM_IC_POLARITY_BOTHEDGE
;
324 motor
->icInitStruct
.ICPrescaler
= LL_TIM_ICPSC_DIV1
;
325 motor
->icInitStruct
.ICFilter
= 2;
328 uint32_t channel
= 0;
329 switch (timerHardware
->channel
) {
330 case TIM_CHANNEL_1
: channel
= LL_TIM_CHANNEL_CH1
; break;
331 case TIM_CHANNEL_2
: channel
= LL_TIM_CHANNEL_CH2
; break;
332 case TIM_CHANNEL_3
: channel
= LL_TIM_CHANNEL_CH3
; break;
333 case TIM_CHANNEL_4
: channel
= LL_TIM_CHANNEL_CH4
; break;
335 motor
->llChannel
= channel
;
337 #ifdef USE_DSHOT_DMAR
339 motor
->timer
->dmaBurstRef
= dmaRef
;
340 #ifdef USE_DSHOT_TELEMETRY
341 motor
->dmaRef
= dmaRef
;
346 motor
->timerDmaSource
= timerDmaSource(timerHardware
->channel
);
347 motor
->timer
->timerDmaSources
&= ~motor
->timerDmaSource
;
350 if (!dmaIsConfigured
) {
351 xLL_EX_DMA_DisableResource(dmaRef
);
352 xLL_EX_DMA_DeInit(dmaRef
);
354 dmaEnable(dmaIdentifier
);
357 LL_DMA_StructInit(&DMAINIT
);
358 #ifdef USE_DSHOT_DMAR
360 motor
->timer
->dmaBurstBuffer
= &dshotBurstDmaBuffer
[timerIndex
][0];
362 #if defined(STM32H7) || defined(STM32G4)
363 DMAINIT
.PeriphRequest
= dmaChannel
;
365 DMAINIT
.Channel
= dmaChannel
;
367 DMAINIT
.MemoryOrM2MDstAddress
= (uint32_t)motor
->timer
->dmaBurstBuffer
;
369 DMAINIT
.FIFOThreshold
= LL_DMA_FIFOTHRESHOLD_FULL
;
371 DMAINIT
.PeriphOrM2MSrcAddress
= (uint32_t)&timerHardware
->tim
->DMAR
;
375 motor
->dmaBuffer
= &dshotDmaBuffer
[motorIndex
][0];
377 #if defined(STM32H7) || defined(STM32G4)
378 DMAINIT
.PeriphRequest
= dmaChannel
;
380 DMAINIT
.Channel
= dmaChannel
;
382 DMAINIT
.MemoryOrM2MDstAddress
= (uint32_t)motor
->dmaBuffer
;
384 DMAINIT
.FIFOThreshold
= LL_DMA_FIFOTHRESHOLD_1_4
;
386 DMAINIT
.PeriphOrM2MSrcAddress
= (uint32_t)timerChCCR(timerHardware
);
389 DMAINIT
.Direction
= LL_DMA_DIRECTION_MEMORY_TO_PERIPH
;
391 DMAINIT
.FIFOMode
= LL_DMA_FIFOMODE_ENABLE
;
392 DMAINIT
.MemBurst
= LL_DMA_MBURST_SINGLE
;
393 DMAINIT
.PeriphBurst
= LL_DMA_PBURST_SINGLE
;
395 DMAINIT
.NbData
= pwmProtocolType
== PWM_TYPE_PROSHOT1000
? PROSHOT_DMA_BUFFER_SIZE
: DSHOT_DMA_BUFFER_SIZE
;
396 DMAINIT
.PeriphOrM2MSrcIncMode
= LL_DMA_PERIPH_NOINCREMENT
;
397 DMAINIT
.MemoryOrM2MDstIncMode
= LL_DMA_MEMORY_INCREMENT
;
398 DMAINIT
.PeriphOrM2MSrcDataSize
= LL_DMA_PDATAALIGN_WORD
;
399 DMAINIT
.MemoryOrM2MDstDataSize
= LL_DMA_MDATAALIGN_WORD
;
400 DMAINIT
.Mode
= LL_DMA_MODE_NORMAL
;
401 DMAINIT
.Priority
= LL_DMA_PRIORITY_HIGH
;
403 if (!dmaIsConfigured
) {
404 xLL_EX_DMA_Init(dmaRef
, &DMAINIT
);
405 xLL_EX_DMA_EnableIT_TC(dmaRef
);
408 motor
->dmaRef
= dmaRef
;
410 #ifdef USE_DSHOT_TELEMETRY
411 motor
->dshotTelemetryDeadtimeUs
= DSHOT_TELEMETRY_DEADTIME_US
+ 1000000 *
412 ( 16 * MOTOR_BITLENGTH
) / getDshotHz(pwmProtocolType
);
413 motor
->timer
->outputPeriod
= (pwmProtocolType
== PWM_TYPE_PROSHOT1000
? (MOTOR_NIBBLE_LENGTH_PROSHOT
) : MOTOR_BITLENGTH
) - 1;
414 pwmDshotSetDirectionOutput(motor
);
416 pwmDshotSetDirectionOutput(motor
, &OCINIT
, &DMAINIT
);
419 #ifdef USE_DSHOT_DMAR
421 if (!dmaIsConfigured
) {
422 dmaSetHandler(dmaIdentifier
, motor_DMA_IRQHandler
, NVIC_PRIO_DSHOT_DMA
, motor
->index
);
427 dmaSetHandler(dmaIdentifier
, motor_DMA_IRQHandler
, NVIC_PRIO_DSHOT_DMA
, motor
->index
);
430 LL_TIM_OC_Init(timer
, channel
, &OCINIT
);
431 LL_TIM_OC_EnablePreload(timer
, channel
);
432 LL_TIM_OC_DisableFast(timer
, channel
);
434 LL_TIM_EnableCounter(timer
);
435 if (output
& TIMER_OUTPUT_N_CHANNEL
) {
436 LL_EX_TIM_CC_EnableNChannel(timer
, channel
);
438 LL_TIM_CC_EnableChannel(timer
, channel
);
441 if (configureTimer
) {
442 LL_TIM_EnableAllOutputs(timer
);
443 LL_TIM_EnableARRPreload(timer
);
444 LL_TIM_EnableCounter(timer
);
446 #ifdef USE_DSHOT_TELEMETRY
447 if (useDshotTelemetry
) {
448 // avoid high line during startup to prevent bootloader activation
449 *timerChCCR(timerHardware
) = 0xffff;
452 motor
->configured
= true;