Merge pull request #11297 from SteveCEvans/baro_state
[betaflight.git] / src / main / drivers / pwm_output_dshot_hal.c
blobe7fac23c05fadd977b708f53f5df210db5da2962
1 /*
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)
8 * any later version.
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/>.
21 #include <stdbool.h>
22 #include <stdint.h>
23 #include <math.h>
25 #include "platform.h"
27 #ifdef USE_DSHOT
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);
55 } else {
56 LL_TIM_CC_EnableChannel(dmaMotors[i].timerHardware->tim, dmaMotors[i].llChannel);
61 #endif
63 void pwmDshotSetDirectionOutput(
64 motorDmaOutput_t * const motor
65 #ifndef USE_DSHOT_TELEMETRY
66 , LL_TIM_OC_InitTypeDef* pOcInit, LL_DMA_InitTypeDef* pDmaInit
67 #endif
70 #ifdef USE_DSHOT_TELEMETRY
71 LL_TIM_OC_InitTypeDef* pOcInit = &motor->ocInitStruct;
72 LL_DMA_InitTypeDef* pDmaInit = &motor->dmaInitStruct;
73 #endif
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;
82 #endif
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;
106 if (!inputStampUs) {
107 inputStampUs = micros();
109 LL_TIM_EnableARRPreload(timer); // Only update the period once all channels are done
110 timer->ARR = 0xffffffff;
112 #ifdef STM32H7
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);
119 #endif
121 LL_TIM_IC_Init(timer, motor->llChannel, &motor->icInitStruct);
123 #ifdef STM32H7
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);
128 } else {
129 LL_GPIO_SetAFPin_8_15(IO_GPIO(motor->io), IO_Pin(motor->io), timerHardware->alternateFunction);
131 #endif
133 motor->dmaInitStruct.Direction = LL_DMA_DIRECTION_PERIPH_TO_MEMORY;
134 xLL_EX_DMA_Init(motor->dmaRef, pDmaInit);
136 #endif
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)) {
143 return;
146 for (int i = 0; i < dmaMotorTimerCount; i++) {
147 #ifdef USE_DSHOT_DMAR
148 if (useBurstDshot) {
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);
156 } else
157 #endif
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();
178 #endif
179 #ifdef USE_DSHOT_DMAR
180 if (useBurstDshot) {
181 xLL_EX_DMA_DisableResource(motor->timerHardware->dmaTimUPRef);
182 LL_TIM_DisableDMAReq_UPDATE(motor->timerHardware->tim);
183 } else
184 #endif
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();
198 #endif
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
209 #else
210 LL_TIM_OC_InitTypeDef ocInitStruct;
211 LL_DMA_InitTypeDef dmaInitStruct;
212 #define OCINIT ocInitStruct
213 #define DMAINIT dmaInitStruct
214 #endif
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;
225 #else
226 dmaRef = timerHardware->dmaRef;
227 dmaChannel = timerHardware->dmaChannel;
228 #endif
230 #ifdef USE_DSHOT_DMAR
231 if (useBurstDshot) {
232 dmaRef = timerHardware->dmaTimUPRef;
233 dmaChannel = timerHardware->dmaTimUPChannel;
235 #endif
237 if (dmaRef == NULL) {
238 return false;
241 dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(dmaRef);
243 bool dmaIsConfigured = false;
244 #ifdef USE_DSHOT_DMAR
245 if (useBurstDshot) {
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))) {
250 return false;
252 } else
253 #endif
255 if (!dmaAllocate(dmaIdentifier, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex))) {
256 return false;
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;
276 #ifdef STM32H7
277 if (output & TIMER_OUTPUT_INVERTED) {
278 IOHi(motorIO);
279 } else {
280 IOLo(motorIO);
282 #endif
284 #endif
285 motor->timerHardware = timerHardware;
287 motor->iocfg = IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, pupMode);
288 #ifdef STM32H7
289 motor->io = motorIO;
290 #endif
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;
314 } else {
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;
326 #endif
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
338 if (useBurstDshot) {
339 motor->timer->dmaBurstRef = dmaRef;
340 #ifdef USE_DSHOT_TELEMETRY
341 motor->dmaRef = dmaRef;
342 #endif
343 } else
344 #endif
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
359 if (useBurstDshot) {
360 motor->timer->dmaBurstBuffer = &dshotBurstDmaBuffer[timerIndex][0];
362 #if defined(STM32H7) || defined(STM32G4)
363 DMAINIT.PeriphRequest = dmaChannel;
364 #else
365 DMAINIT.Channel = dmaChannel;
366 #endif
367 DMAINIT.MemoryOrM2MDstAddress = (uint32_t)motor->timer->dmaBurstBuffer;
368 #ifndef STM32G4
369 DMAINIT.FIFOThreshold = LL_DMA_FIFOTHRESHOLD_FULL;
370 #endif
371 DMAINIT.PeriphOrM2MSrcAddress = (uint32_t)&timerHardware->tim->DMAR;
372 } else
373 #endif
375 motor->dmaBuffer = &dshotDmaBuffer[motorIndex][0];
377 #if defined(STM32H7) || defined(STM32G4)
378 DMAINIT.PeriphRequest = dmaChannel;
379 #else
380 DMAINIT.Channel = dmaChannel;
381 #endif
382 DMAINIT.MemoryOrM2MDstAddress = (uint32_t)motor->dmaBuffer;
383 #ifndef STM32G4
384 DMAINIT.FIFOThreshold = LL_DMA_FIFOTHRESHOLD_1_4;
385 #endif
386 DMAINIT.PeriphOrM2MSrcAddress = (uint32_t)timerChCCR(timerHardware);
389 DMAINIT.Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH;
390 #ifndef STM32G4
391 DMAINIT.FIFOMode = LL_DMA_FIFOMODE_ENABLE;
392 DMAINIT.MemBurst = LL_DMA_MBURST_SINGLE;
393 DMAINIT.PeriphBurst = LL_DMA_PBURST_SINGLE;
394 #endif
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);
415 #else
416 pwmDshotSetDirectionOutput(motor, &OCINIT, &DMAINIT);
417 #endif
419 #ifdef USE_DSHOT_DMAR
420 if (useBurstDshot) {
421 if (!dmaIsConfigured) {
422 dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
424 } else
425 #endif
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);
437 } else {
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;
451 #endif
452 motor->configured = true;
454 return true;
456 #endif