...cleanup and fix rx decoding
This commit is contained in:
Wastl Kraus
parent
84adfe026a
commit
c2f1bf84d4
125
DShotRMT.cpp
125
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;
|
||||
}
|
||||
|
||||
//
|
||||
|
|
|
|||
55
DShotRMT.h
55
DShotRMT.h
|
|
@ -9,22 +9,31 @@
|
|||
#pragma once
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <driver/gpio.h>
|
||||
#include <driver/rmt_tx.h>
|
||||
#include <driver/rmt_rx.h>
|
||||
|
||||
// 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] = {};
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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: ");
|
||||
|
|
|
|||
Loading…
Reference in New Issue