Paparazzi UAS  v5.17_devel-24-g2ae834f
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
35 /*
36 # _ __ _ _ _ _
37 # | | / _| (_) (_) | | (_)
38 # __| | ___ | |_ _ _ __ _ | |_ _ ___ _ __
39 # / _` | / _ \ | _| | | | '_ \ | | | __| | | / _ \ | '_ \
40 # | (_| | | __/ | | | | | | | | | | \ |_ | | | (_) | | | | |
41 # \__,_| \___| |_| |_| |_| |_| |_| \__| |_| \___/ |_| |_|
42 */
43 
47 #ifndef DSHOT_SPEED
48 #define DSHOT_SPEED 600
49 #endif
50 
54 #ifndef DSHOT_TELEMETRY_BAUD
55 #define DSHOT_TELEMETRY_BAUD 115200
56 #endif
57 
60 #ifndef DSHOT_TELEMETRY_TIMEOUT_MS
61 #define DSHOT_TELEMETRY_TIMEOUT_MS 3
62 #endif
63 
65 #define PWM_FREQ (STM32_SYSCLK/2000)
66 
73 #define TICKS_PER_PERIOD 1000
74 
75 // Some extra defines and macros
76 #define DSHOT_FREQ (DSHOT_SPEED*1000) // in Hz
77 #define TICK_FREQ (PWM_FREQ * TICKS_PER_PERIOD)
78 #define DSHOT_PWM_PERIOD (TICK_FREQ/DSHOT_FREQ)
79 #define DSHOT_BIT0_DUTY (DSHOT_PWM_PERIOD * 373 / 1000)
80 #define DSHOT_BIT1_DUTY (DSHOT_BIT0_DUTY*2)
81 #define DCR_DBL ((DSHOT_CHANNELS-1) << 8) // DSHOT_CHANNELS transfert(s), first register to get is CCR1
82 #define DCR_DBA(pwmd) (((uint32_t *) (&pwmd->tim->CCR) - ((uint32_t *) pwmd->tim)))
83 
84 #define ARRAY_LEN(a) (sizeof(a)/sizeof(a[0]))
85 #define Min(x,y) (x < y ? x : y)
86 
87 #define DSHOT_MAX_VALUE ((1<<11)-1) // 11 bits used to send command, so maximum value is 2047
88 
89 /*
90 # _ __ _ _ _ _ _ __
91 # | '_ \ | | | | | | | | | '_ \
92 # | |_) | _ __ ___ | |_ ___ | |_ | |_| | | |_) | ___
93 # | .__/ | '__| / _ \ | __| / _ \ | __| \__, | | .__/ / _ \
94 # | | | | | (_) | \ |_ | (_) | \ |_ __/ | | | | __/
95 # |_| |_| \___/ \__| \___/ \__| |___/ |_| \___|
96 */
97 static DshotPacket makeDshotPacket(const uint16_t throttle, const bool tlmRequest);
98 static inline void setDshotPacketThrottle(DshotPacket * const dp, const uint16_t throttle);
99 static inline void setDshotPacketTlm(DshotPacket * const dp, const bool tlmRequest);
100 static void buildDshotDmaBuffer(DshotPackets * const dsp, DshotDmaBuffer * const dma, const size_t timerWidth);
101 static inline uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed);
102 static uint8_t calculateCrc8(const uint8_t *Buf, const uint8_t BufLen);
103 static noreturn void dshotTlmRec(void *arg);
104 static size_t getTimerWidth(const PWMDriver *pwmp);
105 
106 /*
107 # _ __ _
108 # | '_ \ (_)
109 # __ _ | |_) | _
110 # / _` | | .__/ | |
111 # | (_| | | | | |
112 # \__,_| |_| |_|
113 */
114 
123 {
124  _Static_assert((void *) &driver->dsdb == (void *) &driver->dsdb.widths16);
125  _Static_assert((void *) &driver->dsdb.widths32 == (void *) &driver->dsdb.widths16);
126 
127  memset((void *) &driver->dsdb, 0, sizeof(driver->dsdb));
128  const size_t timerWidthInBytes = getTimerWidth(config->pwmp);
129 
130  static const SerialConfig tlmcfg = {
131  .speed = DSHOT_TELEMETRY_BAUD,
132  .cr1 = 0, // pas de parité
133  .cr2 = USART_CR2_STOP1_BITS, // 1 bit de stop
134  .cr3 = 0 // pas de controle de flux hardware (CTS, RTS)
135  };
136 
137  driver->config = config;
138  // use pburst, mburst only if buffer size satisfy aligmnent requirement
139  driver->dma_conf = (DMAConfig) {
140  .stream = config->dma_stream,
141  .channel = config->dma_channel,
142  .dma_priority = 3,
143  .irq_priority = 6,
144  .direction = DMA_DIR_M2P,
145  .psize = timerWidthInBytes,
146  .msize = timerWidthInBytes,
147  .inc_peripheral_addr = false,
148  .inc_memory_addr = true,
149  .circular = false,
150  .error_cb = NULL,
151  .end_cb = NULL,
152  .pburst = 0,
153  .mburst = 0,
154  .fifo = 0
155  };
156 
157  driver->pwm_conf = (PWMConfig) {
158  .frequency = TICK_FREQ,
159  .period = TICKS_PER_PERIOD,
160  .callback = NULL,
161  .channels = {
162  {
163  .mode = PWM_OUTPUT_ACTIVE_HIGH,
164  .callback = NULL
165  },
166  {
167  .mode = DSHOT_CHANNELS > 1 ? PWM_OUTPUT_ACTIVE_HIGH : PWM_OUTPUT_DISABLED,
168  .callback = NULL
169  },
170  {
171  .mode = DSHOT_CHANNELS > 2 ? PWM_OUTPUT_ACTIVE_HIGH : PWM_OUTPUT_DISABLED,
172  .callback = NULL
173  },
174  {
175  .mode = DSHOT_CHANNELS > 3 ? PWM_OUTPUT_ACTIVE_HIGH : PWM_OUTPUT_DISABLED,
176  .callback = NULL
177  },
178  },
179  .cr2 = STM32_TIM_CR2_CCDS,
180  .dier = STM32_TIM_DIER_UDE
181  };
182 
183  driver->crc_errors = 0;
184  dmaObjectInit(&driver->dmap);
185  chMBObjectInit(&driver->mb, driver->_mbBuf, ARRAY_LEN(driver->_mbBuf));
186 
187  dmaStart(&driver->dmap, &driver->dma_conf);
188 
189  if (driver->config->tlm_sd) {
190  sdStart(driver->config->tlm_sd, &tlmcfg);
191  chThdCreateStatic(driver->waDshotTlmRec, sizeof(driver->waDshotTlmRec), NORMALPRIO,
192  dshotTlmRec, driver);
193  }
194 
195  pwmStart(driver->config->pwmp, &driver->pwm_conf);
196  driver->config->pwmp->tim->DCR = DCR_DBL | DCR_DBA(driver->config->pwmp); // enable bloc register DMA transaction
197  pwmChangePeriod(driver->config->pwmp, DSHOT_PWM_PERIOD);
198 
199  for (size_t j = 0; j < DSHOT_CHANNELS; j++) {
200  pwmEnableChannel(driver->config->pwmp, j, 0);
201  driver->dshotMotors.dp[j] = makeDshotPacket(0, 0);
202  }
203 
204 }
205 
216 void dshotSetThrottle(DSHOTDriver *driver, const uint8_t index,
217  const uint16_t throttle)
218 {
219  if (throttle > 0 && throttle <= DSHOT_CMD_MAX) {
220  return; // special commands (except MOTOR_STOP) can't be applied from this function
221  } else {
222  // send normal throttle
223  if (index < DSHOT_CHANNELS) {
224  setDshotPacketThrottle(&driver->dshotMotors.dp[index], Min(throttle, DSHOT_MAX_VALUE));
225  } else if (index == DSHOT_ALL_MOTORS) {
226  for (uint8_t _index = 0; _index < DSHOT_CHANNELS; _index++) {
227  setDshotPacketThrottle(&driver->dshotMotors.dp[_index], Min(throttle, DSHOT_MAX_VALUE));
228  }
229  }
230  }
231 }
232 
241 void dshotSendSpecialCommand(DSHOTDriver *driver, const uint8_t index,
242  const dshot_special_commands_t specmd)
243 {
244  if (specmd > DSHOT_CMD_MAX) {
245  return; // Don't apply special commands from this function
246  }
247  if (index < DSHOT_CHANNELS) {
248  setDshotPacketThrottle(&driver->dshotMotors.dp[index], specmd);
249  setDshotPacketTlm(&driver->dshotMotors.dp[index], driver->config->tlm_sd != NULL);
250  } else if (index == DSHOT_ALL_MOTORS) {
251  for (uint8_t _index = 0; _index < DSHOT_CHANNELS; _index++) {
252  setDshotPacketThrottle(&driver->dshotMotors.dp[_index], specmd);
253  setDshotPacketTlm(&driver->dshotMotors.dp[_index], driver->config->tlm_sd != NULL);
254  }
255  }
256 
257  uint8_t repeat;
258  switch (specmd) {
267  repeat = 10;
268  break;
269  default:
270  repeat = 1;
271  }
272 
273  while (repeat--) {
274  dshotSendFrame(driver);
275  chThdSleepMilliseconds(1);
276  }
277 }
278 
288 void dshotSendThrottles(DSHOTDriver *driver, const uint16_t throttles[DSHOT_CHANNELS])
289 {
290  for (uint8_t index = 0; index < DSHOT_CHANNELS; index++) {
291  setDshotPacketThrottle(&driver->dshotMotors.dp[index], throttles[index]);
292  }
293 
294  dshotSendFrame(driver);
295 }
296 
305 {
306  if (driver->dmap.state == DMA_READY) {
307  if ((driver->config->tlm_sd != NULL) &&
308  (driver->dshotMotors.onGoingQry == false)) {
309  driver->dshotMotors.onGoingQry = true;
310  const uint32_t index = (driver->dshotMotors.currentTlmQry + 1) % DSHOT_CHANNELS;
311  driver->dshotMotors.currentTlmQry = index;
312  setDshotPacketTlm(&driver->dshotMotors.dp[index], true);
313  chMBPostTimeout(&driver->mb, driver->dshotMotors.currentTlmQry, TIME_IMMEDIATE);
314  }
315 
316  buildDshotDmaBuffer(&driver->dshotMotors, &driver->dsdb, getTimerWidth(driver->config->pwmp));
317  dmaStartTransfert(&driver->dmap,
318  &driver->config->pwmp->tim->DMAR,
320 
321  }
322 }
323 
332 {
333  return driver->crc_errors;
334 }
335 
344 const DshotTelemetry *dshotGetTelemetry(const DSHOTDriver *driver, const uint32_t index)
345 {
346  return &driver->dshotMotors.dt[index];
347 }
348 
349 
350 /*
351 # _ __ _ _
352 # | '_ \ (_) | |
353 # | |_) | _ __ _ __ __ __ _ | |_ ___
354 # | .__/ | '__| | | \ \ / / / _` | | __| / _ \
355 # | | | | | | \ V / | (_| | \ |_ | __/
356 # |_| |_| |_| \_/ \__,_| \__| \___|
357 */
358 
359 static DshotPacket makeDshotPacket(const uint16_t _throttle, const bool tlmRequest)
360 {
361  DshotPacket dp = {.throttle = _throttle,
362  .telemetryRequest = (tlmRequest ? 1 : 0),
363  .crc = 0
364  };
365 
366  // compute checksum
367  uint16_t csum = (_throttle << 1) | dp.telemetryRequest;
368  for (int i = 0; i < 3; i++) {
369  dp.crc ^= csum; // xor data by nibbles
370  csum >>= 4;
371  }
372 
373  return dp;
374 }
375 
376 static inline void setDshotPacketThrottle(DshotPacket *const dp, const uint16_t throttle)
377 {
378  dp->throttle = throttle;
379  dp->telemetryRequest = 0;
380 }
381 
382 static inline void setDshotPacketTlm(DshotPacket *const dp, const bool tlmRequest)
383 {
384  dp->telemetryRequest = tlmRequest ? 1 : 0;
385 }
386 
387 static void buildDshotDmaBuffer(DshotPackets *const dsp, DshotDmaBuffer *const dma, const size_t timerWidth)
388 {
389  for (size_t chanIdx = 0; chanIdx < DSHOT_CHANNELS; chanIdx++) {
390  // compute checksum
391  DshotPacket *const dp = &dsp->dp[chanIdx];
392  dp->crc = 0;
393  uint16_t csum = (dp->throttle << 1) | dp->telemetryRequest;
394  for (int i = 0; i < 3; i++) {
395  dp->crc ^= csum; // xor data by nibbles
396  csum >>= 4;
397  }
398  // generate pwm frame
399  for (size_t bitIdx = 0; bitIdx < DSHOT_BIT_WIDTHS; bitIdx++) {
400  const uint16_t value = dp->rawFrame &
401  (1 << ((DSHOT_BIT_WIDTHS - 1) - bitIdx)) ?
403  if (timerWidth == 2) {
404  dma->widths16[bitIdx+DSHOT_PRE_FRAME_SILENT_SYNC_BITS][chanIdx] = value;
405  } else {
406 #if DSHOT_AT_LEAST_ONE_32B_TIMER
407  dma->widths32[bitIdx+DSHOT_PRE_FRAME_SILENT_SYNC_BITS][chanIdx] = value;
408 #else
409  chSysHalt("use of 32 bit timer implies to define DSHOT_AT_LEAST_ONE_32B_TIMER to TRUE");
410 #endif
411  }
412  }
413  // the bits for silence sync in case of continous sending are zeroed once at init
414  }
415 }
416 
417 static inline uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed)
418 {
419  uint8_t crc_u = crc;
420  crc_u ^= crc_seed;
421 
422  for (int i = 0; i < 8; i++) {
423  crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1);
424  }
425 
426  return (crc_u);
427 }
428 
429 static uint8_t calculateCrc8(const uint8_t *Buf, const uint8_t BufLen)
430 {
431  uint8_t crc = 0;
432  for (int i = 0; i < BufLen; i++) {
433  crc = updateCrc8(Buf[i], crc);
434  }
435 
436  return crc;
437 }
438 
439 __attribute__((const))
440 static size_t getTimerWidth(const PWMDriver *pwmp)
441 {
442  (void) pwmp;
443 
444  return (0
446  || (pwmp == &PWMD2)
447 #endif
448 #if STM32_PWM_USE_TIM5
449  || (pwmp == &PWMD5)
450 #endif
451  ) ? 4 : 2;
452 }
453 
454 
455 /*
456 # _ _ _
457 # | | | | | |
458 # | |_ | |__ _ __ ___ __ _ __| | ___
459 # | __| | '_ \ | '__| / _ \ / _` | / _` | / __|
460 # \ |_ | | | | | | | __/ | (_| | | (_| | \__ \
461 # \__| |_| |_| |_| \___| \__,_| \__,_| |___/
462 */
463 
464 static noreturn void dshotTlmRec(void *arg)
465 {
466  DSHOTDriver *driver = (DSHOTDriver *) arg;
467 
468  uint32_t escIdx = 0;
469 
470  chRegSetThreadName("dshotTlmRec");
471  while (true) {
472  chMBFetchTimeout(&driver->mb, (msg_t *) &escIdx, TIME_INFINITE);
473  const uint32_t idx = escIdx;
474  const bool success =
475  (sdReadTimeout(driver->config->tlm_sd, driver->dshotMotors.dt[idx].rawData, sizeof(DshotTelemetry),
476  TIME_MS2I(DSHOT_TELEMETRY_TIMEOUT_MS)) == sizeof(DshotTelemetry));
477  if (!success ||
478  (calculateCrc8(driver->dshotMotors.dt[idx].rawData,
479  sizeof(driver->dshotMotors.dt[idx].rawData)) != driver->dshotMotors.dt[idx].crc8)) {
480  // empty buffer to resync
481  while (sdGetTimeout(driver->config->tlm_sd, TIME_IMMEDIATE) >= 0) {};
482  memset(driver->dshotMotors.dt[idx].rawData, 0U, sizeof(DshotTelemetry));
483  // count errors
484  driver->crc_errors++;
485  }
486  driver->dshotMotors.onGoingQry = false;
487  }
488 }
489 
Ready.
Definition: hal_stm32_dma.h:62
unsigned short uint16_t
Definition: types.h:16
#define DCR_DBL
Definition: esc_dshot.c:81
#define DSHOT_TELEMETRY_TIMEOUT_MS
Telemetry timeout in ms.
Definition: esc_dshot.c:61
DSHOT driver based on ChibiOS.
#define DSHOT_PWM_PERIOD
Definition: esc_dshot.c:78
static uint32_t idx
telemetry packed as sent by some KISS ESC
Definition: esc_dshot.h:121
#define Min(x, y)
Definition: esc_dshot.c:85
#define DSHOT_BIT_WIDTHS
DMA buffer size and number of channels.
Definition: esc_dshot.h:45
static void setDshotPacketTlm(DshotPacket *const dp, const bool tlmRequest)
Definition: esc_dshot.c:382
bool dmaStart(DMADriver *dmap, const DMAConfig *cfg)
Configures and activates the DMA peripheral.
Definition: hal_stm32_dma.c:85
DshotTelemetry dt[DSHOT_CHANNELS]
Definition: esc_dshot.h:197
PWMConfig pwm_conf
PWM config associated with pwm timer.
Definition: esc_dshot.h:226
static size_t getTimerWidth(const PWMDriver *pwmp)
Definition: esc_dshot.c:440
uint32_t stream
stream associated with transaction
DSHOT Driver configuration structure.
Definition: esc_dshot.h:145
PWMDriver * pwmp
PWM driver that feed up to 4 dshot lines.
Definition: esc_dshot.h:159
static void setDshotPacketThrottle(DshotPacket *const dp, const uint16_t throttle)
Definition: esc_dshot.c:376
void dshotStart(DSHOTDriver *driver, const DSHOTConfig *config)
Configures and activates the DSHOT peripheral.
Definition: esc_dshot.c:122
void dshotSetThrottle(DSHOTDriver *driver, const uint8_t index, const uint16_t throttle)
prepare throttle order for specified ESC
Definition: esc_dshot.c:216
#define DCR_DBA(pwmd)
Definition: esc_dshot.c:82
void dshotSendFrame(DSHOTDriver *driver)
send throttle order
Definition: esc_dshot.c:304
#define TICK_FREQ
Definition: esc_dshot.c:77
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:241
DSHOT driver structure.
Definition: esc_dshot.h:212
#define DSHOT_BIT1_DUTY
Definition: esc_dshot.c:80
DshotDmaBuffer dsdb
Definition: esc_dshot.h:254
const DSHOTConfig * config
DMA config associated with pwm timer.
Definition: esc_dshot.h:216
#define DSHOT_ALL_MOTORS
special value for index : send order to all channels
Definition: esc_dshot.h:59
uint8_t crc8
Definition: esc_dshot.h:132
static DshotPacket makeDshotPacket(const uint16_t throttle, const bool tlmRequest)
Definition: esc_dshot.c:359
#define DSHOT_MAX_VALUE
Definition: esc_dshot.c:87
DMA stream configuration structure.
static void buildDshotDmaBuffer(DshotPackets *const dsp, DshotDmaBuffer *const dma, const size_t timerWidth)
Definition: esc_dshot.c:387
#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:55
SerialDriver * tlm_sd
if non null : dshot telemetry serial driver
Definition: esc_dshot.h:164
unsigned long uint32_t
Definition: types.h:18
volatile uint8_t currentTlmQry
Definition: esc_dshot.h:198
DshotPackets dshotMotors
Definition: esc_dshot.h:253
#define STM32_PWM_USE_TIM2
Definition: mcuconf.h:230
const DshotTelemetry * dshotGetTelemetry(const DSHOTDriver *driver, const uint32_t index)
return last received telemetry data
Definition: esc_dshot.c:344
uint8_t dma_channel
: dma channel associated with pwm timer used to generate dshot output
Definition: esc_dshot.h:154
#define DSHOT_CHANNELS
Definition: esc_dshot.h:52
#define DSHOT_BIT0_DUTY
Definition: esc_dshot.c:79
#define ARRAY_LEN(a)
Definition: esc_dshot.c:84
MEMORY to PERIPHERAL.
Definition: hal_stm32_dma.h:84
mailbox_t mb
mailbox for dshot telemetry thread
Definition: esc_dshot.h:241
uint32_t crc_errors
number of crc errors
Definition: esc_dshot.h:246
static uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed)
Definition: esc_dshot.c:417
bool dmaStartTransfert(DMADriver *dmap, volatile void *periphp, void *mem0p, const size_t size)
Starts a DMA transaction.
unsigned char uint8_t
Definition: types.h:14
uint32_t dma_stream
: dma stream associated with pwm timer used to generate dshot output
Definition: esc_dshot.h:149
#define DSHOT_PRE_FRAME_SILENT_SYNC_BITS
Definition: esc_dshot.h:46
static const struct usb_config_descriptor config
Definition: usb_ser_hw.c:200
void dmaObjectInit(DMADriver *dmap)
Definition: hal_stm32_dma.c:53
dshot_special_commands_t
DSHOT special commands (0-47) for KISS and BLHELI ESC.
Definition: esc_dshot.h:87
static noreturn void dshotTlmRec(void *arg)
Definition: esc_dshot.c:464
DMADriver dmap
DMA driver associated with pwm timer.
Definition: esc_dshot.h:231
volatile dmastate_t state
Driver state.
DMAConfig dma_conf
DMA config associated with pwm timer.
Definition: esc_dshot.h:221
uint16_t rawFrame
Definition: esc_dshot.h:192
uint32_t dshotGetCrcErrorsCount(DSHOTDriver *driver)
return number of telemetry crc error since dshotStart
Definition: esc_dshot.c:331
DshotPacket dp[DSHOT_CHANNELS]
Definition: esc_dshot.h:196
static uint8_t calculateCrc8(const uint8_t *Buf, const uint8_t BufLen)
Definition: esc_dshot.c:429
volatile bool onGoingQry
Definition: esc_dshot.h:199
void dshotSendThrottles(DSHOTDriver *driver, const uint16_t throttles[DSHOT_CHANNELS])
send throttle packed order to all of the ESCs
Definition: esc_dshot.c:288
msg_t _mbBuf[1]
mailbox buffer for dshot telemetry thread
Definition: esc_dshot.h:236
uint16_t widths16[DSHOT_DMA_BUFFER_SIZE][DSHOT_CHANNELS]
Definition: esc_dshot.h:203
#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:73
#define DSHOT_DMA_BUFFER_SIZE
Definition: esc_dshot.h:48