esc_dshot.c

Go to the documentation of this file.

/*
 * Copyright (C) 2018 Alexandre Bustico <alexandre.bustico@enac.fr>
 *                    Gautier Hattenberger <gautier.hattenberger@enac.fr>
 *
 * This file is part of paparazzi
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, see
 * <http://www.gnu.org/licenses/>.
 */
 
#include "modules/actuators/esc_dshot.h"
#include <stdnoreturn.h>
#include <math.h>
#include <string.h>
#include "mcu_periph/sys_time_arch.h"
 
/*
#                     _            __   _            _    _      _
#                    | |          / _| (_)          (_)  | |    (_)
#                  __| |    ___  | |_   _    _ __    _   | |_    _     ___    _ __
#                 / _` |   / _ \ |  _| | |  | '_ \  | |  | __|  | |   / _ \  | '_ \
#                | (_| |  |  __/ | |   | |  | | | | | |  \ |_   | |  | (_) | | | | |
#                 \__,_|   \___| |_|   |_|  |_| |_| |_|   \__|  |_|   \___/  |_| |_|
*/
 
 
#ifndef DSHOT_TELEMETRY_BAUD
#define DSHOT_TELEMETRY_BAUD 115200U
#endif
 
#ifndef DSHOT_BIDIR_EXTENTED_TELEMETRY
#define DSHOT_BIDIR_EXTENTED_TELEMETRY  FALSE
#endif
 
#ifndef DSHOT_TELEMETRY_TIMEOUT_MS
#define DSHOT_TELEMETRY_TIMEOUT_MS 3
#endif
 
 
#ifdef STM32H7XX
// each H7 timer have the same max clock speed
#define PWM_FREQ         (STM32_TIMCLK1 / 1000U) // the timer will beat @240Mhz on STM32H7
#else
// some F4 and F7 timers are limited to  / 2
// others are limited to STM32_SYSCLK
// so we take the max frequency that all timers can run
#define PWM_FREQ         (STM32_SYSCLK / 2000U) // the timer will beat @84Mhz on STM32F4
#endif
 
 
#define TICKS_PER_PERIOD 1000
 
// ESCs are quite sensitive to the DSHOT duty cycle.
// 333 should work most of the time, but some ESC need 373
#ifndef DSHOT_BIT0_DUTY_RATIO
#define DSHOT_BIT0_DUTY_RATIO 373U
#endif
 
#if DSHOT_SPEED != 0 // statically defined
#   define DSHOT_FREQ (DSHOT_SPEED*1000)
#   define DSHOT_BIT0_DUTY (DSHOT_PWM_PERIOD * DSHOT_BIT0_DUTY_RATIO  / 1000U)
#   define DSHOT_BIT1_DUTY (DSHOT_BIT0_DUTY*2)
#else      // dynamically defined
#   define DSHOT_FREQ (driver->config->speed_khz * 1000U)
#   define DSHOT_BIT0_DUTY (driver->bit0Duty)
#   define DSHOT_BIT1_DUTY (driver->bit1Duty)
#endif
#define TICK_FREQ (PWM_FREQ * TICKS_PER_PERIOD)
#define DSHOT_PWM_PERIOD (TICK_FREQ/DSHOT_FREQ)
 
 
#define DCR_DBL ((DSHOT_CHANNELS-1) << 8) //  DSHOT_CHANNELS transfert(s)
// first register to get is CCR1
#define DCR_DBA(pwmd) (((uint32_t *) (&pwmd->tim->CCR) - ((uint32_t *) pwmd->tim)))
 
#define DSHOT_MAX_VALUE ((1U<<11U)-1U) // 11 bits used to send command, so maximum value is 2047
 
#define ARRAY_LEN(a) (sizeof(a)/sizeof(a[0]))
#define Min(x,y) (x < y ? x : y)
 
 
/*
#                 _ __                   _              _      _   _    _ __
#                | '_ \                 | |            | |    | | | |  | '_ \
#                | |_) |  _ __    ___   | |_     ___   | |_   | |_| |  | |_) |   ___
#                | .__/  | '__|  / _ \  | __|   / _ \  | __|   \__, |  | .__/   / _ \
#                | |     | |    | (_) | \ |_   | (_) | \ |_     __/ |  | |     |  __/
#                |_|     |_|     \___/   \__|   \___/   \__|   |___/   |_|      \___|
*/
static DshotPacket makeDshotPacket(const uint16_t throttle, const bool tlmRequest);
static void setCrc4(DshotPacket *dp);
static inline void setDshotPacketThrottle(DshotPacket * const dp, const uint16_t throttle);
static inline void setDshotPacketTlm(DshotPacket * const dp, const bool tlmRequest);
static void buildDshotDmaBuffer(DSHOTDriver *driver);
//static void buildDshotDmaBuffer(DshotPackets * const dsp,  DshotDmaBuffer * const dma, const size_t timerWidth);
static inline uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed);
static uint8_t calculateCrc8(const uint8_t *Buf, const uint8_t BufLen);
static noreturn void dshotTlmRec (void *arg);
static size_t getTimerWidth(const PWMDriver *pwmp);
#if DSHOT_BIDIR
static void processBidirErpm(DSHOTDriver *driver);
static void     dshotRestart(DSHOTDriver *driver);
#if DSHOT_BIDIR_EXTENTED_TELEMETRY
static void updateTelemetryFromBidirEdt(const DshotErps *erps, DshotTelemetry *tlm);
#endif
#endif
 
/*
#                         _ __    _
#                        | '_ \  (_)
#                  __ _  | |_) |  _
#                 / _` | | .__/  | |
#                | (_| | | |     | |
#                 \__,_| |_|     |_|
*/
 
void dshotStart(DSHOTDriver *driver, const DSHOTConfig *config)
{
    chDbgAssert(config->dma_buf != NULL, ".dma_buf must reference valid DshotDmaBuffer object");
 
#if DSHOT_BIDIR
  dshotRpmCaptureStart(&driver->rpm_capture, &config->dma_capt_cfg, config->pwmp->tim);
#endif
  
  memset((void *) config->dma_buf, 0, sizeof(*(config->dma_buf)));
  const size_t timerWidthInBytes = getTimerWidth(config->pwmp);
 
  static const SerialConfig  tlmcfg =  {
    .speed = DSHOT_TELEMETRY_BAUD,
    .cr1 = 0,                          // pas de parité
    .cr2 = USART_CR2_STOP1_BITS,       // 1 bit de stop
    .cr3 = 0                           // pas de controle de flux hardware (CTS, RTS)
  };
  // when dshot is bidir, the polarity is inverted
  static const uint32_t pwmPolarity = DSHOT_BIDIR ? PWM_OUTPUT_ACTIVE_LOW : PWM_OUTPUT_ACTIVE_HIGH;
 
  driver->config = config;
  // use pburst, mburst only if buffer size satisfy aligmnent requirement
  driver->dma_conf = (DMAConfig) {
    .stream = config->dma_stream,
#if STM32_DMA_SUPPORTS_DMAMUX
    .dmamux = config->dmamux,
#else
    .channel = config->dma_channel,
#endif
    .dma_priority = 3,
    .irq_priority = CORTEX_MAX_KERNEL_PRIORITY + 1,
    .direction = DMA_DIR_M2P,
    .psize = timerWidthInBytes,
    .msize = timerWidthInBytes,
#if __DCACHE_PRESENT
    .dcache_memory_in_use = config->dcache_memory_in_use,
#endif
    .inc_peripheral_addr = false,
    .inc_memory_addr = true,
    .op_mode = DMA_ONESHOT,
    .error_cb = NULL,
    .end_cb = NULL,
    .pburst = 0,
    .mburst = 0,
    .fifo = 4
  };
 
  driver->pwm_conf = (PWMConfig) {
  .frequency = TICK_FREQ,
  .period    = TICKS_PER_PERIOD,
  .callback  = NULL,
  .channels  = {
    {.mode = pwmPolarity,
     .callback = NULL},
    {.mode = DSHOT_CHANNELS > 1 ? pwmPolarity : PWM_OUTPUT_DISABLED,
     .callback = NULL},
    {.mode = DSHOT_CHANNELS > 2 ? pwmPolarity : PWM_OUTPUT_DISABLED,
     .callback = NULL},
    {.mode = DSHOT_CHANNELS > 3 ? pwmPolarity : PWM_OUTPUT_DISABLED,
     .callback = NULL},
  },
  .cr2  =  STM32_TIM_CR2_CCDS,
  .dier =  STM32_TIM_DIER_UDE
  };
 
  driver->crc_errors = 0;
    driver->tlm_frame_nb = 0;
#if DSHOT_SPEED == 0
  driver->bit0Duty = (DSHOT_PWM_PERIOD * DSHOT_BIT0_DUTY_RATIO / 1000U);
  driver->bit1Duty = (driver->bit0Duty*2U)            ;
#endif
 
 
 
  dmaObjectInit(&driver->dmap);
  chMBObjectInit(&driver->mb, driver->_mbBuf, ARRAY_LEN(driver->_mbBuf));
 
  const bool dmaOk = dmaStart(&driver->dmap, &driver->dma_conf);
  chDbgAssert(dmaOk == true, "dshot dma start error");
 
  if (driver->config->tlm_sd) {
    sdStart(driver->config->tlm_sd, &tlmcfg);
    chThdCreateStatic(driver->waDshotTlmRec, sizeof(driver->waDshotTlmRec), NORMALPRIO,
                      dshotTlmRec, driver);
  }
 
  pwmStart(driver->config->pwmp, &driver->pwm_conf);
  driver->config->pwmp->tim->DCR = DCR_DBL | DCR_DBA(driver->config->pwmp); // enable bloc register DMA transaction
  pwmChangePeriod(driver->config->pwmp, DSHOT_PWM_PERIOD);
 
  for (size_t j=0; j<DSHOT_CHANNELS; j++) {
    pwmEnableChannel(driver->config->pwmp, j, 0);
    driver->dshotMotors.dp[j] =  makeDshotPacket(0,0);
    chMtxObjectInit(&driver->dshotMotors.tlmMtx[j]);
  }
  driver->dshotMotors.onGoingQry = false;
  driver->dshotMotors.currentTlmQry = 0U;
}
 
void     dshotStop(DSHOTDriver *driver)
{
  pwmStop(driver->config->pwmp);
  dmaStopTransfert(&driver->dmap);
  dmaStop(&driver->dmap);
}
 
#if DSHOT_BIDIR
static void dshotRestart(DSHOTDriver *driver)
{
  const bool dmaOk = dmaStart(&driver->dmap, &driver->dma_conf);
  chDbgAssert(dmaOk == true, "dshot dma start error");
 
  pwmStart(driver->config->pwmp, &driver->pwm_conf);
  driver->config->pwmp->tim->DCR = DCR_DBL | DCR_DBA(driver->config->pwmp); // enable bloc register DMA transaction
  pwmChangePeriod(driver->config->pwmp, DSHOT_PWM_PERIOD);
 
  for (size_t j=0; j<DSHOT_CHANNELS; j++) {
    pwmEnableChannel(driver->config->pwmp, j, 0);
    driver->dshotMotors.dp[j] =  makeDshotPacket(0,0);
  }
}
#endif
 
void dshotSetThrottle(DSHOTDriver *driver, const uint8_t index,
                      const  uint16_t throttle)
{
  if (throttle > 0 && throttle <= DSHOT_CMD_MAX) {
    chDbgAssert(false, "dshotSetThrottle throttle error");
    return; // special commands (except MOTOR_STOP) can't be applied from this function
  } else {
    // send normal throttle
    if (index == DSHOT_ALL_MOTORS) {
      for (uint8_t _index = 0; _index < DSHOT_CHANNELS; _index++) {
        setDshotPacketThrottle(&driver->dshotMotors.dp[_index], Min(throttle, DSHOT_MAX_VALUE));
      }
    } else if ((index - DSHOT_CHANNEL_FIRST_INDEX) < DSHOT_CHANNELS) {
      setDshotPacketThrottle(&driver->dshotMotors.dp[index - DSHOT_CHANNEL_FIRST_INDEX],
           Min(throttle, DSHOT_MAX_VALUE));
    } else {
      chDbgAssert(false, "dshotSetThrottle index error");
    }
  }
}
 
void dshotSendSpecialCommand(DSHOTDriver *driver, const  uint8_t index,
                             const dshot_special_commands_t specmd)
{
  if (specmd > DSHOT_CMD_MAX) {
    return; // Don't apply special commands from this function
  }
 
  // some dangerous special commands need to be repeated 6 times
  // to avoid catastrophic failure
  uint8_t repeat;
  switch (specmd) {
    case DSHOT_CMD_SPIN_DIRECTION_1:
    case DSHOT_CMD_SPIN_DIRECTION_2:
    case DSHOT_CMD_3D_MODE_OFF:
    case DSHOT_CMD_3D_MODE_ON:
    case DSHOT_CMD_SAVE_SETTINGS:
    case DSHOT_CMD_SETTINGS_REQUEST:
    case DSHOT_CMD_AUDIO_STREAM_MODE_ON_OFF:
    case DSHOT_CMD_SILENT_MODE_ON_OFF:
      case DSHOT_CMD_BIDIR_EDT_MODE_ON:
    case DSHOT_CMD_BIDIR_EDT_MODE_OFF:
      repeat = 10;
      break;
    default:
      repeat = 1;
  }
 
  while (repeat--) {
    systime_t now = chVTGetSystemTimeX();
    if (index < DSHOT_CHANNELS) {
      setDshotPacketThrottle(&driver->dshotMotors.dp[index], specmd);
      setDshotPacketTlm(&driver->dshotMotors.dp[index], driver->config->tlm_sd != NULL);
  } else if (index == DSHOT_ALL_MOTORS) {
    for (uint8_t _index = 0; _index < DSHOT_CHANNELS; _index++) {
      setDshotPacketThrottle(&driver->dshotMotors.dp[_index], specmd);
      setDshotPacketTlm(&driver->dshotMotors.dp[_index], driver->config->tlm_sd != NULL);
    }
  } else {
      chDbgAssert(false, "dshotSetThrottle index error");
    }
    dshotSendFrame(driver);
    if (repeat)
      chThdSleepUntilWindowed(now, now + TIME_US2I(500));
  }
}
 
void dshotSendThrottles(DSHOTDriver *driver, const  uint16_t throttles[DSHOT_CHANNELS])
{
  for (uint8_t index = 0; index < DSHOT_CHANNELS; index++) {
    setDshotPacketThrottle(&driver->dshotMotors.dp[index], throttles[index]);
  }
 
  dshotSendFrame(driver);
}
 
 
 
 
void dshotSendFrame(DSHOTDriver *driver)
{
  if (driver->dmap.state == DMA_READY) {
#if DSHOT_BIDIR
    const tprio_t currentPrio = chThdSetPriority(HIGHPRIO);
#endif
    if ((driver->config->tlm_sd != NULL) &&
        (driver->dshotMotors.onGoingQry == false)) {
      driver->dshotMotors.onGoingQry = true;
      const msg_t index = (driver->dshotMotors.currentTlmQry + 1U) % DSHOT_CHANNELS;
      driver->dshotMotors.currentTlmQry = (uint8_t) index;
      setDshotPacketTlm(&driver->dshotMotors.dp[index], true);
      chMBPostTimeout(&driver->mb, index, TIME_IMMEDIATE);
    }
 
    //buildDshotDmaBuffer(&driver->dshotMotors, driver->config->dma_buf, getTimerWidth(driver->config->pwmp));
    buildDshotDmaBuffer(driver);
    dmaTransfert(&driver->dmap,
                      &driver->config->pwmp->tim->DMAR,
                      driver->config->dma_buf, DSHOT_DMA_BUFFER_SIZE * DSHOT_CHANNELS);
 
#if DSHOT_BIDIR
    dshotStop(driver);
    dshotRpmCatchErps(&driver->rpm_capture);
    processBidirErpm(driver);
    dshotRestart(driver);
    chThdSetPriority(currentPrio);
#endif
  }
}
 
uint32_t dshotGetCrcErrorCount(const DSHOTDriver *driver)
{
  return driver->crc_errors;
}
 
uint32_t dshotGetTelemetryFrameCount(const DSHOTDriver *driver)
{
  return driver->tlm_frame_nb;
}
 
 
DshotTelemetry dshotGetTelemetry(DSHOTDriver *driver, uint32_t index)
{
  index -= DSHOT_CHANNEL_FIRST_INDEX;
  chDbgAssert(index < DSHOT_CHANNELS, "dshot index error");
  chMtxLock(&driver->dshotMotors.tlmMtx[index]);
  const DshotTelemetry tlm = driver->dshotMotors.dt[index];
  chMtxUnlock(&driver->dshotMotors.tlmMtx[index]);
  return tlm;
}
 
/*
 
    [2] 0010 mmmm mmmm - Temperature frame in degree Celsius, just like Blheli_32 and KISS [0, 1, ..., 255]
    [4] 0100 mmmm mmmm - Voltage frame with a step size of 0,25V [0, 0.25 ..., 63,75]
    [6] 0110 mmmm mmmm - Current frame with a step size of 1A [0, 1, ..., 255]
    [8] 1000 mmmm mmmm - Debug frame 1 not associated with any specific value, can be used to debug ESC firmware
    [10] 1010 mmmm mmmm - Debug frame 2 not associated with any specific value, can be used to debug ESC firmware
    [12] 1100 mmmm mmmm - Stress level frame [0, 1, ..., 255] (since v2.0.0)
    [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)
 
 */
#if DSHOT_BIDIR && DSHOT_BIDIR_EXTENTED_TELEMETRY
static void updateTelemetryFromBidirEdt(const DshotErps *erps, DshotTelemetry *tlm)
{
  switch(DshotErpsEdtType(erps)) {
  case EDT_TEMP:
    tlm->frame.temp = DshotErpsEdtTempCentigrade(erps); break;
    
  case EDT_VOLT:
    tlm->frame.voltage = DshotErpsEdtCentiVolts(erps); break;
    
  case EDT_CURRENT:
    tlm->frame.current = DshotErpsEdtCurrentAmp(erps) * 100U; break;
 
  case EDT_STRESS:
   tlm->stress = DshotErpsEdtStress(erps); break;
 
  case EDT_STATUS:
    tlm->status = DshotErpsEdtStatus(erps);break;
    
  default: {};
  }
  tlm->ts = chVTGetSystemTimeX();
}
#endif
 
#if DSHOT_BIDIR
uint32_t dshotGetEperiod(DSHOTDriver *driver, const uint32_t index)
{
  chDbgAssert(index < DSHOT_CHANNELS, "index check failed");
   DshotErpsSetFromFrame(&driver->erps,  driver->rpms_frame[index]);
   if (DshotErpsCheckCrc4(&driver->erps)) {
#if DSHOT_BIDIR_EXTENTED_TELEMETRY
     if (DshotErpsIsEdt(&driver->erps)) {
       if (driver->config->tlm_sd == NULL) {
   DshotTelemetry *tlm = &driver->dshotMotors.dt[index];
   updateTelemetryFromBidirEdt(&driver->erps, tlm);
       }
       return DSHOT_BIDIR_TLM_EDT;
     }
#endif
     return DshotErpsGetEperiod(&driver->erps);
   } else {
     return DSHOT_BIDIR_ERR_CRC;
   }
}
uint32_t dshotGetRpm(DSHOTDriver *driver, uint32_t index)
{
  index -= DSHOT_CHANNEL_FIRST_INDEX;
  chDbgAssert(index < DSHOT_CHANNELS, "index check failed");
   DshotErpsSetFromFrame(&driver->erps,  driver->rpms_frame[index]);
   if (DshotErpsCheckCrc4(&driver->erps)) {
#if DSHOT_BIDIR_EXTENTED_TELEMETRY
     if (DshotErpsIsEdt(&driver->erps)) {
       if (driver->config->tlm_sd == NULL) {
   DshotTelemetry *tlm = &driver->dshotMotors.dt[index];
   updateTelemetryFromBidirEdt(&driver->erps, tlm);
       }
       return DSHOT_BIDIR_TLM_EDT;
     }
#endif
     return DshotErpsGetRpm(&driver->erps);
   } else {
     return DSHOT_BIDIR_ERR_CRC;
   }
}
#endif
 
 
 
/*
#                 _ __           _                    _
#                | '_ \         (_)                  | |
#                | |_) |  _ __   _   __   __   __ _  | |_     ___
#                | .__/  | '__| | |  \ \ / /  / _` | | __|   / _ \
#                | |     | |    | |   \ V /  | (_| | \ |_   |  __/
#                |_|     |_|    |_|    \_/    \__,_|  \__|   \___|
*/
 
static void setCrc4(DshotPacket *dp)
{
  // compute checksum
  dp->crc = 0;
  uint16_t csum = (dp->throttle << 1) | dp->telemetryRequest;
  for (int i = 0; i < 3; i++) {
    dp->crc ^=  csum;   // xor data by nibbles
    csum >>= 4;
  }
#if DSHOT_BIDIR
  dp->crc = ~(dp->crc); // crc is inverted when dshot bidir protocol is choosed
#endif
}
 
static DshotPacket makeDshotPacket(const uint16_t _throttle, const bool tlmRequest)
{
  DshotPacket dp = {.throttle = _throttle,
                    .telemetryRequest = (tlmRequest ? 1 : 0),
                    .crc = 0
                   };
 
  setCrc4(&dp);
  return dp;
}
 
static inline void setDshotPacketThrottle(DshotPacket *const dp, const uint16_t throttle)
{
  dp->throttle = throttle;
  dp->telemetryRequest = 0;
}
 
static inline void setDshotPacketTlm(DshotPacket *const dp, const bool tlmRequest)
{
  dp->telemetryRequest =  tlmRequest ? 1 : 0;
}
 
static void buildDshotDmaBuffer(DSHOTDriver *driver)
{
  DshotPackets *const dsp = &driver->dshotMotors;
  DshotDmaBuffer *const dma =  driver->config->dma_buf;
  const size_t timerWidth = getTimerWidth(driver->config->pwmp);
  
  for (size_t chanIdx = 0; chanIdx < DSHOT_CHANNELS; chanIdx++) {
    // compute checksum
    DshotPacket * const dp  = &dsp->dp[chanIdx];
    setCrc4(dp);
    // generate pwm frame
    for (size_t bitIdx = 0; bitIdx < DSHOT_BIT_WIDTHS; bitIdx++) {
      const uint16_t value = dp->rawFrame &
                             (1 << ((DSHOT_BIT_WIDTHS - 1) - bitIdx)) ?
                             DSHOT_BIT1_DUTY : DSHOT_BIT0_DUTY;
      if (timerWidth == 2) {
        dma->widths16[bitIdx+DSHOT_PRE_FRAME_SILENT_SYNC_BITS][chanIdx] = value;
      } else {
#if DSHOT_AT_LEAST_ONE_32B_TIMER
        dma->widths32[bitIdx+DSHOT_PRE_FRAME_SILENT_SYNC_BITS][chanIdx] = value;
#else
        chSysHalt("use of 32 bit timer implies to define DSHOT_AT_LEAST_ONE_32B_TIMER to TRUE");
#endif
      }
    }
    // the bits for silence sync (pre and post) in case of continous sending are zeroed once at init
  }
}
 
 
static inline uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed)
{
  uint8_t crc_u = crc;
  crc_u ^= crc_seed;
 
  for (int i = 0; i < 8; i++) {
    crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1);
  }
 
  return (crc_u);
}
 
static uint8_t calculateCrc8(const uint8_t *Buf, const uint8_t BufLen)
{
  uint8_t crc = 0;
  for (int i = 0; i < BufLen; i++) {
    crc = updateCrc8(Buf[i], crc);
  }
 
  return crc;
}
 
 
#if DSHOT_BIDIR
static void processBidirErpm(DSHOTDriver *driver)
{
  for (size_t idx = 0; idx < DSHOT_CHANNELS; idx++) {
    driver->rpms_frame[idx] = dshotRpmGetFrame(&driver->rpm_capture, idx);
  }
}
#endif
 
 
__attribute__((const))
static size_t   getTimerWidth(const PWMDriver *pwmp)
{
  (void) pwmp;
 
  return (0
#if STM32_PWM_USE_TIM2
          || (pwmp == &PWMD2)
#endif
#if STM32_PWM_USE_TIM5
          || (pwmp == &PWMD5)
#endif
         ) ? 4 : 2;
}
 
 
/*
#                 _      _                                 _
#                | |    | |                               | |
#                | |_   | |__    _ __    ___    __ _    __| |   ___
#                | __|  | '_ \  | '__|  / _ \  / _` |  / _` |  / __|
#                \ |_   | | | | | |    |  __/ | (_| | | (_| |  \__ \
#                 \__|  |_| |_| |_|     \___|  \__,_|  \__,_|  |___/
*/
 
 
static noreturn void dshotTlmRec(void *arg)
{
  DSHOTDriver *driver = (DSHOTDriver *) arg;
  DshotTelemetry tlm;
 
  msg_t escIdx = 0;
 
  chRegSetThreadName("dshotTlmRec");
  while (true) {
    chMBFetchTimeout(&driver->mb,  &escIdx, TIME_INFINITE);
    const uint32_t idx = escIdx;
    const bool success =
      (sdReadTimeout(driver->config->tlm_sd, tlm.frame.rawData, sizeof(DshotTelemetryFrame),
                     TIME_MS2I(DSHOT_TELEMETRY_TIMEOUT_MS)) == sizeof(DshotTelemetryFrame));
    if (!success ||
        (calculateCrc8(tlm.frame.rawData, sizeof(tlm.frame.rawData)) != tlm.frame.crc8)) {
      // empty buffer to resync
      while (sdGetTimeout(driver->config->tlm_sd, TIME_IMMEDIATE) >= 0) {};
      memset(tlm.frame.rawData, 0U, sizeof(DshotTelemetry));
      // count errors
      if (success)
        driver->crc_errors++;
    } else {
      // big-endian to little-endian conversion
      tlm.frame.voltage = __builtin_bswap16(tlm.frame.voltage);
      tlm.frame.current = __builtin_bswap16(tlm.frame.current);
      tlm.frame.consumption = __builtin_bswap16(tlm.frame.consumption);
      tlm.frame.rpm = __builtin_bswap16(tlm.frame.rpm);
      tlm.ts = chVTGetSystemTimeX();
      driver->tlm_frame_nb++;
    }
    
    chMtxLock(&driver->dshotMotors.tlmMtx[idx]);
    driver->dshotMotors.dt[idx] = tlm;
    chMtxUnlock(&driver->dshotMotors.tlmMtx[idx]);
    driver->dshotMotors.onGoingQry = false;
  }
}