Paparazzi UAS  v5.15_devel-81-gd13dafb
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  const size_t timerWidthInBytes = getTimerWidth(config->pwmp);
125 
126  static const SerialConfig tlmcfg = {
127  .speed = DSHOT_TELEMETRY_BAUD,
128  .cr1 = 0, // pas de parité
129  .cr2 = USART_CR2_STOP1_BITS, // 1 bit de stop
130  .cr3 = 0 // pas de controle de flux hardware (CTS, RTS)
131  };
132 
133  driver->config = config;
134 
135  driver->dma_conf = (DMAConfig) {
136  .stream = config->dma_stream,
137  .channel = config->dma_channel,
138  .dma_priority = 2,
139  .irq_priority = 6,
140  .direction = DMA_DIR_M2P,
141  .psize = timerWidthInBytes,
142  .msize = timerWidthInBytes,
143  .inc_peripheral_addr = false,
144  .inc_memory_addr = true,
145  .circular = false,
146  .error_cb = NULL,
147  .end_cb = NULL,
148  .pburst = 0,
149  // use mburst only if buffer size satisfy aligmnent requirement
150  .mburst = DSHOT_DMA_BUFFER_SIZE % (timerWidthInBytes * 4) ? 0 : 4,
151  .fifo = 0
152  };
153 
154  driver->pwm_conf = (PWMConfig) {
155  .frequency = TICK_FREQ,
156  .period = TICKS_PER_PERIOD,
157  .callback = NULL,
158  .channels = {
159  {
160  .mode = PWM_OUTPUT_ACTIVE_HIGH,
161  .callback = NULL
162  },
163  {
164  .mode = DSHOT_CHANNELS > 1 ? PWM_OUTPUT_ACTIVE_HIGH : PWM_OUTPUT_DISABLED,
165  .callback = NULL
166  },
167  {
168  .mode = DSHOT_CHANNELS > 2 ? PWM_OUTPUT_ACTIVE_HIGH : PWM_OUTPUT_DISABLED,
169  .callback = NULL
170  },
171  {
172  .mode = DSHOT_CHANNELS > 3 ? PWM_OUTPUT_ACTIVE_HIGH : PWM_OUTPUT_DISABLED,
173  .callback = NULL
174  },
175  },
176  .cr2 = STM32_TIM_CR2_CCDS,
177  .dier = STM32_TIM_DIER_UDE
178  };
179 
180  driver->crc_errors = 0;
181  memset(&driver->dsdb, 0UL, sizeof(driver->dsdb));
182 
183  dmaObjectInit(&driver->dmap);
184  chMBObjectInit(&driver->mb, driver->_mbBuf, ARRAY_LEN(driver->_mbBuf));
185 
186  dmaStart(&driver->dmap, &driver->dma_conf);
187 
188  if (driver->config->tlm_sd) {
189  sdStart(driver->config->tlm_sd, &tlmcfg);
190  chThdCreateStatic(driver->waDshotTlmRec, sizeof(driver->waDshotTlmRec), NORMALPRIO,
191  dshotTlmRec, driver);
192  }
193 
194  pwmStart(driver->config->pwmp, &driver->pwm_conf);
195  driver->config->pwmp->tim->DCR = DCR_DBL | DCR_DBA(driver->config->pwmp); // enable bloc register DMA transaction
196  pwmChangePeriod(driver->config->pwmp, DSHOT_PWM_PERIOD);
197 
198  for (size_t j = 0; j < DSHOT_CHANNELS; j++) {
199  pwmEnableChannel(driver->config->pwmp, j, 0);
200  driver->dshotMotors.dp[j] = makeDshotPacket(0, 0);
201  }
202 
203 }
204 
215 void dshotSetThrottle(DSHOTDriver *driver, const uint8_t index,
216  const uint16_t throttle)
217 {
218  if (throttle > 0 && throttle <= DSHOT_CMD_MAX) {
219  return; // special commands (except MOTOR_STOP) can't be applied from this function
220  } else {
221  // send normal throttle
222  if (index < DSHOT_CHANNELS) {
223  setDshotPacketThrottle(&driver->dshotMotors.dp[index], Min(throttle, DSHOT_MAX_VALUE));
224  } else if (index == DSHOT_ALL_MOTORS) {
225  for (uint8_t _index = 0; _index < DSHOT_CHANNELS; _index++) {
226  setDshotPacketThrottle(&driver->dshotMotors.dp[_index], Min(throttle, DSHOT_MAX_VALUE));
227  }
228  }
229  }
230 }
231 
240 void dshotSendSpecialCommand(DSHOTDriver *driver, const uint8_t index,
241  const dshot_special_commands_t specmd)
242 {
243  if (specmd > DSHOT_CMD_MAX) {
244  return; // Don't apply special commands from this function
245  }
246  if (index < DSHOT_CHANNELS) {
247  setDshotPacketThrottle(&driver->dshotMotors.dp[index], specmd);
248  setDshotPacketTlm(&driver->dshotMotors.dp[index], driver->config->tlm_sd != NULL);
249  } else if (index == DSHOT_ALL_MOTORS) {
250  for (uint8_t _index = 0; _index < DSHOT_CHANNELS; _index++) {
251  setDshotPacketThrottle(&driver->dshotMotors.dp[_index], specmd);
252  setDshotPacketTlm(&driver->dshotMotors.dp[_index], driver->config->tlm_sd != NULL);
253  }
254  }
255 
256  uint8_t repeat;
257  switch (specmd) {
266  repeat = 10;
267  break;
268  default:
269  repeat = 1;
270  }
271 
272  while (repeat--) {
273  dshotSendFrame(driver);
274  chThdSleepMilliseconds(1);
275  }
276 }
277 
287 void dshotSendThrottles(DSHOTDriver *driver, const uint16_t throttles[DSHOT_CHANNELS])
288 {
289  for (uint8_t index = 0; index < DSHOT_CHANNELS; index++) {
290  setDshotPacketThrottle(&driver->dshotMotors.dp[index], throttles[index]);
291  }
292 
293  dshotSendFrame(driver);
294 }
295 
304 {
305  if (driver->dmap.state == DMA_READY) {
306  if ((driver->config->tlm_sd != NULL) &&
307  (driver->dshotMotors.onGoingQry == false)) {
308  driver->dshotMotors.onGoingQry = true;
309  const uint32_t index = (driver->dshotMotors.currentTlmQry + 1) % DSHOT_CHANNELS;
310  driver->dshotMotors.currentTlmQry = index;
311  setDshotPacketTlm(&driver->dshotMotors.dp[index], true);
312  chMBPostTimeout(&driver->mb, driver->dshotMotors.currentTlmQry, TIME_IMMEDIATE);
313  }
314 
315  buildDshotDmaBuffer(&driver->dshotMotors, &driver->dsdb, getTimerWidth(driver->config->pwmp));
316  dmaStartTransfert(&driver->dmap,
317  &driver->config->pwmp->tim->DMAR,
319 
320  }
321 }
322 
331 {
332  return driver->crc_errors;
333 }
334 
343 const DshotTelemetry *dshotGetTelemetry(const DSHOTDriver *driver, const uint32_t index)
344 {
345  return &driver->dshotMotors.dt[index];
346 }
347 
348 
349 /*
350 # _ __ _ _
351 # | '_ \ (_) | |
352 # | |_) | _ __ _ __ __ __ _ | |_ ___
353 # | .__/ | '__| | | \ \ / / / _` | | __| / _ \
354 # | | | | | | \ V / | (_| | \ |_ | __/
355 # |_| |_| |_| \_/ \__,_| \__| \___|
356 */
357 
358 static DshotPacket makeDshotPacket(const uint16_t _throttle, const bool tlmRequest)
359 {
360  DshotPacket dp = {.throttle = _throttle,
361  .telemetryRequest = (tlmRequest ? 1 : 0),
362  .crc = 0
363  };
364 
365  // compute checksum
366  uint16_t csum = (_throttle << 1) | dp.telemetryRequest;
367  for (int i = 0; i < 3; i++) {
368  dp.crc ^= csum; // xor data by nibbles
369  csum >>= 4;
370  }
371 
372  return dp;
373 }
374 
375 static inline void setDshotPacketThrottle(DshotPacket *const dp, const uint16_t throttle)
376 {
377  dp->throttle = throttle;
378  dp->telemetryRequest = 0;
379 }
380 
381 static inline void setDshotPacketTlm(DshotPacket *const dp, const bool tlmRequest)
382 {
383  dp->telemetryRequest = tlmRequest ? 1 : 0;
384 }
385 
386 static void buildDshotDmaBuffer(DshotPackets *const dsp, DshotDmaBuffer *const dma, const size_t timerWidth)
387 {
388  for (size_t chanIdx = 0; chanIdx < DSHOT_CHANNELS; chanIdx++) {
389  // compute checksum
390  DshotPacket *const dp = &dsp->dp[chanIdx];
391  dp->crc = 0;
392  uint16_t csum = (dp->throttle << 1) | dp->telemetryRequest;
393  for (int i = 0; i < 3; i++) {
394  dp->crc ^= csum; // xor data by nibbles
395  csum >>= 4;
396  }
397  // generate pwm frame
398  for (size_t bitIdx = 0; bitIdx < DSHOT_BIT_WIDTHS; bitIdx++) {
399  const uint16_t value = dp->rawFrame &
400  (1 << ((DSHOT_BIT_WIDTHS - 1) - bitIdx)) ?
402  if (timerWidth == 2) {
403  dma->widths16[bitIdx][chanIdx] = value;
404  } else {
405 #if DSHOT_AT_LEAST_ONE_32B_TIMER
406  dma->widths32[bitIdx][chanIdx] = value;
407 #else
408  chSysHalt("use of 32 bit timer implies to define DSHOT_AT_LEAST_ONE_32B_TIMER to TRUE");
409 #endif
410  }
411  }
412  // the bits for silence sync in case of continous sending are zeroed once at init
413  }
414 }
415 
416 static inline uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed)
417 {
418  uint8_t crc_u = crc;
419  crc_u ^= crc_seed;
420 
421  for (int i = 0; i < 8; i++) {
422  crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1);
423  }
424 
425  return (crc_u);
426 }
427 
428 static uint8_t calculateCrc8(const uint8_t *Buf, const uint8_t BufLen)
429 {
430  uint8_t crc = 0;
431  for (int i = 0; i < BufLen; i++) {
432  crc = updateCrc8(Buf[i], crc);
433  }
434 
435  return crc;
436 }
437 
438 __attribute__((const))
439 static size_t getTimerWidth(const PWMDriver *pwmp)
440 {
441  (void) pwmp;
442 
443  return (0
445  || (pwmp == &PWMD2)
446 #endif
447 #if STM32_PWM_USE_TIM5
448  || (pwmp == &PWMD5)
449 #endif
450  ) ? 4 : 2;
451 }
452 
453 
454 /*
455 # _ _ _
456 # | | | | | |
457 # | |_ | |__ _ __ ___ __ _ __| | ___
458 # | __| | '_ \ | '__| / _ \ / _` | / _` | / __|
459 # \ |_ | | | | | | | __/ | (_| | | (_| | \__ \
460 # \__| |_| |_| |_| \___| \__,_| \__,_| |___/
461 */
462 
463 static noreturn void dshotTlmRec(void *arg)
464 {
465  DSHOTDriver *driver = (DSHOTDriver *) arg;
466 
467  uint32_t escIdx = 0;
468 
469  chRegSetThreadName("dshotTlmRec");
470  while (true) {
471  chMBFetchTimeout(&driver->mb, (msg_t *) &escIdx, TIME_INFINITE);
472  const uint32_t idx = escIdx;
473  const bool success =
474  (sdReadTimeout(driver->config->tlm_sd, driver->dshotMotors.dt[idx].rawData, sizeof(DshotTelemetry),
475  TIME_MS2I(DSHOT_TELEMETRY_TIMEOUT_MS)) == sizeof(DshotTelemetry));
476  if (!success ||
477  (calculateCrc8(driver->dshotMotors.dt[idx].rawData,
478  sizeof(driver->dshotMotors.dt[idx].rawData)) != driver->dshotMotors.dt[idx].crc8)) {
479  // empty buffer to resync
480  while (sdGetTimeout(driver->config->tlm_sd, TIME_IMMEDIATE) >= 0) {};
481  memset(driver->dshotMotors.dt[idx].rawData, 0U, sizeof(DshotTelemetry));
482  // count errors
483  driver->crc_errors++;
484  }
485  driver->dshotMotors.onGoingQry = false;
486  }
487 }
488 
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:117
#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:381
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:193
PWMConfig pwm_conf
PWM config associated with pwm timer.
Definition: esc_dshot.h:222
static size_t getTimerWidth(const PWMDriver *pwmp)
Definition: esc_dshot.c:439
uint32_t stream
stream associated with transaction
DSHOT Driver configuration structure.
Definition: esc_dshot.h:141
PWMDriver * pwmp
PWM driver that feed up to 4 dshot lines.
Definition: esc_dshot.h:155
static void setDshotPacketThrottle(DshotPacket *const dp, const uint16_t throttle)
Definition: esc_dshot.c:375
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:215
#define DCR_DBA(pwmd)
Definition: esc_dshot.c:82
void dshotSendFrame(DSHOTDriver *driver)
send throttle order
Definition: esc_dshot.c:303
#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:240
DSHOT driver structure.
Definition: esc_dshot.h:208
#define DSHOT_BIT1_DUTY
Definition: esc_dshot.c:80
DshotDmaBuffer dsdb
Definition: esc_dshot.h:250
const DSHOTConfig * config
DMA config associated with pwm timer.
Definition: esc_dshot.h:212
dmastate_t state
Driver state.
#define DSHOT_ALL_MOTORS
special value for index : send order to all channels
Definition: esc_dshot.h:55
uint8_t crc8
Definition: esc_dshot.h:128
static DshotPacket makeDshotPacket(const uint16_t throttle, const bool tlmRequest)
Definition: esc_dshot.c:358
#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:386
#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:160
unsigned long uint32_t
Definition: types.h:18
volatile uint8_t currentTlmQry
Definition: esc_dshot.h:194
DshotPackets dshotMotors
Definition: esc_dshot.h:249
#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:343
uint8_t dma_channel
: dma channel associated with pwm timer used to generate dshot output
Definition: esc_dshot.h:150
#define DSHOT_CHANNELS
Definition: esc_dshot.h:48
#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:237
uint32_t crc_errors
number of crc errors
Definition: esc_dshot.h:242
static uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed)
Definition: esc_dshot.c:416
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:145
static const struct usb_config_descriptor config
Definition: usb_ser_hw.c:199
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:83
static noreturn void dshotTlmRec(void *arg)
Definition: esc_dshot.c:463
DMADriver dmap
DMA driver associated with pwm timer.
Definition: esc_dshot.h:227
DMAConfig dma_conf
DMA config associated with pwm timer.
Definition: esc_dshot.h:217
uint16_t rawFrame
Definition: esc_dshot.h:188
uint32_t dshotGetCrcErrorsCount(DSHOTDriver *driver)
return number of telemetry crc error since dshotStart
Definition: esc_dshot.c:330
DshotPacket dp[DSHOT_CHANNELS]
Definition: esc_dshot.h:192
static uint8_t calculateCrc8(const uint8_t *Buf, const uint8_t BufLen)
Definition: esc_dshot.c:428
volatile bool onGoingQry
Definition: esc_dshot.h:195
void dshotSendThrottles(DSHOTDriver *driver, const uint16_t throttles[DSHOT_CHANNELS])
send throttle packed order to all of the ESCs
Definition: esc_dshot.c:287
msg_t _mbBuf[1]
mailbox buffer for dshot telemetry thread
Definition: esc_dshot.h:232
uint16_t widths16[DSHOT_DMA_BUFFER_SIZE][DSHOT_CHANNELS]
Definition: esc_dshot.h:199
#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:47