Cansat
/
DShotRMT
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

...fixing biDirectional 

...Hardware testing needed for biDirectional Mode
This commit is contained in:
Wastl Kraus 2025-07-17 23:24:28 +02:00
parent d8338c3346
commit ab8cb8b15c
2 changed files with 141 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

225
DShotRMT.cpp
View File

@ -8,13 +8,31 @@
#include <DShotRMT.h> #include <DShotRMT.h>
// // --- 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.
DShotRMT::DShotRMT(gpio_num_t gpio, dshot_mode_t mode, bool isBidirectional) DShotRMT::DShotRMT(gpio_num_t gpio, dshot_mode_t mode, bool isBidirectional)
: _gpio(gpio), _mode(mode), _isBidirectional(isBidirectional) {} : _gpio(gpio), _mode(mode), _isBidirectional(isBidirectional) {}
// Sets up RMT TX and RX channels as well as encoder configuration // Sets up RMT TX and RX channels as well as encoder configuration
void DShotRMT::begin() void DShotRMT::begin()
{ {
// TX RMT Channel Configuration
_rmt_tx_channel_config = {
.gpio_num = _gpio,
.clk_src = DSHOT_CLOCK_SRC_DEFAULT,
.resolution_hz = DSHOT_RMT_RESOLUTION,
.mem_block_symbols = 64,
.trans_queue_depth = 2,
// .flags = {
// invert Signal if BiDirectional DShot Mode
// .invert_out = _isBidirectional,
// .with_dma = false}
};
rmt_new_tx_channel(&_rmt_tx_channel_config, &_rmt_tx_channel);
rmt_enable(_rmt_tx_channel);
// RX RMT Channel Configuration (for BiDirectional DShot) // RX RMT Channel Configuration (for BiDirectional DShot)
if (_isBidirectional) if (_isBidirectional)
{ {
@ -23,32 +41,18 @@ void DShotRMT::begin()
.clk_src = DSHOT_CLOCK_SRC_DEFAULT, .clk_src = DSHOT_CLOCK_SRC_DEFAULT,
.resolution_hz = DSHOT_RMT_RESOLUTION, .resolution_hz = DSHOT_RMT_RESOLUTION,
.mem_block_symbols = 64, .mem_block_symbols = 64,
.flags = { // .flags = {
.invert_in = false, // .invert_in = false,
.with_dma = false}}; // .with_dma = false}
};
rmt_new_rx_channel(&_rmt_rx_channel_config, &_rmt_rx_channel); rmt_new_rx_channel(&_rmt_rx_channel_config, &_rmt_rx_channel);
// rmt_enable(_rmt_rx_channel); rmt_enable(_rmt_rx_channel);
_receive_config.signal_range_min_ns = 300; _receive_config.signal_range_min_ns = 300;
_receive_config.signal_range_max_ns = 5000; _receive_config.signal_range_max_ns = 5000;
} }
// TX RMT Channel Configuration
_rmt_tx_channel_config = {
.gpio_num = _gpio,
.clk_src = DSHOT_CLOCK_SRC_DEFAULT,
.resolution_hz = DSHOT_RMT_RESOLUTION,
.mem_block_symbols = 64,
.trans_queue_depth = 1,
.flags = {
// invert Signal if BiDirectional DShot Mode
.invert_out = _isBidirectional,
.with_dma = false}};
rmt_new_tx_channel(&_rmt_tx_channel_config, &_rmt_tx_channel);
// rmt_enable(_rmt_tx_channel);
// Use a copy encoder to send raw symbols // Use a copy encoder to send raw symbols
if (!_dshot_encoder) if (!_dshot_encoder)
{ {
@ -80,9 +84,11 @@ void DShotRMT::setThrottle(uint16_t throttle)
size_t count = 0; size_t count = 0;
encodeDShotTX(_tx_packet, _tx_symbols, count); encodeDShotTX(_tx_packet, _tx_symbols, count);
rmt_disable(_rmt_tx_channel); //
rmt_enable(_rmt_tx_channel);
rmt_transmit(_rmt_tx_channel, _dshot_encoder, _tx_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);
if (_isBidirectional)
esp_rom_delay_us(120);
} }
// --- Get eRPM from ESC --- // --- Get eRPM from ESC ---
@ -91,58 +97,58 @@ uint32_t DShotRMT::getERPM()
{ {
if (_isBidirectional) if (_isBidirectional)
{ {
static size_t rx_size = sizeof(_rx_symbols);
if (_rmt_rx_channel == nullptr) if (_rmt_rx_channel == nullptr)
return _last_erpm; return _last_erpm;
// Attempt to receive a new frame // Attempt to receive a new frame
if (!rmt_receive(_rmt_rx_channel, _rx_symbols, rx_size, &_receive_config)) if (!rmt_receive(_rmt_rx_channel, _rx_symbols, sizeof(_rx_symbols), &_receive_config))
return _last_erpm; return _last_erpm;
uint16_t received_bits = 0; //
_received_packet = 0; _last_erpm = decodeDShotRX(_rx_symbols, DSHOT_BITS_PER_FRAME);
// 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; 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 < rx_size; ++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 > symbols.duration1);
// _received_packet <<= 1;
// _received_packet |= bit ? 1 : 0;
// received_bits++;
// }
// // 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;
} }
// Nothing to do here // Nothing to do here
return _last_erpm; return _last_erpm;
} }
@ -162,30 +168,34 @@ uint32_t DShotRMT::getMotorRPM(uint8_t magnet_count)
// Calculate CRC for DShot Paket // Calculate CRC for DShot Paket
uint16_t DShotRMT::calculateCRC(uint16_t dshot_packet) uint16_t DShotRMT::calculateCRC(uint16_t dshot_packet)
{ {
uint16_t _packet = (dshot_packet << 1) | (_isBidirectional ? 1 : 0);
// Clear container before new calculation // Clear container before new calculation
_packet_crc = DSHOT_NULL_PACKET; _packet_crc = DSHOT_NULL_PACKET;
// CRC is inverted for biDirectional DSHot // CRC calculation for DShot (4 bits)
_packet_crc = _isBidirectional _packet_crc = ((_packet ^ (_packet >> 4) ^ (_packet >> 8)) & 0b0000000000001111);
? (~(dshot_packet ^ (dshot_packet >> 4) ^ (dshot_packet >> 8))) & 0x0F
: (dshot_packet ^ (dshot_packet >> 4) ^ (dshot_packet >> 8)) & 0x0F; // CRC is inverted for biDirectional DShot
if (_isBidirectional)
_packet_crc = (~_packet_crc) & 0b0000000000001111;
return _packet_crc; return _packet_crc;
} }
// Assamble DShot Paket (10 bit throttle + 1 bit telemetry request + 4 bit crc) // Assamble DShot Paket (11 bit throttle + 1 bit telemetry request + 4 bit crc)
uint16_t DShotRMT::assambleDShotPaket(uint16_t value) uint16_t DShotRMT::assambleDShotPaket(uint16_t value)
{ {
// Dummy conversion to 11 bits
uint16_t _value = value & 0b0000011111111111;
// Clear container // Clear container
_tx_packet = DSHOT_NULL_PACKET; _tx_packet = DSHOT_NULL_PACKET;
// dummy 11bit convertion
_tx_packet = value & 0b0000011111111111;
// Assemble raw DShot packet and add checksum // Assemble raw DShot packet and add checksum
_tx_packet = (value << 1) | (_isBidirectional ? 1 : 0); _packet_crc = calculateCRC(_value);
_packet_crc = calculateCRC(_tx_packet);
_tx_packet = (_value << 1) | (_isBidirectional ? 1 : 0);
_tx_packet = (_tx_packet << 4) | _packet_crc; _tx_packet = (_tx_packet << 4) | _packet_crc;
return _tx_packet; return _tx_packet;
@ -198,7 +208,6 @@ void DShotRMT::encodeDShotTX(uint16_t dshot_packet, rmt_symbol_word_t *symbols,
// Always start encoding from the top // Always start encoding from the top
count = 0; count = 0;
//
uint32_t ticks_per_bit = 0; uint32_t ticks_per_bit = 0;
uint32_t ticks_zero_high = 0; uint32_t ticks_zero_high = 0;
uint32_t ticks_one_high = 0; uint32_t ticks_one_high = 0;
@ -225,7 +234,6 @@ void DShotRMT::encodeDShotTX(uint16_t dshot_packet, rmt_symbol_word_t *symbols,
ticks_zero_high = 3; ticks_zero_high = 3;
ticks_one_high = 6; ticks_one_high = 6;
break; break;
// Safety first
case DSHOT_OFF: case DSHOT_OFF:
default: default:
ticks_per_bit = 0; ticks_per_bit = 0;
@ -234,7 +242,6 @@ void DShotRMT::encodeDShotTX(uint16_t dshot_packet, rmt_symbol_word_t *symbols,
break; break;
} }
//
uint32_t ticks_zero_low = ticks_per_bit - ticks_zero_high; uint32_t ticks_zero_low = ticks_per_bit - ticks_zero_high;
uint32_t ticks_one_low = ticks_per_bit - ticks_one_high; uint32_t ticks_one_low = ticks_per_bit - ticks_one_high;
@ -242,17 +249,55 @@ void DShotRMT::encodeDShotTX(uint16_t dshot_packet, rmt_symbol_word_t *symbols,
for (int i = 15; i >= 0; i--) for (int i = 15; i >= 0; i--)
{ {
bool bit = (dshot_packet >> i) & 0x01; bool bit = (dshot_packet >> i) & 0x01;
symbols[count].level0 = 1; if (_isBidirectional)
symbols[count].duration0 = bit ? ticks_one_high : ticks_zero_high; {
symbols[count].level1 = 0; symbols[count].level0 = 0;
symbols[count].duration1 = bit ? ticks_one_low : ticks_zero_low; symbols[count].duration0 = bit ? ticks_one_high : ticks_zero_high;
symbols[count].level1 = 1;
symbols[count].duration1 = bit ? ticks_one_low : ticks_zero_low;
}
else
{
symbols[count].level0 = 1;
symbols[count].duration0 = bit ? ticks_one_high : ticks_zero_high;
symbols[count].level1 = 0;
symbols[count].duration1 = bit ? ticks_one_low : ticks_zero_low;
}
count++; count++;
} }
// Append the Pause Bits
symbols[count].level0 = 0;
symbols[count].duration0 = ticks_per_bit * (_isBidirectional ? SWITCH_PAUSE : PAUSE_BITS);
symbols[count].level1 = 0;
symbols[count].duration1 = 0;
count++;
} }
// Decodes a response frame from ESC containing eRPM info
uint16_t DShotRMT::decodeDShotRX(const rmt_symbol_word_t *symbols, uint32_t count)
{
// Container for received frame
uint16_t _rec_frame = DSHOT_NULL_PACKET;
// Fill the Frame bit by bit
for (size_t i = 0; i < DSHOT_BITS_PER_FRAME && i < count; ++i)
{
bool bit = (symbols[i].duration0 < symbols[i].duration1);
_rec_frame = (_rec_frame << 1) | bit;
}
// Store the received CRC for checking
uint16_t _temp = _rec_frame >> 4;
// Masking the received CRC
uint8_t crc_recv = _rec_frame & 0x0F;
// Calculate CRC for received frame again
uint8_t crc_calc = (_temp ^ (_temp >> 4) ^ (_temp >> 8)) & 0b0000000000001111;
if (_isBidirectional)
crc_calc = (~crc_calc) & 0x0F;
// Checking CRC
if (crc_recv != crc_calc)
return _last_erpm;
// Cut "telemetric" bit leaving "raw" value
uint16_t raw = _temp >> 1;
return _last_erpm = raw;
}

10
DShotRMT.h
View File

@ -13,14 +13,13 @@
#include <driver/gpio.h> #include <driver/gpio.h>
#include <driver/rmt_tx.h> #include <driver/rmt_tx.h>
#include <driver/rmt_rx.h> #include <driver/rmt_rx.h>
#include <driver/rmt_types.h>
// --- DShot Protocol Constants --- // --- DShot Protocol Constants ---
// Constants to define timing and encoding rules for DShot Protocol // Constants to define timing and encoding rules for DShot Protocol
static constexpr auto DSHOT_THROTTLE_FAILSAVE = 0; static constexpr auto DSHOT_THROTTLE_FAILSAVE = 0;
static constexpr auto DSHOT_THROTTLE_MIN = 48; static constexpr auto DSHOT_THROTTLE_MIN = 48;
static constexpr auto DSHOT_THROTTLE_MAX = 2047; static constexpr auto DSHOT_THROTTLE_MAX = 2047;
static constexpr auto DSHOT_BITS_PER_FRAME = 17; static constexpr auto DSHOT_BITS_PER_FRAME = 16;
static constexpr auto PAUSE_BITS = 21; static constexpr auto PAUSE_BITS = 21;
static constexpr auto SWITCH_PAUSE = 4; static constexpr auto SWITCH_PAUSE = 4;
@ -30,7 +29,7 @@ static constexpr auto NO_ERPM_SIGNAL = 0;
// RMT configuration parameters // RMT configuration parameters
static constexpr auto DSHOT_CLOCK_SRC_DEFAULT = RMT_CLK_SRC_DEFAULT; static constexpr auto DSHOT_CLOCK_SRC_DEFAULT = RMT_CLK_SRC_DEFAULT;
static constexpr auto DSHOT_RMT_RESOLUTION = 10 * 1000 * 1000; // 10 MHz - 0.1 µs Tick static constexpr auto DSHOT_RMT_RESOLUTION = 10 * 1000 * 1000; // 10 MHz Clock
static constexpr auto TX_BUFFER_SIZE = DSHOT_BITS_PER_FRAME; static constexpr auto TX_BUFFER_SIZE = DSHOT_BITS_PER_FRAME;
static constexpr auto RX_BUFFER_SIZE = 32; // Padding for RX decoding static constexpr auto RX_BUFFER_SIZE = 32; // Padding for RX decoding
@ -79,6 +78,9 @@ private:
// Converts a 16-bit DShot packet into RMT symbols and appends pause // 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); void encodeDShotTX(uint16_t dshot_packet, rmt_symbol_word_t *symbols, size_t &count);
// Decodes the ESC answer
uint16_t decodeDShotRX(const rmt_symbol_word_t *symbols, uint32_t count);
// --- Configuration Parameter --- // --- Configuration Parameter ---
gpio_num_t _gpio; gpio_num_t _gpio;
dshot_mode_t _mode; dshot_mode_t _mode;
@ -108,5 +110,5 @@ private:
rmt_symbol_word_t _tx_symbols[TX_BUFFER_SIZE] = {}; rmt_symbol_word_t _tx_symbols[TX_BUFFER_SIZE] = {};
// Stores the last valid eRPM received from the ESC // Stores the last valid eRPM received from the ESC
uint32_t _last_erpm = 0; uint16_t _last_erpm = 0;
}; };