From c2f1bf84d48c34861a9e0e1f8627e8634f577393 Mon Sep 17 00:00:00 2001 From: Wastl Kraus Date: Fri, 13 Jun 2025 21:50:00 +0200 Subject: [PATCH] ...cleanup and fix rx decoding --- DShotRMT.cpp | 125 +++++++++++++++++++++++---------- DShotRMT.h | 55 ++++++++++++--- examples/dshot300/dshot300.ino | 18 ++++- 3 files changed, 151 insertions(+), 47 deletions(-) diff --git a/DShotRMT.cpp b/DShotRMT.cpp index 680b27b..070d6b6 100644 --- a/DShotRMT.cpp +++ b/DShotRMT.cpp @@ -12,9 +12,10 @@ DShotRMT::DShotRMT(gpio_num_t gpio, dshot_mode_t mode, bool isBidirectional) : _gpio(gpio), _mode(mode), _isBidirectional(isBidirectional) {} -// +// Sets up RMT TX and RX channels as well as encoder configuration void DShotRMT::begin() { + // RX RMT Channel Configuration (for BiDirectional DShot) if (_isBidirectional) { rmt_rx_channel_config_t rmt_rx_channel_config = { @@ -28,8 +29,12 @@ void DShotRMT::begin() rmt_new_rx_channel(&rmt_rx_channel_config, &_rmt_rx_channel); rmt_enable(_rmt_rx_channel); + + _receive_config.signal_range_min_ns = 100; + _receive_config.signal_range_max_ns = 10000; } + // TX RMT Channel Configuration rmt_tx_channel_config_t rmt_tx_channel_config = { .gpio_num = _gpio, .clk_src = DSHOT_CLOCK_SRC_DEFAULT, @@ -44,22 +49,23 @@ void DShotRMT::begin() rmt_new_tx_channel(&rmt_tx_channel_config, &_rmt_tx_channel); rmt_enable(_rmt_tx_channel); - // Create new encoder + // Use a copy encoder to send raw symbols if (!_dshot_encoder) { rmt_copy_encoder_config_t enc_cfg = {}; rmt_new_copy_encoder(&enc_cfg, &_dshot_encoder); } + // Configure transmission looping _transmit_config.loop_count = -1; _transmit_config.flags.eot_level = _isBidirectional; } -// +// Encodes and transmits a valid DShot Throttle value (48 - 2047) void DShotRMT::setThrottle(uint16_t throttle) { - // Fake 10 Bit transformation to be sure - throttle = throttle & 0b0000011111111111; + // Safety first - double check input range and 10 bit "translation" + throttle = (constrain(throttle, DSHOT_THROTTLE_MIN, DSHOT_THROTTLE_MAX)) & 0b0000011111111111; // Has Throttle really changed? if (throttle == _lastThrottle) @@ -67,43 +73,88 @@ void DShotRMT::setThrottle(uint16_t throttle) _lastThrottle = throttle; - // Prepare Throttle paket - dshot_packet = (throttle << 1) | (_isBidirectional ? 1 : 0); + // Assemble raw DShot packet and compute checksum + _tx_packet = (throttle << 1) | (_isBidirectional ? 1 : 0); - // CRC Calculation - uint16_t crc = 0; + uint16_t crc = _isBidirectional + ? (~(_tx_packet ^ (_tx_packet >> 4) ^ (_tx_packet >> 8))) & 0x0F + : (_tx_packet ^ (_tx_packet >> 4) ^ (_tx_packet >> 8)) & 0x0F; - if (_isBidirectional) - { - // Calculate checksum in inverted/BiDirectional Mode - crc = (~(dshot_packet ^ (dshot_packet >> 4) ^ (dshot_packet >> 8))) & 0x0F; - } - else - { - // - crc = (dshot_packet ^ (dshot_packet >> 4) ^ (dshot_packet >> 8)) & 0x0F; - } + _tx_packet = (_tx_packet << 4) | crc; - // attach CRC to DShot Paket - dshot_packet = (dshot_packet << 4) | crc; - - // Encode DShot Paket - rmt_symbol_word_t symbols[DSHOT_BITS_PER_FRAME] = {}; // 16 DShot Bits + Pause Bit + // Encode RMT symbols size_t count = 0; + encodeDShotTX(_tx_packet, _tx_symbols, count); - buildFrameSymbols(dshot_packet, symbols, count); - // Reset RMT Signnal loop before sending new value + // Restart transmission with new data rmt_disable(_rmt_tx_channel); rmt_enable(_rmt_tx_channel); - // Finally transmit the complete DShot Paket - rmt_transmit(_rmt_tx_channel, _dshot_encoder, symbols, count * sizeof(rmt_symbol_word_t), &_transmit_config); + rmt_transmit(_rmt_tx_channel, _dshot_encoder, _tx_symbols, count * sizeof(rmt_symbol_word_t), &_transmit_config); } -// -void DShotRMT::buildFrameSymbols(uint16_t dshot_packet, rmt_symbol_word_t *symbols, size_t &count) +// --- Get eRPM from ESC --- +// Receives and decodes a response frame from ESC containing eRPM info +uint32_t DShotRMT::getERPM() { - // Always start from the top + static size_t rx_size = sizeof(_rx_symbols); + + if (_rmt_rx_channel == nullptr) + return _last_erpm; + + // Attempt to receive a new frame + if (!rmt_receive(_rmt_rx_channel, _rx_symbols, rx_size, &_receive_config)) + return _last_erpm; + + uint16_t received_bits = 0; + _received_packet = 0; + + // Decode raw RMT encoded bits + for (int i = 0; i < DSHOT_BITS_PER_FRAME; ++i) + { + rmt_symbol_word_t symbols = _rx_symbols[i]; + + // Validate signal polarity + if (symbols.level0 != 1 || symbols.level1 != 0) + break; + + uint32_t total_ticks = symbols.duration0 + symbols.duration1; + bool bit = (symbols.duration0 > (total_ticks / 2)); + + _received_packet <<= 1; + _received_packet |= bit ? 1 : 0; + + received_bits++; + } + + if (received_bits < 16) + return _last_erpm; + + // Extract data & checksum from packet + uint16_t packet_data = _received_packet >> 4; + uint8_t recalc_packet_crc = (packet_data ^ (packet_data >> 4) ^ (packet_data >> 8)) & 0x0F; + uint8_t packet_crc = _received_packet & 0x0F; + + if (recalc_packet_crc != packet_crc) + return _last_erpm; + + // Assume received value is DShot eRPM + uint16_t throttle = packet_data >> 1; + + // Filter noise values + if (throttle < DSHOT_THROTTLE_MIN || throttle > DSHOT_THROTTLE_MAX) + return _last_erpm; + + // Approximate eRPM (ESC dependent, scale factor can be tuned) + _last_erpm = throttle * 100; + return _last_erpm; +} + +// --- Encode DShot TX Frame --- +// Converts a 16-bit packet into a valid DShot Frame for RMT +void DShotRMT::encodeDShotTX(uint16_t dshot_packet, rmt_symbol_word_t *symbols, size_t &count) +{ + // Always start encoding from the top count = 0; // @@ -113,11 +164,6 @@ void DShotRMT::buildFrameSymbols(uint16_t dshot_packet, rmt_symbol_word_t *symbo switch (_mode) { - case DSHOT_OFF: - ticks_per_bit = 0; - ticks_zero_high = 0; - ticks_one_high = 0; - break; case DSHOT150: ticks_per_bit = 64; ticks_zero_high = 24; @@ -138,6 +184,13 @@ void DShotRMT::buildFrameSymbols(uint16_t dshot_packet, rmt_symbol_word_t *symbo ticks_zero_high = 3; ticks_one_high = 6; break; + // Safety first + case DSHOT_OFF: + default: + ticks_per_bit = 0; + ticks_zero_high = 0; + ticks_one_high = 0; + break; } // diff --git a/DShotRMT.h b/DShotRMT.h index 916cd6e..120aef9 100644 --- a/DShotRMT.h +++ b/DShotRMT.h @@ -9,22 +9,31 @@ #pragma once #include +#include #include #include -// DShot Protocol Constants +// --- DShot Protocol Constants --- +// Constants to define timing and encoding rules for DShot Protocol static constexpr auto DSHOT_THROTTLE_FAILSAVE = 0; static constexpr auto DSHOT_THROTTLE_MIN = 48; static constexpr auto DSHOT_THROTTLE_MAX = 2047; static constexpr auto DSHOT_BITS_PER_FRAME = 17; static constexpr auto PAUSE_BITS = 21; + static constexpr auto DSHOT_NULL_PACKET = 0b0000000000000000; static constexpr auto DSHOT_FULL_PACKET = 0b1111111111111111; static constexpr auto NO_ERPM_SIGNAL = 0; -static constexpr auto DSHOT_CLOCK_SRC_DEFAULT = RMT_CLK_SRC_DEFAULT; -static constexpr auto DSHOT_RMT_RESOLUTION = 10 * 1000 * 1000; // set 10 MHz RMT Resolution -// DShot Mode Selection +// RMT configuration parameters +static constexpr auto DSHOT_CLOCK_SRC_DEFAULT = RMT_CLK_SRC_DEFAULT; +static constexpr auto DSHOT_RMT_RESOLUTION = 10 * 1000 * 1000; // 10 MHz + +static constexpr auto TX_BUFFER_SIZE = DSHOT_BITS_PER_FRAME; +static constexpr auto RX_BUFFER_SIZE = DSHOT_BITS_PER_FRAME + 4; // Padding for RX decoding + +// --- DShot Mode Selection --- +// Select the appropriate bit timing for the protocol typedef enum dshot_mode_e { DSHOT_OFF, @@ -34,29 +43,57 @@ typedef enum dshot_mode_e DSHOT1200 } dshot_mode_t; -// +// --- DShotRMT Class --- +// This class provides an abstraction for sending and optionally receiving DShot frames. +// It uses ESP32's RMT peripheral for precise timing control, including BiDirectional RX. class DShotRMT { public: + // Constructor: initializes configuration state DShotRMT(gpio_num_t gpio, dshot_mode_t mode = DSHOT300, bool isBidirectional = false); + // Initializes the RMT TX and RX channels void begin(); + + // Sets a new throttle value (0-2047) and sends it repeatedly void setThrottle(uint16_t throttle); + // Receives and decodes the latest eRPM value from ESC, if available + uint32_t getERPM(); + + // Accessors for GPIO and DShot mode gpio_num_t getGPIO() const { return _gpio; } dshot_mode_t getDShotMode() const { return _mode; } + // Stores the last valid eRPM received from the ESC + uint32_t _last_erpm = 0; + private: + // Converts a 16-bit DShot packet into RMT symbols and appends pause + void encodeDShotTX(uint16_t dshot_packet, rmt_symbol_word_t *symbols, size_t &count); + + // --- Configuration Parameter --- gpio_num_t _gpio; dshot_mode_t _mode; bool _isBidirectional; - uint16_t _lastThrottle = DSHOT_FULL_PACKET; - uint16_t dshot_packet = DSHOT_NULL_PACKET; - rmt_channel_handle_t _rmt_tx_channel = nullptr; + // --- DShot Packets Container --- + uint16_t _lastThrottle = DSHOT_FULL_PACKET; + uint16_t _received_packet = DSHOT_NULL_PACKET; + uint16_t _tx_packet = DSHOT_NULL_PACKET; + + // --- RMT Channel --- rmt_channel_handle_t _rmt_rx_channel = nullptr; + rmt_channel_handle_t _rmt_tx_channel = nullptr; + + // --- DShot RMT Encoder --- rmt_encoder_handle_t _dshot_encoder = nullptr; + + // --- RMT Configuration --- + rmt_receive_config_t _receive_config = {}; rmt_transmit_config_t _transmit_config = {}; - void buildFrameSymbols(uint16_t dshot_packet, rmt_symbol_word_t *symbols, size_t &count); + // --- RMT Symbol Buffer --- + rmt_symbol_word_t _rx_symbols[RX_BUFFER_SIZE] = {}; + rmt_symbol_word_t _tx_symbols[TX_BUFFER_SIZE] = {}; }; diff --git a/examples/dshot300/dshot300.ino b/examples/dshot300/dshot300.ino index c67876f..c3d8ca3 100644 --- a/examples/dshot300/dshot300.ino +++ b/examples/dshot300/dshot300.ino @@ -48,9 +48,23 @@ void loop() // Now send the value motor01.setThrottle(throttle_input); - // BiDirectional DShot: print out the received eRPMs + // BiDirectional DShot: print out the received eRPMs every 2 seconds if (IS_BIDIRECTIONAL) { + static unsigned long last_print_time = 0; + unsigned long now = millis(); + + if (now - last_print_time >= 2000) + { + last_print_time = now; + + uint32_t erpm = motor01.getERPM(); + + USB_Serial.print("Sent Throttle: "); + USB_Serial.print(throttle_input); + USB_Serial.print(" | eRPM: "); + USB_Serial.println(erpm); + } } } @@ -66,7 +80,7 @@ int readSerialThrottle() int throttle_input = input.toInt(); // Clamp the value to the DShot range - throttle_input = constrain(throttle_input, 48, 2047); + throttle_input = constrain(throttle_input, DSHOT_THROTTLE_MIN, DSHOT_THROTTLE_MAX); last_throttle = throttle_input; USB_Serial.print("Throttle set to: ");