Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
esc_dshot.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2018 Alexandre Bustico <alexandre.bustico@enac.fr>
3  * Gautier Hattenberger <gautier.hattenberger@enac.fr>
4  *
5  * This file is part of paparazzi
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, see
19  * <http://www.gnu.org/licenses/>.
20  */
21 
31 #include <stdnoreturn.h>
32 #include <math.h>
33 #include <string.h>
34 #include "mcu_periph/sys_time_arch.h"
35 
36 /*
37 # _ __ _ _ _ _
38 # | | / _| (_) (_) | | (_)
39 # __| | ___ | |_ _ _ __ _ | |_ _ ___ _ __
40 # / _` | / _ \ | _| | | | '_ \ | | | __| | | / _ \ | '_ \
41 # | (_| | | __/ | | | | | | | | | | \ |_ | | | (_) | | | | |
42 # \__,_| \___| |_| |_| |_| |_| |_| \__| |_| \___/ |_| |_|
43 */
44 
45 
49 #ifndef DSHOT_TELEMETRY_BAUD
50 #define DSHOT_TELEMETRY_BAUD 115200U
51 #endif
52 
53 #ifndef DSHOT_BIDIR_EXTENTED_TELEMETRY
54 #define DSHOT_BIDIR_EXTENTED_TELEMETRY FALSE
55 #endif
56 
59 #ifndef DSHOT_TELEMETRY_TIMEOUT_MS
60 #define DSHOT_TELEMETRY_TIMEOUT_MS 3
61 #endif
62 
63 
64 #ifdef STM32H7XX
65 // each H7 timer have the same max clock speed
66 #define PWM_FREQ (STM32_TIMCLK1 / 1000U) // the timer will beat @240Mhz on STM32H7
67 #else
68 // some F4 and F7 timers are limited to / 2
69 // others are limited to STM32_SYSCLK
70 // so we take the max frequency that all timers can run
71 #define PWM_FREQ (STM32_SYSCLK / 2000U) // the timer will beat @84Mhz on STM32F4
72 #endif
73 
74 
81 #define TICKS_PER_PERIOD 1000
82 
83 // ESCs are quite sensitive to the DSHOT duty cycle.
84 // 333 should work most of the time, but some ESC need 373
85 #ifndef DSHOT_BIT0_DUTY_RATIO
86 #define DSHOT_BIT0_DUTY_RATIO 373U
87 #endif
88 
89 #if DSHOT_SPEED != 0 // statically defined
90 # define DSHOT_FREQ (DSHOT_SPEED*1000)
91 # define DSHOT_BIT0_DUTY (DSHOT_PWM_PERIOD * DSHOT_BIT0_DUTY_RATIO / 1000U)
92 # define DSHOT_BIT1_DUTY (DSHOT_BIT0_DUTY*2)
93 #else // dynamically defined
94 # define DSHOT_FREQ (driver->config->speed_khz * 1000U)
95 # define DSHOT_BIT0_DUTY (driver->bit0Duty)
96 # define DSHOT_BIT1_DUTY (driver->bit1Duty)
97 #endif
98 #define TICK_FREQ (PWM_FREQ * TICKS_PER_PERIOD)
99 #define DSHOT_PWM_PERIOD (TICK_FREQ/DSHOT_FREQ)
100 
101 
102 #define DCR_DBL ((DSHOT_CHANNELS-1) << 8) // DSHOT_CHANNELS transfert(s)
103 // first register to get is CCR1
104 #define DCR_DBA(pwmd) (((uint32_t *) (&pwmd->tim->CCR) - ((uint32_t *) pwmd->tim)))
105 
106 #define DSHOT_MAX_VALUE ((1U<<11U)-1U) // 11 bits used to send command, so maximum value is 2047
107 
108 #define ARRAY_LEN(a) (sizeof(a)/sizeof(a[0]))
109 #define Min(x,y) (x < y ? x : y)
110 
111 
112 /*
113 # _ __ _ _ _ _ _ __
114 # | '_ \ | | | | | | | | | '_ \
115 # | |_) | _ __ ___ | |_ ___ | |_ | |_| | | |_) | ___
116 # | .__/ | '__| / _ \ | __| / _ \ | __| \__, | | .__/ / _ \
117 # | | | | | (_) | \ |_ | (_) | \ |_ __/ | | | | __/
118 # |_| |_| \___/ \__| \___/ \__| |___/ |_| \___|
119 */
120 static DshotPacket makeDshotPacket(const uint16_t throttle, const bool tlmRequest);
121 static void setCrc4(DshotPacket *dp);
122 static inline void setDshotPacketThrottle(DshotPacket * const dp, const uint16_t throttle);
123 static inline void setDshotPacketTlm(DshotPacket * const dp, const bool tlmRequest);
124 static void buildDshotDmaBuffer(DSHOTDriver *driver);
125 //static void buildDshotDmaBuffer(DshotPackets * const dsp, DshotDmaBuffer * const dma, const size_t timerWidth);
126 static inline uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed);
127 static uint8_t calculateCrc8(const uint8_t *Buf, const uint8_t BufLen);
128 static noreturn void dshotTlmRec (void *arg);
129 static size_t getTimerWidth(const PWMDriver *pwmp);
130 #if DSHOT_BIDIR
131 static void processBidirErpm(DSHOTDriver *driver);
132 static void dshotRestart(DSHOTDriver *driver);
133 #if DSHOT_BIDIR_EXTENTED_TELEMETRY
134 static void updateTelemetryFromBidirEdt(const DshotErps *erps, DshotTelemetry *tlm);
135 #endif
136 #endif
137 
138 /*
139 # _ __ _
140 # | '_ \ (_)
141 # __ _ | |_) | _
142 # / _` | | .__/ | |
143 # | (_| | | | | |
144 # \__,_| |_| |_|
145 */
146 
155 {
156  chDbgAssert(config->dma_buf != NULL, ".dma_buf must reference valid DshotDmaBuffer object");
157 
158 #if DSHOT_BIDIR
159  dshotRpmCaptureStart(&driver->rpm_capture, &config->dma_capt_cfg, config->pwmp->tim);
160 #endif
161 
162  memset((void *) config->dma_buf, 0, sizeof(*(config->dma_buf)));
163  const size_t timerWidthInBytes = getTimerWidth(config->pwmp);
164 
165  static const SerialConfig tlmcfg = {
166  .speed = DSHOT_TELEMETRY_BAUD,
167  .cr1 = 0, // pas de parité
168  .cr2 = USART_CR2_STOP1_BITS, // 1 bit de stop
169  .cr3 = 0 // pas de controle de flux hardware (CTS, RTS)
170  };
171  // when dshot is bidir, the polarity is inverted
172  static const uint32_t pwmPolarity = DSHOT_BIDIR ? PWM_OUTPUT_ACTIVE_LOW : PWM_OUTPUT_ACTIVE_HIGH;
173 
174  driver->config = config;
175  // use pburst, mburst only if buffer size satisfy aligmnent requirement
176  driver->dma_conf = (DMAConfig) {
177  .stream = config->dma_stream,
178 #if STM32_DMA_SUPPORTS_DMAMUX
179  .dmamux = config->dmamux,
180 #else
181  .channel = config->dma_channel,
182 #endif
183  .dma_priority = 3,
184  .irq_priority = CORTEX_MAX_KERNEL_PRIORITY + 1,
185  .direction = DMA_DIR_M2P,
186  .psize = timerWidthInBytes,
187  .msize = timerWidthInBytes,
188 #if __DCACHE_PRESENT
189  .dcache_memory_in_use = config->dcache_memory_in_use,
190 #endif
191  .inc_peripheral_addr = false,
192  .inc_memory_addr = true,
193  .op_mode = DMA_ONESHOT,
194  .error_cb = NULL,
195  .end_cb = NULL,
196  .pburst = 0,
197  .mburst = 0,
198  .fifo = 4
199  };
200 
201  driver->pwm_conf = (PWMConfig) {
202  .frequency = TICK_FREQ,
203  .period = TICKS_PER_PERIOD,
204  .callback = NULL,
205  .channels = {
206  {.mode = pwmPolarity,
207  .callback = NULL},
208  {.mode = DSHOT_CHANNELS > 1 ? pwmPolarity : PWM_OUTPUT_DISABLED,
209  .callback = NULL},
210  {.mode = DSHOT_CHANNELS > 2 ? pwmPolarity : PWM_OUTPUT_DISABLED,
211  .callback = NULL},
212  {.mode = DSHOT_CHANNELS > 3 ? pwmPolarity : PWM_OUTPUT_DISABLED,
213  .callback = NULL},
214  },
215  .cr2 = STM32_TIM_CR2_CCDS,
216  .dier = STM32_TIM_DIER_UDE
217  };
218 
219  driver->crc_errors = 0;
220  driver->tlm_frame_nb = 0;
221 #if DSHOT_SPEED == 0
222  driver->bit0Duty = (DSHOT_PWM_PERIOD * DSHOT_BIT0_DUTY_RATIO / 1000U);
223  driver->bit1Duty = (driver->bit0Duty*2U) ;
224 #endif
225 
226 
227 
228  dmaObjectInit(&driver->dmap);
229  chMBObjectInit(&driver->mb, driver->_mbBuf, ARRAY_LEN(driver->_mbBuf));
230 
231  const bool dmaOk = dmaStart(&driver->dmap, &driver->dma_conf);
232  chDbgAssert(dmaOk == true, "dshot dma start error");
233 
234  if (driver->config->tlm_sd) {
235  sdStart(driver->config->tlm_sd, &tlmcfg);
236  chThdCreateStatic(driver->waDshotTlmRec, sizeof(driver->waDshotTlmRec), NORMALPRIO,
237  dshotTlmRec, driver);
238  }
239 
240  pwmStart(driver->config->pwmp, &driver->pwm_conf);
241  driver->config->pwmp->tim->DCR = DCR_DBL | DCR_DBA(driver->config->pwmp); // enable bloc register DMA transaction
242  pwmChangePeriod(driver->config->pwmp, DSHOT_PWM_PERIOD);
243 
244  for (size_t j=0; j<DSHOT_CHANNELS; j++) {
245  pwmEnableChannel(driver->config->pwmp, j, 0);
246  driver->dshotMotors.dp[j] = makeDshotPacket(0,0);
247  chMtxObjectInit(&driver->dshotMotors.tlmMtx[j]);
248  }
249  driver->dshotMotors.onGoingQry = false;
250  driver->dshotMotors.currentTlmQry = 0U;
251 }
252 
260 void dshotStop(DSHOTDriver *driver)
261 {
262  pwmStop(driver->config->pwmp);
263  dmaStopTransfert(&driver->dmap);
264  dmaStop(&driver->dmap);
265 }
266 
267 #if DSHOT_BIDIR
275 static void dshotRestart(DSHOTDriver *driver)
276 {
277  const bool dmaOk = dmaStart(&driver->dmap, &driver->dma_conf);
278  chDbgAssert(dmaOk == true, "dshot dma start error");
279 
280  pwmStart(driver->config->pwmp, &driver->pwm_conf);
281  driver->config->pwmp->tim->DCR = DCR_DBL | DCR_DBA(driver->config->pwmp); // enable bloc register DMA transaction
282  pwmChangePeriod(driver->config->pwmp, DSHOT_PWM_PERIOD);
283 
284  for (size_t j=0; j<DSHOT_CHANNELS; j++) {
285  pwmEnableChannel(driver->config->pwmp, j, 0);
286  driver->dshotMotors.dp[j] = makeDshotPacket(0,0);
287  }
288 }
289 #endif
290 
301 void dshotSetThrottle(DSHOTDriver *driver, const uint8_t index,
302  const uint16_t throttle)
303 {
304  if (throttle > 0 && throttle <= DSHOT_CMD_MAX) {
305  chDbgAssert(false, "dshotSetThrottle throttle error");
306  return; // special commands (except MOTOR_STOP) can't be applied from this function
307  } else {
308  // send normal throttle
309  if (index == DSHOT_ALL_MOTORS) {
310  for (uint8_t _index = 0; _index < DSHOT_CHANNELS; _index++) {
311  setDshotPacketThrottle(&driver->dshotMotors.dp[_index], Min(throttle, DSHOT_MAX_VALUE));
312  }
313  } else if ((index - DSHOT_CHANNEL_FIRST_INDEX) < DSHOT_CHANNELS) {
315  Min(throttle, DSHOT_MAX_VALUE));
316  } else {
317  chDbgAssert(false, "dshotSetThrottle index error");
318  }
319  }
320 }
321 
330 void dshotSendSpecialCommand(DSHOTDriver *driver, const uint8_t index,
331  const dshot_special_commands_t specmd)
332 {
333  if (specmd > DSHOT_CMD_MAX) {
334  return; // Don't apply special commands from this function
335  }
336 
337  // some dangerous special commands need to be repeated 6 times
338  // to avoid catastrophic failure
339  uint8_t repeat;
340  switch (specmd) {
351  repeat = 10;
352  break;
353  default:
354  repeat = 1;
355  }
356 
357  while (repeat--) {
358  systime_t now = chVTGetSystemTimeX();
359  if (index < DSHOT_CHANNELS) {
360  setDshotPacketThrottle(&driver->dshotMotors.dp[index], specmd);
361  setDshotPacketTlm(&driver->dshotMotors.dp[index], driver->config->tlm_sd != NULL);
362  } else if (index == DSHOT_ALL_MOTORS) {
363  for (uint8_t _index = 0; _index < DSHOT_CHANNELS; _index++) {
364  setDshotPacketThrottle(&driver->dshotMotors.dp[_index], specmd);
365  setDshotPacketTlm(&driver->dshotMotors.dp[_index], driver->config->tlm_sd != NULL);
366  }
367  } else {
368  chDbgAssert(false, "dshotSetThrottle index error");
369  }
370  dshotSendFrame(driver);
371  if (repeat)
372  chThdSleepUntilWindowed(now, now + TIME_US2I(500));
373  }
374 }
375 
385 void dshotSendThrottles(DSHOTDriver *driver, const uint16_t throttles[DSHOT_CHANNELS])
386 {
387  for (uint8_t index = 0; index < DSHOT_CHANNELS; index++) {
388  setDshotPacketThrottle(&driver->dshotMotors.dp[index], throttles[index]);
389  }
390 
391  dshotSendFrame(driver);
392 }
393 
394 
395 
396 
405 {
406  if (driver->dmap.state == DMA_READY) {
407 #if DSHOT_BIDIR
408  const tprio_t currentPrio = chThdSetPriority(HIGHPRIO);
409 #endif
410  if ((driver->config->tlm_sd != NULL) &&
411  (driver->dshotMotors.onGoingQry == false)) {
412  driver->dshotMotors.onGoingQry = true;
413  const msg_t index = (driver->dshotMotors.currentTlmQry + 1U) % DSHOT_CHANNELS;
414  driver->dshotMotors.currentTlmQry = (uint8_t) index;
415  setDshotPacketTlm(&driver->dshotMotors.dp[index], true);
416  chMBPostTimeout(&driver->mb, index, TIME_IMMEDIATE);
417  }
418 
419  //buildDshotDmaBuffer(&driver->dshotMotors, driver->config->dma_buf, getTimerWidth(driver->config->pwmp));
420  buildDshotDmaBuffer(driver);
421  dmaTransfert(&driver->dmap,
422  &driver->config->pwmp->tim->DMAR,
424 
425 #if DSHOT_BIDIR
426  dshotStop(driver);
427  dshotRpmCatchErps(&driver->rpm_capture);
428  processBidirErpm(driver);
429  dshotRestart(driver);
430  chThdSetPriority(currentPrio);
431 #endif
432  }
433 }
434 
443 {
444  return driver->crc_errors;
445 }
446 
455 {
456  return driver->tlm_frame_nb;
457 }
458 
459 
469 {
470  index -= DSHOT_CHANNEL_FIRST_INDEX;
471  chDbgAssert(index < DSHOT_CHANNELS, "dshot index error");
472  chMtxLock(&driver->dshotMotors.tlmMtx[index]);
473  const DshotTelemetry tlm = driver->dshotMotors.dt[index];
474  chMtxUnlock(&driver->dshotMotors.tlmMtx[index]);
475  return tlm;
476 }
477 
478 /*
479 
480  [2] 0010 mmmm mmmm - Temperature frame in degree Celsius, just like Blheli_32 and KISS [0, 1, ..., 255]
481  [4] 0100 mmmm mmmm - Voltage frame with a step size of 0,25V [0, 0.25 ..., 63,75]
482  [6] 0110 mmmm mmmm - Current frame with a step size of 1A [0, 1, ..., 255]
483  [8] 1000 mmmm mmmm - Debug frame 1 not associated with any specific value, can be used to debug ESC firmware
484  [10] 1010 mmmm mmmm - Debug frame 2 not associated with any specific value, can be used to debug ESC firmware
485  [12] 1100 mmmm mmmm - Stress level frame [0, 1, ..., 255] (since v2.0.0)
486  [14] 1110 mmmm mmmm - Status frame: Bit[7] = alert event, Bit[6] = warning event, Bit[5] = error event, Bit[3-1] - Max. stress level [0-15] (since v2.0.0)
487 
488  */
489 #if DSHOT_BIDIR && DSHOT_BIDIR_EXTENTED_TELEMETRY
490 static void updateTelemetryFromBidirEdt(const DshotErps *erps, DshotTelemetry *tlm)
491 {
492  switch(DshotErpsEdtType(erps)) {
493  case EDT_TEMP:
494  tlm->frame.temp = DshotErpsEdtTempCentigrade(erps); break;
495 
496  case EDT_VOLT:
497  tlm->frame.voltage = DshotErpsEdtCentiVolts(erps); break;
498 
499  case EDT_CURRENT:
500  tlm->frame.current = DshotErpsEdtCurrentAmp(erps) * 100U; break;
501 
502  case EDT_STRESS:
503  tlm->stress = DshotErpsEdtStress(erps); break;
504 
505  case EDT_STATUS:
506  tlm->status = DshotErpsEdtStatus(erps);break;
507 
508  default: {};
509  }
510  tlm->ts = chVTGetSystemTimeX();
511 }
512 #endif
513 
514 #if DSHOT_BIDIR
525 uint32_t dshotGetEperiod(DSHOTDriver *driver, const uint32_t index)
526 {
527  chDbgAssert(index < DSHOT_CHANNELS, "index check failed");
528  DshotErpsSetFromFrame(&driver->erps, driver->rpms_frame[index]);
529  if (DshotErpsCheckCrc4(&driver->erps)) {
530 #if DSHOT_BIDIR_EXTENTED_TELEMETRY
531  if (DshotErpsIsEdt(&driver->erps)) {
532  if (driver->config->tlm_sd == NULL) {
533  DshotTelemetry *tlm = &driver->dshotMotors.dt[index];
534  updateTelemetryFromBidirEdt(&driver->erps, tlm);
535  }
536  return DSHOT_BIDIR_TLM_EDT;
537  }
538 #endif
539  return DshotErpsGetEperiod(&driver->erps);
540  } else {
541  return DSHOT_BIDIR_ERR_CRC;
542  }
543 }
554 uint32_t dshotGetRpm(DSHOTDriver *driver, uint32_t index)
555 {
556  index -= DSHOT_CHANNEL_FIRST_INDEX;
557  chDbgAssert(index < DSHOT_CHANNELS, "index check failed");
558  DshotErpsSetFromFrame(&driver->erps, driver->rpms_frame[index]);
559  if (DshotErpsCheckCrc4(&driver->erps)) {
560 #if DSHOT_BIDIR_EXTENTED_TELEMETRY
561  if (DshotErpsIsEdt(&driver->erps)) {
562  if (driver->config->tlm_sd == NULL) {
563  DshotTelemetry *tlm = &driver->dshotMotors.dt[index];
564  updateTelemetryFromBidirEdt(&driver->erps, tlm);
565  }
566  return DSHOT_BIDIR_TLM_EDT;
567  }
568 #endif
569  return DshotErpsGetRpm(&driver->erps);
570  } else {
571  return DSHOT_BIDIR_ERR_CRC;
572  }
573 }
574 #endif
575 
576 
577 
578 /*
579 # _ __ _ _
580 # | '_ \ (_) | |
581 # | |_) | _ __ _ __ __ __ _ | |_ ___
582 # | .__/ | '__| | | \ \ / / / _` | | __| / _ \
583 # | | | | | | \ V / | (_| | \ |_ | __/
584 # |_| |_| |_| \_/ \__,_| \__| \___|
585 */
586 
587 static void setCrc4(DshotPacket *dp)
588 {
589  // compute checksum
590  dp->crc = 0;
591  uint16_t csum = (dp->throttle << 1) | dp->telemetryRequest;
592  for (int i = 0; i < 3; i++) {
593  dp->crc ^= csum; // xor data by nibbles
594  csum >>= 4;
595  }
596 #if DSHOT_BIDIR
597  dp->crc = ~(dp->crc); // crc is inverted when dshot bidir protocol is choosed
598 #endif
599 }
600 
601 static DshotPacket makeDshotPacket(const uint16_t _throttle, const bool tlmRequest)
602 {
603  DshotPacket dp = {.throttle = _throttle,
604  .telemetryRequest = (tlmRequest ? 1 : 0),
605  .crc = 0
606  };
607 
608  setCrc4(&dp);
609  return dp;
610 }
611 
612 static inline void setDshotPacketThrottle(DshotPacket *const dp, const uint16_t throttle)
613 {
614  dp->throttle = throttle;
615  dp->telemetryRequest = 0;
616 }
617 
618 static inline void setDshotPacketTlm(DshotPacket *const dp, const bool tlmRequest)
619 {
620  dp->telemetryRequest = tlmRequest ? 1 : 0;
621 }
622 
623 static void buildDshotDmaBuffer(DSHOTDriver *driver)
624 {
625  DshotPackets *const dsp = &driver->dshotMotors;
626  DshotDmaBuffer *const dma = driver->config->dma_buf;
627  const size_t timerWidth = getTimerWidth(driver->config->pwmp);
628 
629  for (size_t chanIdx = 0; chanIdx < DSHOT_CHANNELS; chanIdx++) {
630  // compute checksum
631  DshotPacket * const dp = &dsp->dp[chanIdx];
632  setCrc4(dp);
633  // generate pwm frame
634  for (size_t bitIdx = 0; bitIdx < DSHOT_BIT_WIDTHS; bitIdx++) {
635  const uint16_t value = dp->rawFrame &
636  (1 << ((DSHOT_BIT_WIDTHS - 1) - bitIdx)) ?
638  if (timerWidth == 2) {
639  dma->widths16[bitIdx+DSHOT_PRE_FRAME_SILENT_SYNC_BITS][chanIdx] = value;
640  } else {
641 #if DSHOT_AT_LEAST_ONE_32B_TIMER
642  dma->widths32[bitIdx+DSHOT_PRE_FRAME_SILENT_SYNC_BITS][chanIdx] = value;
643 #else
644  chSysHalt("use of 32 bit timer implies to define DSHOT_AT_LEAST_ONE_32B_TIMER to TRUE");
645 #endif
646  }
647  }
648  // the bits for silence sync (pre and post) in case of continous sending are zeroed once at init
649  }
650 }
651 
652 
653 static inline uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed)
654 {
655  uint8_t crc_u = crc;
656  crc_u ^= crc_seed;
657 
658  for (int i = 0; i < 8; i++) {
659  crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1);
660  }
661 
662  return (crc_u);
663 }
664 
665 static uint8_t calculateCrc8(const uint8_t *Buf, const uint8_t BufLen)
666 {
667  uint8_t crc = 0;
668  for (int i = 0; i < BufLen; i++) {
669  crc = updateCrc8(Buf[i], crc);
670  }
671 
672  return crc;
673 }
674 
675 
676 #if DSHOT_BIDIR
677 static void processBidirErpm(DSHOTDriver *driver)
678 {
679  for (size_t idx = 0; idx < DSHOT_CHANNELS; idx++) {
680  driver->rpms_frame[idx] = dshotRpmGetFrame(&driver->rpm_capture, idx);
681  }
682 }
683 #endif
684 
685 
686 __attribute__((const))
687 static size_t getTimerWidth(const PWMDriver *pwmp)
688 {
689  (void) pwmp;
690 
691  return (0
693  || (pwmp == &PWMD2)
694 #endif
695 #if STM32_PWM_USE_TIM5
696  || (pwmp == &PWMD5)
697 #endif
698  ) ? 4 : 2;
699 }
700 
701 
702 /*
703 # _ _ _
704 # | | | | | |
705 # | |_ | |__ _ __ ___ __ _ __| | ___
706 # | __| | '_ \ | '__| / _ \ / _` | / _` | / __|
707 # \ |_ | | | | | | | __/ | (_| | | (_| | \__ \
708 # \__| |_| |_| |_| \___| \__,_| \__,_| |___/
709 */
710 
711 
712 static noreturn void dshotTlmRec(void *arg)
713 {
714  DSHOTDriver *driver = (DSHOTDriver *) arg;
715  DshotTelemetry tlm;
716 
717  msg_t escIdx = 0;
718 
719  chRegSetThreadName("dshotTlmRec");
720  while (true) {
721  chMBFetchTimeout(&driver->mb, &escIdx, TIME_INFINITE);
722  const uint32_t idx = escIdx;
723  const bool success =
724  (sdReadTimeout(driver->config->tlm_sd, tlm.frame.rawData, sizeof(DshotTelemetryFrame),
725  TIME_MS2I(DSHOT_TELEMETRY_TIMEOUT_MS)) == sizeof(DshotTelemetryFrame));
726  if (!success ||
727  (calculateCrc8(tlm.frame.rawData, sizeof(tlm.frame.rawData)) != tlm.frame.crc8)) {
728  // empty buffer to resync
729  while (sdGetTimeout(driver->config->tlm_sd, TIME_IMMEDIATE) >= 0) {};
730  memset(tlm.frame.rawData, 0U, sizeof(DshotTelemetry));
731  // count errors
732  if (success)
733  driver->crc_errors++;
734  } else {
735  // big-endian to little-endian conversion
736  tlm.frame.voltage = __builtin_bswap16(tlm.frame.voltage);
737  tlm.frame.current = __builtin_bswap16(tlm.frame.current);
738  tlm.frame.consumption = __builtin_bswap16(tlm.frame.consumption);
739  tlm.frame.rpm = __builtin_bswap16(tlm.frame.rpm);
740  tlm.ts = chVTGetSystemTimeX();
741  driver->tlm_frame_nb++;
742  }
743 
744  chMtxLock(&driver->dshotMotors.tlmMtx[idx]);
745  driver->dshotMotors.dt[idx] = tlm;
746  chMtxUnlock(&driver->dshotMotors.tlmMtx[idx]);
747  driver->dshotMotors.onGoingQry = false;
748  }
749 }
750 
const DshotErps * DshotErpsSetFromFrame(DshotErps *derpsp, uint32_t frame)
initialise from GCR encoded frame
Definition: dshot_erps.c:44
uint32_t DshotErpsGetRpm(const DshotErps *derpsp)
calculate and return rpm
Definition: dshot_erps.c:87
uint32_t DshotErpsGetEperiod(const DshotErps *derpsp)
return eperiod from mantisse and exponent
Definition: dshot_erps.c:72
bool DshotErpsCheckCrc4(const DshotErps *derpsp)
check packed validity
Definition: dshot_erps.c:99
static uint16_t DshotErpsEdtStatus(const DshotErps *derpsp)
return status value
Definition: dshot_erps.h:144
@ EDT_CURRENT
Definition: dshot_erps.h:16
@ EDT_STATUS
Definition: dshot_erps.h:18
@ EDT_STRESS
Definition: dshot_erps.h:17
@ EDT_VOLT
Definition: dshot_erps.h:16
@ EDT_TEMP
Definition: dshot_erps.h:16
static uint8_t DshotErpsEdtTempCentigrade(const DshotErps *derpsp)
return temperature for a temperature telemetry frame
Definition: dshot_erps.h:106
static uint16_t DshotErpsEdtStress(const DshotErps *derpsp)
return stress value
Definition: dshot_erps.h:134
static EdtType DshotErpsEdtType(const DshotErps *derpsp)
return type of a telemetry frame
Definition: dshot_erps.h:97
static uint16_t DshotErpsEdtCentiVolts(const DshotErps *derpsp)
return voltage for a voltage telemetry frame
Definition: dshot_erps.h:115
static uint16_t DshotErpsEdtCurrentAmp(const DshotErps *derpsp)
return current intensity for a current telemetry frame
Definition: dshot_erps.h:124
static bool DshotErpsIsEdt(const DshotErps *derpsp)
return true if current frame is a telemetry frame
Definition: dshot_erps.h:84
ERPS complete frame, raw and decoded.
Definition: dshot_erps.h:58
void dshotRpmCaptureStart(DshotRpmCapture *drcp, const DshotRpmCaptureConfig *cfg, stm32_tim_t *timer)
Configures and activates the DSHOT ERPS CAPTURE driver.
void dshotRpmCatchErps(DshotRpmCapture *drcp)
capture the DSHOT ERPS frame(s) : one frame for each DSHOT_CHANNELS
static uint32_t dshotRpmGetFrame(const DshotRpmCapture *drcp, uint8_t index)
return last collected erps frame
static void buildDshotDmaBuffer(DSHOTDriver *driver)
Definition: esc_dshot.c:623
void dshotStart(DSHOTDriver *driver, const DSHOTConfig *config)
Configures and activates the DSHOT peripheral.
Definition: esc_dshot.c:154
void dshotSetThrottle(DSHOTDriver *driver, const uint8_t index, const uint16_t throttle)
prepare throttle order for specified ESC
Definition: esc_dshot.c:301
static size_t getTimerWidth(const PWMDriver *pwmp)
Definition: esc_dshot.c:687
#define TICKS_PER_PERIOD
Ticks per period that let use any timer: does not care if linked to PCLK1 or PCLK2 tick_per_period wi...
Definition: esc_dshot.c:81
#define Min(x, y)
Definition: esc_dshot.c:109
uint32_t dshotGetTelemetryFrameCount(const DSHOTDriver *driver)
return number of telemetry succesfull frame since dshotStart
Definition: esc_dshot.c:454
#define TICK_FREQ
Definition: esc_dshot.c:98
static uint8_t calculateCrc8(const uint8_t *Buf, const uint8_t BufLen)
Definition: esc_dshot.c:665
#define DSHOT_MAX_VALUE
Definition: esc_dshot.c:106
DshotTelemetry dshotGetTelemetry(DSHOTDriver *driver, uint32_t index)
return last received telemetry data
Definition: esc_dshot.c:468
static void setDshotPacketThrottle(DshotPacket *const dp, const uint16_t throttle)
Definition: esc_dshot.c:612
void dshotSendThrottles(DSHOTDriver *driver, const uint16_t throttles[DSHOT_CHANNELS])
send throttle packed order to all of the ESCs
Definition: esc_dshot.c:385
static void setCrc4(DshotPacket *dp)
Definition: esc_dshot.c:587
#define DSHOT_BIT0_DUTY
Definition: esc_dshot.c:95
uint32_t dshotGetCrcErrorCount(const DSHOTDriver *driver)
return number of telemetry crc error since dshotStart
Definition: esc_dshot.c:442
#define DCR_DBA(pwmd)
Definition: esc_dshot.c:104
void dshotStop(DSHOTDriver *driver)
stop the DSHOT driver and free the related resources : pwm driver and dma driver.
Definition: esc_dshot.c:260
#define DSHOT_TELEMETRY_TIMEOUT_MS
Telemetry timeout in ms.
Definition: esc_dshot.c:60
#define DSHOT_BIT0_DUTY_RATIO
Definition: esc_dshot.c:86
void dshotSendFrame(DSHOTDriver *driver)
send throttle order
Definition: esc_dshot.c:404
static noreturn void dshotTlmRec(void *arg)
Definition: esc_dshot.c:712
#define ARRAY_LEN(a)
Definition: esc_dshot.c:108
static void setDshotPacketTlm(DshotPacket *const dp, const bool tlmRequest)
Definition: esc_dshot.c:618
#define DSHOT_TELEMETRY_BAUD
Baudrate of the serial link used for telemetry data Can depend on the ESC, but only 115k have been us...
Definition: esc_dshot.c:50
#define DSHOT_PWM_PERIOD
Definition: esc_dshot.c:99
#define DSHOT_BIT1_DUTY
Definition: esc_dshot.c:96
static DshotPacket makeDshotPacket(const uint16_t throttle, const bool tlmRequest)
Definition: esc_dshot.c:601
#define DCR_DBL
Definition: esc_dshot.c:102
void dshotSendSpecialCommand(DSHOTDriver *driver, const uint8_t index, const dshot_special_commands_t specmd)
send special order to one of the ESC (BHELIX, KISS, ...)
Definition: esc_dshot.c:330
static uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed)
Definition: esc_dshot.c:653
DSHOT driver based on ChibiOS.
mutex_t tlmMtx[DSHOT_CHANNELS]
Definition: esc_dshot.h:278
#define DSHOT_PRE_FRAME_SILENT_SYNC_BITS
Definition: esc_dshot.h:49
SerialDriver * tlm_sd
if non null : dshot telemetry serial driver
Definition: esc_dshot.h:205
#define DSHOT_BIT_WIDTHS
DMA buffer size and number of channels.
Definition: esc_dshot.h:48
volatile bool onGoingQry
Definition: esc_dshot.h:279
PWMDriver * pwmp
PWM driver that feed up to 4 dshot lines.
Definition: esc_dshot.h:200
#define DSHOT_DMA_BUFFER_SIZE
Definition: esc_dshot.h:51
uint16_t widths16[DSHOT_DMA_BUFFER_SIZE][DSHOT_CHANNELS]
Definition: esc_dshot.h:170
#define DSHOT_CHANNEL_FIRST_INDEX
Definition: esc_dshot.h:43
DshotTelemetry dt[DSHOT_CHANNELS]
Definition: esc_dshot.h:277
uint8_t status
Definition: esc_dshot.h:161
uint8_t stress
Definition: esc_dshot.h:160
DshotTelemetryFrame frame
Definition: esc_dshot.h:159
systime_t ts
Definition: esc_dshot.h:162
uint16_t rawFrame
Definition: esc_dshot.h:271
#define DSHOT_ALL_MOTORS
special value for index : send order to all channels
Definition: esc_dshot.h:74
#define DSHOT_BIDIR_TLM_EDT
Definition: esc_dshot.h:66
DshotDmaBuffer * dma_buf
dshot dma buffer, should be defined in a non Dcached region
Definition: esc_dshot.h:210
dshot_special_commands_t
DSHOT special commands (0-47) for KISS and BLHELI ESC.
Definition: esc_dshot.h:102
@ DSHOT_CMD_SILENT_MODE_ON_OFF
Definition: esc_dshot.h:129
@ DSHOT_CMD_SAVE_SETTINGS
Definition: esc_dshot.h:115
@ DSHOT_CMD_AUDIO_STREAM_MODE_ON_OFF
Definition: esc_dshot.h:128
@ DSHOT_CMD_SETTINGS_REQUEST
Definition: esc_dshot.h:114
@ DSHOT_CMD_BIDIR_EDT_MODE_OFF
Definition: esc_dshot.h:117
@ DSHOT_CMD_SPIN_DIRECTION_1
Definition: esc_dshot.h:110
@ DSHOT_CMD_SPIN_DIRECTION_2
Definition: esc_dshot.h:111
@ DSHOT_CMD_3D_MODE_OFF
Definition: esc_dshot.h:112
@ DSHOT_CMD_MAX
Definition: esc_dshot.h:130
@ DSHOT_CMD_3D_MODE_ON
Definition: esc_dshot.h:113
@ DSHOT_CMD_BIDIR_EDT_MODE_ON
Definition: esc_dshot.h:116
#define DSHOT_BIDIR_ERR_CRC
special values returned by dshotGetRpm function
Definition: esc_dshot.h:65
uint8_t currentTlmQry
Definition: esc_dshot.h:280
DshotPacket dp[DSHOT_CHANNELS]
Definition: esc_dshot.h:276
DSHOT Driver configuration structure.
Definition: esc_dshot.h:182
telemetry with timestamp
Definition: esc_dshot.h:158
telemetry packed as sent by some KISS ESC
Definition: esc_dshot.h:139
#define DSHOT_CHANNELS
#define DSHOT_BIDIR
void dmaObjectInit(DMADriver *dmap)
Definition: hal_stm32_dma.c:69
void dmaStop(DMADriver *dmap)
Deactivates the DMA peripheral.
void dmaStopTransfert(DMADriver *dmap)
Stops an ongoing transaction.
bool dmaStart(DMADriver *dmap, const DMAConfig *cfg)
Configures and activates the DMA peripheral.
@ DMA_ONESHOT
One transert then stop
@ DMA_DIR_M2P
MEMORY to PERIPHERAL
uint32_t stream
stream associated with transaction
static msg_t dmaTransfert(DMADriver *dmap, volatile void *periphp, void *mem0p, const size_t size)
@ DMA_READY
Ready.
Definition: hal_stm32_dma.h:76
volatile dmastate_t state
Driver state.
DMA stream configuration structure.
#define STM32_PWM_USE_TIM2
Definition: mcuconf_h7.h:419
static uint32_t idx
static const struct usb_config_descriptor config
Definition: usb_ser_hw.c:200
DSHOT driver structure.
Definition: esc_dshot.h:287
uint32_t crc_errors
number of crc errors
Definition: esc_dshot.h:321
DshotPackets dshotMotors
object managing dma control frame for outgoing command
Definition: esc_dshot.h:356
mailbox_t mb
mailbox for dshot telemetry thread
Definition: esc_dshot.h:316
DMADriver dmap
DMA driver associated with pwm timer.
Definition: esc_dshot.h:306
PWMConfig pwm_conf
PWM config associated with pwm timer.
Definition: esc_dshot.h:301
uint16_t bit1Duty
Definition: esc_dshot.h:330
const DSHOTConfig * config
DMA config associated with pwm timer.
Definition: esc_dshot.h:291
msg_t _mbBuf[1]
mailbox buffer for dshot telemetry thread
Definition: esc_dshot.h:311
uint32_t tlm_frame_nb
number of sucessful telemetry frame received
Definition: esc_dshot.h:326
DMAConfig dma_conf
DMA config associated with pwm timer.
Definition: esc_dshot.h:296
uint16_t bit0Duty
Definition: esc_dshot.h:329
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98