...update release 0.6.3
* ...clean up * ...sendThrottle takes "0" ...good one * ...preparing update release 0.6.3
This commit is contained in:
parent
0e30fc8dfc
commit
8f8222c620
299
DShotRMT.cpp
299
DShotRMT.cpp
|
|
@ -9,8 +9,10 @@
|
||||||
#include "DShotRMT.h"
|
#include "DShotRMT.h"
|
||||||
#include <driver/rmt_tx.h>
|
#include <driver/rmt_tx.h>
|
||||||
|
|
||||||
// --- DShot Timings ---
|
// DShot Timing Configurations
|
||||||
// frame_length_us, ticks_per_bit, ticks_one_high, ticks_one_low, ticks_zero_high, ticks_zero_low
|
|
||||||
|
// Timing parameters for each DShot mode
|
||||||
|
// Format: {frame_length_us, ticks_per_bit, ticks_one_high, ticks_one_low, ticks_zero_high, ticks_zero_low}
|
||||||
constexpr dshot_timing_t DSHOT_TIMINGS[] = {
|
constexpr dshot_timing_t DSHOT_TIMINGS[] = {
|
||||||
{0, 0, 0, 0, 0, 0}, // DSHOT_OFF
|
{0, 0, 0, 0, 0, 0}, // DSHOT_OFF
|
||||||
{128, 64, 48, 16, 24, 40}, // DSHOT150
|
{128, 64, 48, 16, 24, 40}, // DSHOT150
|
||||||
|
|
@ -19,8 +21,9 @@ constexpr dshot_timing_t DSHOT_TIMINGS[] = {
|
||||||
{16, 8, 6, 2, 3, 5} // DSHOT1200
|
{16, 8, 6, 2, 3, 5} // DSHOT1200
|
||||||
};
|
};
|
||||||
|
|
||||||
// --- DShot Config Constructor ---
|
// Constructor with GPIO number
|
||||||
DShotRMT::DShotRMT(gpio_num_t gpio, dshot_mode_t mode, bool is_bidirectional) : _gpio(gpio),
|
DShotRMT::DShotRMT(gpio_num_t gpio, dshot_mode_t mode, bool is_bidirectional)
|
||||||
|
: _gpio(gpio),
|
||||||
_mode(mode),
|
_mode(mode),
|
||||||
_is_bidirectional(is_bidirectional),
|
_is_bidirectional(is_bidirectional),
|
||||||
_timing_config(DSHOT_TIMINGS[mode]),
|
_timing_config(DSHOT_TIMINGS[mode]),
|
||||||
|
|
@ -28,118 +31,113 @@ DShotRMT::DShotRMT(gpio_num_t gpio, dshot_mode_t mode, bool is_bidirectional) :
|
||||||
_rmt_rx_channel(nullptr),
|
_rmt_rx_channel(nullptr),
|
||||||
_dshot_encoder(nullptr),
|
_dshot_encoder(nullptr),
|
||||||
_last_erpm(0),
|
_last_erpm(0),
|
||||||
_current_packet(0),
|
_parsed_packet(0),
|
||||||
_packet{0},
|
_packet{0},
|
||||||
_last_transmission_time(0)
|
_last_transmission_time(0)
|
||||||
{
|
{
|
||||||
// Calculates frame time and adds switch/pause time
|
// Calculate frame timing including switch/pause time
|
||||||
_frame_timer_us = _timing_config.frame_length_us + DSHOT_SWITCH_TIME;
|
_frame_timer_us = _timing_config.frame_length_us + DSHOT_SWITCH_TIME;
|
||||||
|
|
||||||
// Doubles up frame time for bidirectional mode
|
// Double frame time for bidirectional mode (includes response time)
|
||||||
if (_is_bidirectional)
|
if (_is_bidirectional)
|
||||||
{
|
{
|
||||||
// DSHOT_SWITCH_TIME also needed two times
|
|
||||||
_frame_timer_us = (_frame_timer_us << 1);
|
_frame_timer_us = (_frame_timer_us << 1);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Easy Constructor
|
// Simple constructor using pin number
|
||||||
DShotRMT::DShotRMT(uint16_t pin_nr, dshot_mode_t mode, bool is_bidirectional) : DShotRMT(
|
DShotRMT::DShotRMT(uint16_t pin_nr, dshot_mode_t mode, bool is_bidirectional)
|
||||||
(gpio_num_t)pin_nr,
|
: DShotRMT((gpio_num_t)pin_nr, mode, is_bidirectional)
|
||||||
mode,
|
|
||||||
is_bidirectional)
|
|
||||||
{
|
{
|
||||||
// ...just to accept pin numbers and GPIO_NUMs
|
// Delegates to primary constructor with type cast
|
||||||
}
|
}
|
||||||
|
|
||||||
// Setup and configure DShotRMT
|
// Initialize DShotRMT
|
||||||
uint16_t DShotRMT::begin()
|
uint16_t DShotRMT::begin()
|
||||||
{
|
{
|
||||||
// Inits TX Channel
|
// Initialize TX channel
|
||||||
if (!_initTXChannel())
|
if (!_initTXChannel())
|
||||||
{
|
{
|
||||||
Serial.println(DSHOT_MSG_01);
|
_dshot_log(TX_INIT_FAILED);
|
||||||
return DSHOT_ERROR;
|
return DSHOT_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Inits RX Channel
|
// Initialize RX channel only if bidirectional mode is enabled
|
||||||
if (!_initRXChannel() && _is_bidirectional)
|
if (!_initRXChannel() && _is_bidirectional)
|
||||||
{
|
{
|
||||||
Serial.println(DSHOT_MSG_02);
|
_dshot_log(RX_INIT_FAILED);
|
||||||
return DSHOT_ERROR;
|
return DSHOT_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Inits DShot Decoder
|
// Initialize DShot encoder
|
||||||
if (!_initDShotEncoder())
|
if (!_initDShotEncoder())
|
||||||
{
|
{
|
||||||
// Serial.println(DSHOT_MSG_03);
|
|
||||||
return DSHOT_ERROR;
|
return DSHOT_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
// All good, ready
|
// All good, start
|
||||||
return DSHOT_OK;
|
return DSHOT_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Deprecated, use "sendThrottle()"" instead
|
// Send throttle value
|
||||||
bool DShotRMT::setThrottle(uint16_t throttle)
|
|
||||||
{
|
|
||||||
return sendThrottle(throttle);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Sends a valid throttle value
|
|
||||||
bool DShotRMT::sendThrottle(uint16_t throttle)
|
bool DShotRMT::sendThrottle(uint16_t throttle)
|
||||||
{
|
{
|
||||||
// Validate throttle value
|
static uint16_t last_throttle = DSHOT_CMD_MOTOR_STOP;
|
||||||
if (throttle < DSHOT_THROTTLE_MIN || throttle > DSHOT_THROTTLE_MAX)
|
|
||||||
|
// Special case: if throttle is 0, use sendCommand() instead
|
||||||
|
if (throttle == 0)
|
||||||
{
|
{
|
||||||
Serial.println(DSHOT_MSG_05);
|
return sendCommand(DSHOT_CMD_MOTOR_STOP);
|
||||||
return DSHOT_ERROR;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Constrain throttle value
|
// Log only if throttle is out of range and different from last time (tricky little thing)
|
||||||
auto value = constrain(throttle, DSHOT_THROTTLE_MIN, DSHOT_THROTTLE_MAX);
|
if ((throttle < DSHOT_THROTTLE_MIN || throttle > DSHOT_THROTTLE_MAX) && throttle != last_throttle)
|
||||||
|
{
|
||||||
|
_dshot_log(THROTTLE_NOT_IN_RANGE);
|
||||||
|
}
|
||||||
|
|
||||||
// Build and send packet
|
// Always store the original throttle value (good one)
|
||||||
_packet = _buildDShotPacket(value);
|
last_throttle = throttle;
|
||||||
|
|
||||||
|
// Constrain throttle for transmission and send
|
||||||
|
uint16_t transmission_throttle = constrain(throttle, DSHOT_THROTTLE_MIN, DSHOT_THROTTLE_MAX);
|
||||||
|
_packet = _buildDShotPacket(transmission_throttle);
|
||||||
return _sendDShotFrame(_packet);
|
return _sendDShotFrame(_packet);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Deprecated, use "sendCommand()"" instead
|
// Send DShot command to ESC
|
||||||
bool DShotRMT::sendDShotCommand(uint16_t command)
|
|
||||||
{
|
|
||||||
return sendCommand(command);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool DShotRMT::sendCommand(uint16_t command)
|
bool DShotRMT::sendCommand(uint16_t command)
|
||||||
{
|
{
|
||||||
// Check for valid command
|
// Validate command is within DShot specification range
|
||||||
if (command < DSHOT_CMD_MOTOR_STOP || command > DSHOT_CMD_MAX)
|
if (command < DSHOT_CMD_MOTOR_STOP || command > DSHOT_CMD_MAX)
|
||||||
{
|
{
|
||||||
Serial.println(DSHOT_MSG_06);
|
_dshot_log(COMMAND_NOT_VALID);
|
||||||
return DSHOT_ERROR;
|
return DSHOT_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Build packet and transmit
|
||||||
_packet = _buildDShotPacket(command);
|
_packet = _buildDShotPacket(command);
|
||||||
return _sendDShotFrame(_packet);
|
return _sendDShotFrame(_packet);
|
||||||
}
|
}
|
||||||
|
|
||||||
//
|
// Get RPM from ESC (bidirectional mode only)
|
||||||
uint16_t DShotRMT::getERPM()
|
uint16_t DShotRMT::getERPM()
|
||||||
{
|
{
|
||||||
|
// Check if bidirectional mode is enabled
|
||||||
if (!_is_bidirectional || !_rmt_rx_channel)
|
if (!_is_bidirectional || !_rmt_rx_channel)
|
||||||
{
|
{
|
||||||
Serial.println(DSHOT_MSG_07);
|
_dshot_log(BIDIR_NOT_ENABLED);
|
||||||
return _last_erpm;
|
return _last_erpm; // Return cached value
|
||||||
}
|
}
|
||||||
|
|
||||||
// Try to receive telemetry data
|
// Attempt to receive telemetry data
|
||||||
if (!rmt_receive(_rmt_rx_channel, _rx_symbols, DSHOT_SYMBOLS_SIZE, &_receive_config))
|
if (!rmt_receive(_rmt_rx_channel, _rx_symbols, TX_BUFFER_SIZE, &_receive_config))
|
||||||
{
|
{
|
||||||
Serial.println(DSHOT_MSG_08);
|
_dshot_log(RX_RMT_MODULE_ERROR);
|
||||||
return _last_erpm;
|
return _last_erpm;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Decodes the response
|
// Decode telemetry frame
|
||||||
uint16_t new_erpm = _decodeDShotFrame(_rx_symbols);
|
uint16_t new_erpm = _decodeDShotFrame(_rx_symbols);
|
||||||
if (new_erpm != 0)
|
if (new_erpm != 0)
|
||||||
{
|
{
|
||||||
|
|
@ -149,91 +147,96 @@ uint16_t DShotRMT::getERPM()
|
||||||
return _last_erpm;
|
return _last_erpm;
|
||||||
}
|
}
|
||||||
|
|
||||||
//
|
// The actual motor rpm
|
||||||
uint32_t DShotRMT::getMotorRPM(uint8_t magnet_count)
|
uint32_t DShotRMT::getMotorRPM(uint8_t magnet_count)
|
||||||
{
|
{
|
||||||
uint8_t pole_pairs = max(1, magnet_count / 2);
|
uint8_t pole_pairs = max(1, magnet_count / 2);
|
||||||
return getERPM() / pole_pairs;
|
return getERPM() / pole_pairs;
|
||||||
}
|
}
|
||||||
|
|
||||||
// --- RMT TX Config ---
|
// Initialize RMT TX channel
|
||||||
bool DShotRMT::_initTXChannel()
|
bool DShotRMT::_initTXChannel()
|
||||||
{
|
{
|
||||||
|
// Configure TX channel
|
||||||
_tx_channel_config.gpio_num = _gpio;
|
_tx_channel_config.gpio_num = _gpio;
|
||||||
_tx_channel_config.clk_src = DSHOT_CLOCK_SRC_DEFAULT;
|
_tx_channel_config.clk_src = DSHOT_CLOCK_SRC_DEFAULT;
|
||||||
_tx_channel_config.resolution_hz = DSHOT_RMT_RESOLUTION;
|
_tx_channel_config.resolution_hz = DSHOT_RMT_RESOLUTION;
|
||||||
_tx_channel_config.mem_block_symbols = DSHOT_SYMBOLS_SIZE;
|
_tx_channel_config.mem_block_symbols = TX_BUFFER_SIZE;
|
||||||
_tx_channel_config.trans_queue_depth = TX_BUFFER_SIZE;
|
_tx_channel_config.trans_queue_depth = RMT_BUFFER_SIZE;
|
||||||
|
|
||||||
// No loops, real time calculation for each frame
|
// Configure transmission
|
||||||
_transmit_config.loop_count = 0;
|
_transmit_config.loop_count = 0; // No automatic loops - real-time calculation
|
||||||
|
|
||||||
// ...it's a trap
|
// Telemetric Bit used as bidir flag
|
||||||
_transmit_config.flags.eot_level = _is_bidirectional ? 1 : 0;
|
_transmit_config.flags.eot_level = _is_bidirectional ? 1 : 0;
|
||||||
|
|
||||||
// Creates and activates RMT TX Channel
|
// Create RMT TX channel
|
||||||
if (rmt_new_tx_channel(&_tx_channel_config, &_rmt_tx_channel) != DSHOT_OK)
|
if (rmt_new_tx_channel(&_tx_channel_config, &_rmt_tx_channel) != DSHOT_OK)
|
||||||
{
|
{
|
||||||
Serial.println(DSHOT_MSG_01);
|
_dshot_log(TX_INIT_FAILED);
|
||||||
return DSHOT_ERROR;
|
return DSHOT_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
return (rmt_enable(_rmt_tx_channel) == DSHOT_OK);
|
return (rmt_enable(_rmt_tx_channel) == DSHOT_OK);
|
||||||
}
|
}
|
||||||
|
|
||||||
// --- RMT RX Config ---
|
// Initialize RMT RX channel
|
||||||
bool DShotRMT::_initRXChannel()
|
bool DShotRMT::_initRXChannel()
|
||||||
{
|
{
|
||||||
|
// Configure RX channel parameters
|
||||||
_rx_channel_config.gpio_num = _gpio;
|
_rx_channel_config.gpio_num = _gpio;
|
||||||
_rx_channel_config.clk_src = DSHOT_CLOCK_SRC_DEFAULT;
|
_rx_channel_config.clk_src = DSHOT_CLOCK_SRC_DEFAULT;
|
||||||
_rx_channel_config.resolution_hz = DSHOT_RMT_RESOLUTION;
|
_rx_channel_config.resolution_hz = DSHOT_RMT_RESOLUTION;
|
||||||
_rx_channel_config.mem_block_symbols = DSHOT_SYMBOLS_SIZE;
|
_rx_channel_config.mem_block_symbols = RX_BUFFER_SIZE;
|
||||||
|
|
||||||
// TODO: figure out ranges
|
// Configure reception parameters (TODO: determine optimal ranges)
|
||||||
_receive_config.signal_range_min_ns = 2;
|
_receive_config.signal_range_min_ns = 2;
|
||||||
_receive_config.signal_range_max_ns = 128;
|
_receive_config.signal_range_max_ns = 128;
|
||||||
|
|
||||||
// Creates and activates RMT TX Channel
|
// Create RMT RX channel
|
||||||
if (rmt_new_rx_channel(&_rx_channel_config, &_rmt_rx_channel) != DSHOT_OK)
|
if (rmt_new_rx_channel(&_rx_channel_config, &_rmt_rx_channel) != DSHOT_OK)
|
||||||
{
|
{
|
||||||
Serial.println(DSHOT_MSG_02);
|
_dshot_log(RX_INIT_FAILED);
|
||||||
return DSHOT_ERROR;
|
return DSHOT_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
//
|
||||||
return (rmt_enable(_rmt_rx_channel) == DSHOT_OK);
|
return (rmt_enable(_rmt_rx_channel) == DSHOT_OK);
|
||||||
}
|
}
|
||||||
|
|
||||||
// --- RMT Encoder Config ---
|
// Initialize DShot encoder
|
||||||
bool DShotRMT::_initDShotEncoder()
|
bool DShotRMT::_initDShotEncoder()
|
||||||
{
|
{
|
||||||
// Encoder "config"
|
// Create copy encoder configuration
|
||||||
rmt_copy_encoder_config_t encoder_config = {};
|
rmt_copy_encoder_config_t encoder_config = {};
|
||||||
|
|
||||||
// Creates a dummy encoder
|
// Create encoder instance
|
||||||
if (rmt_new_copy_encoder(&encoder_config, &_dshot_encoder) != DSHOT_OK)
|
if (rmt_new_copy_encoder(&encoder_config, &_dshot_encoder) != DSHOT_OK)
|
||||||
{
|
{
|
||||||
Serial.println(DSHOT_MSG_03);
|
_dshot_log(ENCODER_INIT_FAILED);
|
||||||
return DSHOT_ERROR;
|
return DSHOT_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
return DSHOT_OK;
|
return DSHOT_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Uses RMT to transmit a prepared DShot packet and returns it
|
// Transmit DShot packet via RMT
|
||||||
bool DShotRMT::_sendDShotFrame(const dshot_packet_t &packet)
|
uint16_t DShotRMT::_sendDShotFrame(const dshot_packet_t &packet)
|
||||||
{
|
{
|
||||||
// Check if we can send (timing check)
|
// RMT is crazy fast
|
||||||
if (!_timer_signal())
|
if (!_timer_signal())
|
||||||
{
|
{
|
||||||
return DSHOT_ERROR;
|
return DSHOT_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Encode the frame
|
// Encode DShot packet into RMT symbols
|
||||||
_encodeDShotFrame(packet, _tx_symbols);
|
_encodeDShotFrame(packet, _tx_symbols);
|
||||||
|
|
||||||
// Attempt to transmit
|
// Calculate transmission data size
|
||||||
size_t tx_size_bytes = DSHOT_BITS_PER_FRAME * sizeof(rmt_symbol_word_t);
|
size_t tx_size_bytes = DSHOT_BITS_PER_FRAME * sizeof(rmt_symbol_word_t);
|
||||||
bool result = rmt_transmit(_rmt_tx_channel, _dshot_encoder, _tx_symbols, tx_size_bytes, &_transmit_config);
|
|
||||||
|
// Actual RMT transmission
|
||||||
|
uint16_t result = rmt_transmit(_rmt_tx_channel, _dshot_encoder, _tx_symbols, tx_size_bytes, &_transmit_config);
|
||||||
|
|
||||||
if (result != DSHOT_OK)
|
if (result != DSHOT_OK)
|
||||||
{
|
{
|
||||||
|
|
@ -246,13 +249,16 @@ bool DShotRMT::_sendDShotFrame(const dshot_packet_t &packet)
|
||||||
return DSHOT_OK;
|
return DSHOT_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Calculates checksum for given package
|
// Calculate CRC
|
||||||
uint16_t DShotRMT::_calculateCRC(const dshot_packet_t &packet)
|
uint16_t DShotRMT::_calculateCRC(const dshot_packet_t &packet)
|
||||||
{
|
{
|
||||||
|
//
|
||||||
uint16_t data = (packet.throttle_value << 1) | packet.telemetric_request;
|
uint16_t data = (packet.throttle_value << 1) | packet.telemetric_request;
|
||||||
|
|
||||||
|
// DShot CRC calculation
|
||||||
uint16_t crc = (data ^ (data >> 4) ^ (data >> 8)) & 0b0000000000001111;
|
uint16_t crc = (data ^ (data >> 4) ^ (data >> 8)) & 0b0000000000001111;
|
||||||
|
|
||||||
// Inverts CRC for bidirectional DShot
|
// Invert CRC for bidirectional DShot mode
|
||||||
if (_is_bidirectional)
|
if (_is_bidirectional)
|
||||||
{
|
{
|
||||||
crc = (~crc) & 0b0000000000001111;
|
crc = (~crc) & 0b0000000000001111;
|
||||||
|
|
@ -261,41 +267,45 @@ uint16_t DShotRMT::_calculateCRC(const dshot_packet_t &packet)
|
||||||
return crc;
|
return crc;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Returns bitwise parsed DShot packet
|
// Parse DShot packet into 16-bit format
|
||||||
uint16_t DShotRMT::_parseDShotPacket(const dshot_packet_t &packet)
|
uint16_t DShotRMT::_parseDShotPacket(const dshot_packet_t &packet)
|
||||||
{
|
{
|
||||||
|
//
|
||||||
uint16_t data = (packet.throttle_value << 1) | packet.telemetric_request;
|
uint16_t data = (packet.throttle_value << 1) | packet.telemetric_request;
|
||||||
|
|
||||||
|
// Add CRC checksum
|
||||||
return (data << 4) | _calculateCRC(packet);
|
return (data << 4) | _calculateCRC(packet);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Returns a "true" DShot Packet ready to roll
|
// Build a complete DShot packet
|
||||||
dshot_packet_t DShotRMT::_buildDShotPacket(const uint16_t value)
|
dshot_packet_t DShotRMT::_buildDShotPacket(const uint16_t value)
|
||||||
{
|
{
|
||||||
// DShot Frame Container
|
// Initialize packet structure
|
||||||
dshot_packet_t packet = {};
|
dshot_packet_t packet = {};
|
||||||
|
|
||||||
// Creates DShot packet
|
// Build packet
|
||||||
packet.throttle_value = value;
|
packet.throttle_value = value;
|
||||||
packet.telemetric_request = _is_bidirectional ? 1 : 0;
|
packet.telemetric_request = _is_bidirectional ? 1 : 0;
|
||||||
packet.checksum = _calculateCRC(packet);
|
packet.checksum = _calculateCRC(packet);
|
||||||
|
|
||||||
//
|
|
||||||
return packet;
|
return packet;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Encodes DShot packet into RMT buffer and places code into IRAM instead of flash
|
// Encode DShot packet into RMT symbol format (placed in IRAM for performance)
|
||||||
bool DShotRMT::_encodeDShotFrame(const dshot_packet_t &packet, rmt_symbol_word_t *symbols)
|
bool DShotRMT::_encodeDShotFrame(const dshot_packet_t &packet, rmt_symbol_word_t *symbols)
|
||||||
{
|
{
|
||||||
// Parse actual packet into buffer
|
// Parse packet
|
||||||
_current_packet = _parseDShotPacket(packet);
|
_parsed_packet = _parseDShotPacket(packet);
|
||||||
|
|
||||||
// Converts the parsed dshot frame to rmt_tx data
|
// Convert each bit to RMT symbol
|
||||||
for (int i = 0; i < DSHOT_BITS_PER_FRAME; i++)
|
for (int i = 0; i < DSHOT_BITS_PER_FRAME; i++)
|
||||||
{
|
{
|
||||||
// Encoded RMT symbols bitwise (MSB first) - tricky
|
// Extract bit from packet
|
||||||
bool bit = (_current_packet >> (DSHOT_BITS_PER_FRAME - 1 - i)) & 0b0000000000000001;
|
bool bit = (_parsed_packet >> (DSHOT_BITS_PER_FRAME - 1 - i)) & 0b0000000000000001;
|
||||||
|
|
||||||
if (_is_bidirectional)
|
if (_is_bidirectional)
|
||||||
{
|
{
|
||||||
|
// Bidirectional DShot uses inverted levels - Idle HIGH
|
||||||
symbols[i].level0 = 0;
|
symbols[i].level0 = 0;
|
||||||
symbols[i].duration0 = bit ? _timing_config.ticks_one_high : _timing_config.ticks_zero_high;
|
symbols[i].duration0 = bit ? _timing_config.ticks_one_high : _timing_config.ticks_zero_high;
|
||||||
symbols[i].level1 = 1;
|
symbols[i].level1 = 1;
|
||||||
|
|
@ -303,72 +313,139 @@ bool DShotRMT::_encodeDShotFrame(const dshot_packet_t &packet, rmt_symbol_word_t
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
|
// Standard DShot levels - Idle LOW
|
||||||
symbols[i].level0 = 1;
|
symbols[i].level0 = 1;
|
||||||
symbols[i].duration0 = bit ? _timing_config.ticks_one_high : _timing_config.ticks_zero_high;
|
symbols[i].duration0 = bit ? _timing_config.ticks_one_high : _timing_config.ticks_zero_high;
|
||||||
symbols[i].level1 = 0;
|
symbols[i].level1 = 0;
|
||||||
symbols[i].duration1 = bit ? _timing_config.ticks_one_low : _timing_config.ticks_zero_low;
|
symbols[i].duration1 = bit ? _timing_config.ticks_one_low : _timing_config.ticks_zero_low;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return DSHOT_OK;
|
return DSHOT_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
//
|
// Decode received RMT symbols
|
||||||
uint16_t DShotRMT::_decodeDShotFrame(const rmt_symbol_word_t *symbols)
|
uint16_t DShotRMT::_decodeDShotFrame(const rmt_symbol_word_t *symbols)
|
||||||
{
|
{
|
||||||
uint16_t received_frame = 0;
|
uint16_t received_frame = 0;
|
||||||
|
|
||||||
// Decodes each symbol to reconstruct the frame
|
// Reconstruct frame from RMT symbols
|
||||||
for (size_t i = 0; i < DSHOT_BITS_PER_FRAME; ++i)
|
for (size_t i = 0; i < DSHOT_BITS_PER_FRAME; ++i)
|
||||||
{
|
{
|
||||||
|
// Determine bit value based on pulse duration comparison
|
||||||
bool bit = symbols[i].duration0 > symbols[i].duration1;
|
bool bit = symbols[i].duration0 > symbols[i].duration1;
|
||||||
received_frame = (received_frame << 1) | bit;
|
received_frame = (received_frame << 1) | bit;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Extracts payload and CRC
|
// Extract data from received frame
|
||||||
uint16_t data = received_frame >> 4;
|
uint16_t data = received_frame >> 4;
|
||||||
uint16_t received_crc = received_frame & 0b0000000000001111;
|
uint16_t received_crc = received_frame & 0b0000000000001111;
|
||||||
|
|
||||||
// Calculates CRC for received frame
|
// Calculate expected CRC
|
||||||
uint16_t calculated_crc = (data ^ (data >> 4) ^ (data >> 8)) & 0b0000000000001111;
|
uint16_t calculated_crc = (data ^ (data >> 4) ^ (data >> 8)) & 0b0000000000001111;
|
||||||
if (_is_bidirectional)
|
if (_is_bidirectional)
|
||||||
{
|
{
|
||||||
calculated_crc = (~calculated_crc) & 0b0000000000001111;
|
calculated_crc = (~calculated_crc) & 0b0000000000001111; // Invert for bidirectional
|
||||||
}
|
}
|
||||||
|
|
||||||
// Compares CRC
|
// Validate CRC
|
||||||
if (received_crc != calculated_crc)
|
if (received_crc != calculated_crc)
|
||||||
{
|
{
|
||||||
Serial.println(DSHOT_MSG_04);
|
_dshot_log(CRC_CHECK_FAILED);
|
||||||
return 0b0000000000000000;
|
return DSHOT_NULL_PACKET;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Removes telemetry bit and returns 10bit value
|
// Remove telemetry bit and return 10-bit value
|
||||||
return data >> 1;
|
return data >> 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
//
|
// Print timing diagnostic information to specified stream (default Serial0)
|
||||||
void DShotRMT::printTimingDiagnostics() const
|
void DShotRMT::printDshotInfo(Stream &output) const
|
||||||
{
|
{
|
||||||
uint32_t current_time = micros();
|
output.println(NEW_LINE);
|
||||||
Serial.println("\n=== DShot Timing Diagnostics ===");
|
output.println(" === DShot Signal Info === ");
|
||||||
Serial.printf("Current mode: DSHOT%d\n", _mode == DSHOT150 ? 150 : _mode == DSHOT300 ? 300
|
|
||||||
: _mode == DSHOT600 ? 600
|
// Current DShot mode
|
||||||
: _mode == DSHOT1200);
|
output.printf("Current Mode: DSHOT%d\n",
|
||||||
Serial.printf("Protocol Frame length: %u µs\n", _timing_config.frame_length_us);
|
_mode == DSHOT150 ? 150 :
|
||||||
Serial.printf("Frame to Frame: %u µs\n", _frame_timer_us);
|
_mode == DSHOT300 ? 300 :
|
||||||
Serial.printf("Bidirectional: %s\n", _is_bidirectional ? "Yes" : "No");
|
_mode == DSHOT600 ? 600 :
|
||||||
|
_mode == DSHOT1200 ? 1200 : 0);
|
||||||
|
output.printf("Bidirectional: %s\n", _is_bidirectional ? "YES" : "NO");
|
||||||
|
|
||||||
|
// Timing Info
|
||||||
|
output.printf("Frame Length: %u us\n", _timing_config.frame_length_us);
|
||||||
|
|
||||||
|
// Packet Info
|
||||||
|
output.printf("Current Packet: ");
|
||||||
|
|
||||||
|
// Print bit by bit
|
||||||
|
for (int i = 15; i >= 0; --i)
|
||||||
|
{
|
||||||
|
if ((_parsed_packet >> i) & 1)
|
||||||
|
{
|
||||||
|
output.print("1");
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
output.print("0");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
output.printf("\n");
|
||||||
|
|
||||||
|
output.printf("Current Value: %u\n", _packet.throttle_value);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Timer triggered
|
// CPU Info
|
||||||
|
void DShotRMT::printCpuInfo(Stream &output) const
|
||||||
|
{
|
||||||
|
output.println(NEW_LINE);
|
||||||
|
output.println(" === CPU Info === ");
|
||||||
|
output.printf("Chip Model: %s\n", ESP.getChipModel());
|
||||||
|
output.printf("Chip Revision: %d\n", ESP.getChipRevision());
|
||||||
|
output.printf("CPU Freq = %lu MHz\n", ESP.getCpuFreqMHz());
|
||||||
|
output.printf("XTAL Freq = %lu MHz\n", getXtalFrequencyMhz());
|
||||||
|
output.printf("APB Freq = %lu Hz\n", getApbFrequency());
|
||||||
|
}
|
||||||
|
|
||||||
|
void DShotRMT::printDebugStream(Stream &output) const
|
||||||
|
{
|
||||||
|
// Debug Values as a list
|
||||||
|
output.print(_mode);
|
||||||
|
output.print(",");
|
||||||
|
output.print(_is_bidirectional);
|
||||||
|
output.print(",");
|
||||||
|
output.print(_packet.throttle_value);
|
||||||
|
output.print(",");
|
||||||
|
// The packet bitwise
|
||||||
|
for (int i = 15; i >= 0; --i)
|
||||||
|
{
|
||||||
|
if ((_parsed_packet >> i) & 1)
|
||||||
|
{
|
||||||
|
output.print("1");
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
output.print("0");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
output.print("*/");
|
||||||
|
output.print("\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
// Check if enough time has passed for next transmission
|
||||||
bool DShotRMT::_timer_signal()
|
bool DShotRMT::_timer_signal()
|
||||||
{
|
{
|
||||||
// fixing possible overflow
|
//
|
||||||
uint32_t current_time = micros();
|
uint32_t current_time = micros();
|
||||||
|
|
||||||
|
// Handle potential overflow
|
||||||
uint32_t elapsed = current_time - _last_transmission_time;
|
uint32_t elapsed = current_time - _last_transmission_time;
|
||||||
|
|
||||||
return elapsed >= _frame_timer_us;
|
return elapsed >= _frame_timer_us;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Updates timestamp
|
// Reset transmission timer to current time
|
||||||
bool DShotRMT::_timer_reset()
|
bool DShotRMT::_timer_reset()
|
||||||
{
|
{
|
||||||
_last_transmission_time = micros();
|
_last_transmission_time = micros();
|
||||||
|
|
|
||||||
129
DShotRMT.h
129
DShotRMT.h
|
|
@ -14,23 +14,24 @@
|
||||||
#include <driver/rmt_tx.h>
|
#include <driver/rmt_tx.h>
|
||||||
#include <driver/rmt_rx.h>
|
#include <driver/rmt_rx.h>
|
||||||
|
|
||||||
// --- DShot Protocol Constants ---
|
// DShot Protocol Constants
|
||||||
constexpr auto DSHOT_THROTTLE_FAILSAFE = 0;
|
constexpr auto DSHOT_THROTTLE_FAILSAFE = 0;
|
||||||
constexpr auto DSHOT_THROTTLE_MIN = 48;
|
constexpr auto DSHOT_THROTTLE_MIN = 48;
|
||||||
constexpr auto DSHOT_THROTTLE_MAX = 2047;
|
constexpr auto DSHOT_THROTTLE_MAX = 2047;
|
||||||
|
|
||||||
constexpr auto DSHOT_BITS_PER_FRAME = 16;
|
constexpr auto DSHOT_BITS_PER_FRAME = 16;
|
||||||
constexpr auto DSHOT_SWITCH_TIME = 30; // 30us
|
constexpr auto DSHOT_SWITCH_TIME = 30;
|
||||||
constexpr auto DSHOT_NULL_PACKET = 0b0000000000000000;
|
constexpr auto DSHOT_NULL_PACKET = 0b0000000000000000;
|
||||||
|
|
||||||
// --- RMT Config Constants ---
|
// RMT Configuration Constants
|
||||||
constexpr auto DSHOT_CLOCK_SRC_DEFAULT = RMT_CLK_SRC_DEFAULT;
|
constexpr auto DSHOT_CLOCK_SRC_DEFAULT = RMT_CLK_SRC_DEFAULT;
|
||||||
constexpr auto DSHOT_RMT_RESOLUTION = 10 * 1000 * 1000; // 10 MHz
|
constexpr auto DSHOT_RMT_RESOLUTION = 10 * 1000 * 1000; // 10 MHz resolution
|
||||||
constexpr auto TX_BUFFER_SIZE = DSHOT_BITS_PER_FRAME;
|
constexpr auto RMT_BUFFER_SIZE = DSHOT_BITS_PER_FRAME;
|
||||||
constexpr auto RX_BUFFER_SIZE = 128;
|
|
||||||
constexpr auto DSHOT_SYMBOLS_SIZE = 64;
|
|
||||||
|
|
||||||
// --- DShot Mode Select ---
|
constexpr auto RX_BUFFER_SIZE = 128;
|
||||||
|
constexpr auto TX_BUFFER_SIZE = 64;
|
||||||
|
|
||||||
|
// DShot Mode
|
||||||
typedef enum dshot_mode_e
|
typedef enum dshot_mode_e
|
||||||
{
|
{
|
||||||
DSHOT_OFF,
|
DSHOT_OFF,
|
||||||
|
|
@ -40,15 +41,15 @@ typedef enum dshot_mode_e
|
||||||
DSHOT1200
|
DSHOT1200
|
||||||
} dshot_mode_t;
|
} dshot_mode_t;
|
||||||
|
|
||||||
// --- DShot Packet Structure ---
|
// DShot Packet Structure
|
||||||
typedef struct dshot_packet_s
|
typedef struct dshot_packet_s
|
||||||
{
|
{
|
||||||
uint16_t throttle_value : 11;
|
uint16_t throttle_value : 11; // 11-bit throttle value
|
||||||
bool telemetric_request : 1;
|
uint16_t telemetric_request : 1; // Telemetry request bit
|
||||||
uint16_t checksum : 4;
|
uint16_t checksum : 4; // 4-bit CRC checksum
|
||||||
} dshot_packet_t;
|
} dshot_packet_t;
|
||||||
|
|
||||||
// --- DShot Timing Config ---
|
// DShot Timing Config Structure
|
||||||
typedef struct dshot_timing_s
|
typedef struct dshot_timing_s
|
||||||
{
|
{
|
||||||
uint32_t frame_length_us;
|
uint32_t frame_length_us;
|
||||||
|
|
@ -59,102 +60,112 @@ typedef struct dshot_timing_s
|
||||||
uint16_t ticks_zero_low;
|
uint16_t ticks_zero_low;
|
||||||
} dshot_timing_t;
|
} dshot_timing_t;
|
||||||
|
|
||||||
// Some typedef magic for DShot timing config
|
// Timing config for DShot modes
|
||||||
extern const dshot_timing_t DSHOT_TIMINGS[];
|
extern const dshot_timing_t DSHOT_TIMINGS[];
|
||||||
|
|
||||||
// --- DShotRMT Class ---
|
// DShotRMT Class
|
||||||
class DShotRMT
|
class DShotRMT
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
// --- DShot Config ---
|
// Primary constructor with GPIO enum
|
||||||
explicit DShotRMT(gpio_num_t gpio = GPIO_NUM_16, dshot_mode_t mode = DSHOT300, bool is_bidirectional = false);
|
explicit DShotRMT(gpio_num_t gpio = GPIO_NUM_16,
|
||||||
|
dshot_mode_t mode = DSHOT300,
|
||||||
|
bool is_bidirectional = false);
|
||||||
|
|
||||||
|
// Alternative constructor with pin number
|
||||||
DShotRMT(uint16_t pin_nr, dshot_mode_t mode, bool is_bidirectional);
|
DShotRMT(uint16_t pin_nr, dshot_mode_t mode, bool is_bidirectional);
|
||||||
|
|
||||||
// --- Init RMT Module ---
|
// Initialize the RMT module and DShot config
|
||||||
uint16_t begin();
|
uint16_t begin();
|
||||||
|
|
||||||
// Sets the throttle value and transmits
|
// Send throttle value (48-2047)
|
||||||
[[deprecated("Use sendThrottle() instead")]]
|
[[deprecated("Use sendThrottle() instead")]]
|
||||||
bool setThrottle(uint16_t throttle);
|
bool setThrottle(uint16_t throttle) { return sendThrottle(throttle); };
|
||||||
|
|
||||||
bool sendThrottle(uint16_t throttle);
|
bool sendThrottle(uint16_t throttle);
|
||||||
|
|
||||||
// Sends a DShot Command
|
// Send DShot command (0-47)
|
||||||
bool sendDShotCommand(uint16_t command); // deprecated
|
[[deprecated("Use sendCommand() instead")]]
|
||||||
|
bool sendDShotCommand(uint16_t command) { return sendCommand(command); };
|
||||||
bool sendCommand(uint16_t command);
|
bool sendCommand(uint16_t command);
|
||||||
|
|
||||||
// Gets eRPM from ESC telemetry
|
// Get telemetry data (bidirectional mode only)
|
||||||
uint16_t getERPM();
|
uint16_t getERPM();
|
||||||
|
uint32_t getMotorRPM(uint8_t magnet_count); // Convert eRPM to motor RPM
|
||||||
|
|
||||||
// Converts eRPM to motor RPM
|
// Tools
|
||||||
uint32_t getMotorRPM(uint8_t magnet_count);
|
gpio_num_t getGPIO() const { return _gpio; } // Get GPIO pin
|
||||||
|
uint16_t getDShotPacket() const { return _parsed_packet; } // Get raw packet
|
||||||
|
bool is_bidirectional() const { return _is_bidirectional; } // Check if bidirectional
|
||||||
|
|
||||||
// Returns pin number
|
// Print DShot Info
|
||||||
uint16_t getGPIO() const { return _gpio; }
|
void printDshotInfo(Stream &output = Serial0) const;
|
||||||
|
void printCpuInfo(Stream &output = Serial0) const;
|
||||||
|
|
||||||
// Debug: returns "raw" Dshot packet sent by RMT
|
// Prints debug values stream
|
||||||
uint16_t getDShotPacket() const { return _current_packet; }
|
void printDebugStream(Stream &output = Serial0) const;
|
||||||
|
|
||||||
//
|
|
||||||
bool is_bidirectional() const { return _is_bidirectional; }
|
|
||||||
|
|
||||||
// --- Performance monitoring functions ---
|
|
||||||
void printTimingDiagnostics() const;
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// --- Config ---
|
// Configuration Variables
|
||||||
gpio_num_t _gpio;
|
gpio_num_t _gpio;
|
||||||
dshot_mode_t _mode;
|
dshot_mode_t _mode;
|
||||||
bool _is_bidirectional;
|
uint16_t _is_bidirectional;
|
||||||
uint32_t _frame_timer_us;
|
uint32_t _frame_timer_us;
|
||||||
|
|
||||||
// --- DShot Timings ---
|
|
||||||
const dshot_timing_t &_timing_config;
|
const dshot_timing_t &_timing_config;
|
||||||
|
|
||||||
// --- RMT Handles ---
|
// RMT Config
|
||||||
rmt_channel_handle_t _rmt_tx_channel;
|
rmt_channel_handle_t _rmt_tx_channel;
|
||||||
rmt_channel_handle_t _rmt_rx_channel;
|
rmt_channel_handle_t _rmt_rx_channel;
|
||||||
rmt_encoder_handle_t _dshot_encoder;
|
rmt_encoder_handle_t _dshot_encoder;
|
||||||
|
|
||||||
// --- RMT Config ---
|
// RMT Config Structures
|
||||||
rmt_symbol_word_t _tx_symbols[DSHOT_SYMBOLS_SIZE];
|
rmt_symbol_word_t _tx_symbols[TX_BUFFER_SIZE];
|
||||||
rmt_symbol_word_t _rx_symbols[RX_BUFFER_SIZE];
|
rmt_symbol_word_t _rx_symbols[RX_BUFFER_SIZE];
|
||||||
rmt_tx_channel_config_t _tx_channel_config;
|
rmt_tx_channel_config_t _tx_channel_config;
|
||||||
rmt_rx_channel_config_t _rx_channel_config;
|
rmt_rx_channel_config_t _rx_channel_config;
|
||||||
rmt_transmit_config_t _transmit_config;
|
rmt_transmit_config_t _transmit_config;
|
||||||
rmt_receive_config_t _receive_config;
|
rmt_receive_config_t _receive_config;
|
||||||
|
|
||||||
// --- Buffers ---
|
//
|
||||||
uint16_t _last_erpm;
|
uint16_t _last_erpm;
|
||||||
uint16_t _current_packet;
|
uint16_t _parsed_packet;
|
||||||
dshot_packet_t _packet;
|
dshot_packet_t _packet;
|
||||||
uint32_t _last_transmission_time;
|
uint32_t _last_transmission_time;
|
||||||
|
|
||||||
// ---Helpers ---
|
// Helpers
|
||||||
bool _initTXChannel();
|
bool _initTXChannel();
|
||||||
bool _initRXChannel();
|
bool _initRXChannel();
|
||||||
bool _initDShotEncoder();
|
bool _initDShotEncoder();
|
||||||
|
|
||||||
bool _sendDShotFrame(const dshot_packet_t &packet);
|
// Utilizing RMT
|
||||||
|
uint16_t _sendDShotFrame(const dshot_packet_t &packet);
|
||||||
|
|
||||||
|
// Packet management
|
||||||
uint16_t _calculateCRC(const dshot_packet_t &packet);
|
uint16_t _calculateCRC(const dshot_packet_t &packet);
|
||||||
dshot_packet_t _buildDShotPacket(const uint16_t value);
|
dshot_packet_t _buildDShotPacket(const uint16_t value);
|
||||||
uint16_t _parseDShotPacket(const dshot_packet_t &packet);
|
uint16_t _parseDShotPacket(const dshot_packet_t &packet);
|
||||||
|
|
||||||
|
// Frame processing
|
||||||
bool IRAM_ATTR _encodeDShotFrame(const dshot_packet_t &packet, rmt_symbol_word_t *symbols);
|
bool IRAM_ATTR _encodeDShotFrame(const dshot_packet_t &packet, rmt_symbol_word_t *symbols);
|
||||||
uint16_t _decodeDShotFrame(const rmt_symbol_word_t *symbols);
|
uint16_t _decodeDShotFrame(const rmt_symbol_word_t *symbols);
|
||||||
|
|
||||||
// --- Simple Timer ---
|
// Timer Config
|
||||||
bool IRAM_ATTR _timer_signal();
|
bool IRAM_ATTR _timer_signal();
|
||||||
bool _timer_reset();
|
bool _timer_reset();
|
||||||
|
|
||||||
// --- Error Handling ---
|
// DShot Messages
|
||||||
static constexpr auto DSHOT_OK = 0;
|
void _dshot_log(char *msg, Stream &output = Serial0) { output.println(msg); };
|
||||||
static constexpr auto DSHOT_ERROR = 1;
|
|
||||||
static constexpr auto *DSHOT_MSG_01 = "Failed to initialize TX channel!";
|
// Error Codes and Messages
|
||||||
static constexpr auto *DSHOT_MSG_02 = "Failed to initialize RX channel!";
|
static constexpr uint16_t DSHOT_OK = 0;
|
||||||
static constexpr auto *DSHOT_MSG_03 = "Failed to initialize DShot encoder!";
|
static constexpr uint16_t DSHOT_ERROR = 1;
|
||||||
static constexpr auto *DSHOT_MSG_04 = "RX CRC Check failed!";
|
|
||||||
static constexpr auto *DSHOT_MSG_05 = "Throttle value not in range (48 - 2047)!";
|
static constexpr char *NEW_LINE = " ";
|
||||||
static constexpr auto *DSHOT_MSG_06 = "Not a valid DShot Command (0 - 47)!";
|
static constexpr char *TX_INIT_FAILED = "Failed to initialize TX channel!";
|
||||||
static constexpr auto *DSHOT_MSG_07 = "Bidirectional DShot support not enabled!";
|
static constexpr char *RX_INIT_FAILED = "Failed to initialize RX channel!";
|
||||||
static constexpr auto *DSHOT_MSG_08 = "RX RMT module failure!";
|
static constexpr char *ENCODER_INIT_FAILED = "Failed to initialize DShot encoder!";
|
||||||
|
static constexpr char *CRC_CHECK_FAILED = "RX CRC Check failed!";
|
||||||
|
static constexpr char *THROTTLE_NOT_IN_RANGE = "Throttle value not in range (48 - 2047)!";
|
||||||
|
static constexpr char *COMMAND_NOT_VALID = "Not a valid DShot Command (0 - 47)!";
|
||||||
|
static constexpr char *BIDIR_NOT_ENABLED = "Bidirectional DShot support not enabled!";
|
||||||
|
static constexpr char *RX_RMT_MODULE_ERROR = "RX RMT module failure!";
|
||||||
};
|
};
|
||||||
|
|
|
||||||
|
|
@ -9,6 +9,9 @@
|
||||||
#include <Arduino.h>
|
#include <Arduino.h>
|
||||||
#include <DShotRMT.h>
|
#include <DShotRMT.h>
|
||||||
|
|
||||||
|
// Debug output
|
||||||
|
static constexpr auto DEBUG = false;
|
||||||
|
|
||||||
// USB serial port settings
|
// USB serial port settings
|
||||||
static constexpr auto &USB_SERIAL = Serial0;
|
static constexpr auto &USB_SERIAL = Serial0;
|
||||||
static constexpr auto USB_SERIAL_BAUD = 115200;
|
static constexpr auto USB_SERIAL_BAUD = 115200;
|
||||||
|
|
@ -26,7 +29,6 @@ static constexpr auto IS_BIDIRECTIONAL = false;
|
||||||
// Motor magnet count for RPM calculation
|
// Motor magnet count for RPM calculation
|
||||||
static constexpr auto MOTOR01_MAGNET_COUNT = 14;
|
static constexpr auto MOTOR01_MAGNET_COUNT = 14;
|
||||||
|
|
||||||
//
|
|
||||||
// Creates the motor instance
|
// Creates the motor instance
|
||||||
DShotRMT motor01(MOTOR01_PIN, DSHOT_MODE, IS_BIDIRECTIONAL);
|
DShotRMT motor01(MOTOR01_PIN, DSHOT_MODE, IS_BIDIRECTIONAL);
|
||||||
|
|
||||||
|
|
@ -39,10 +41,10 @@ void setup()
|
||||||
// Initializes DShot Signal
|
// Initializes DShot Signal
|
||||||
motor01.begin();
|
motor01.begin();
|
||||||
|
|
||||||
USB_SERIAL.printf("CPU Freq = %lu MHz\n", getCpuFrequencyMhz());
|
// Print CPU Info
|
||||||
USB_SERIAL.printf("XTAL Freq = %lu MHz\n", getXtalFrequencyMhz());
|
motor01.printCpuInfo();
|
||||||
USB_SERIAL.printf("APB Freq = %lu Hz\n", getApbFrequency());
|
|
||||||
|
|
||||||
|
USB_SERIAL.println(" ");
|
||||||
USB_SERIAL.println("***********************************");
|
USB_SERIAL.println("***********************************");
|
||||||
USB_SERIAL.println(" === DShotRMT Demo started. === ");
|
USB_SERIAL.println(" === DShotRMT Demo started. === ");
|
||||||
USB_SERIAL.println("Enter a throttle value (48 – 2047):");
|
USB_SERIAL.println("Enter a throttle value (48 – 2047):");
|
||||||
|
|
@ -51,75 +53,34 @@ void setup()
|
||||||
//
|
//
|
||||||
void loop()
|
void loop()
|
||||||
{
|
{
|
||||||
// Safety first: start with DSHOT_MIN_THROTTLE
|
/// ...safety first
|
||||||
static auto throttle = DSHOT_THROTTLE_MIN;
|
static uint16_t throttle = DSHOT_CMD_MOTOR_STOP;
|
||||||
|
|
||||||
// Performance monitoring
|
// Time Measurement
|
||||||
static uint32_t last_stats_print = 0;
|
static uint32_t last_stats_print = 0;
|
||||||
|
|
||||||
// Takes "every" throttle value
|
// Read throttle value
|
||||||
if (USB_SERIAL.available() > 0)
|
if (USB_SERIAL.available() > 0)
|
||||||
{
|
{
|
||||||
throttle = (USB_SERIAL.readStringUntil('\n').toInt());
|
throttle = USB_SERIAL.readStringUntil('\n').toInt();
|
||||||
|
|
||||||
USB_SERIAL.println("*********************");
|
|
||||||
USB_SERIAL.print("Throttle set to: ");
|
|
||||||
USB_SERIAL.println(throttle);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Sends the value to the ESC
|
// Send the current throttle value
|
||||||
motor01.sendThrottle(throttle);
|
motor01.sendThrottle(throttle);
|
||||||
|
|
||||||
// Prints out RPM if BiDirectional DShot is enabled every 2 seconds
|
// Debug output
|
||||||
// printRPMPeriodically(2000);
|
if (DEBUG)
|
||||||
|
{
|
||||||
|
motor01.printDebugStream();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
// Print performance statistics every 2 seconds
|
// Print motor stats every 2 seconds
|
||||||
if (millis() - last_stats_print >= 2000)
|
if (millis() - last_stats_print >= 2000)
|
||||||
{
|
{
|
||||||
motor01.printTimingDiagnostics();
|
motor01.printDshotInfo();
|
||||||
print_RMT_packet();
|
|
||||||
|
// Time Stamp
|
||||||
last_stats_print = millis();
|
last_stats_print = millis();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Prints RPM every X_ms
|
|
||||||
void printRPMPeriodically(auto timer_ms)
|
|
||||||
{
|
|
||||||
if (IS_BIDIRECTIONAL)
|
|
||||||
{
|
|
||||||
static auto last_print_time = 0;
|
|
||||||
|
|
||||||
if (millis() - last_print_time >= timer_ms)
|
|
||||||
{
|
|
||||||
auto rpm = motor01.getMotorRPM(MOTOR01_MAGNET_COUNT);
|
|
||||||
|
|
||||||
USB_SERIAL.print("RPM: ");
|
|
||||||
USB_SERIAL.println(rpm);
|
|
||||||
|
|
||||||
last_print_time = millis();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Prints "raw" packet every ms
|
|
||||||
void print_RMT_packet()
|
|
||||||
{
|
|
||||||
auto packet = motor01.getDShotPacket();
|
|
||||||
|
|
||||||
USB_SERIAL.print("Current Frame: ");
|
|
||||||
|
|
||||||
// Print bit by bit
|
|
||||||
for (int i = 15; i >= 0; --i)
|
|
||||||
{
|
|
||||||
if ((packet >> i) & 1)
|
|
||||||
{
|
|
||||||
USB_SERIAL.print("1");
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
USB_SERIAL.print("0");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
USB_SERIAL.println("");
|
|
||||||
}
|
|
||||||
|
|
|
||||||
|
|
@ -1,5 +1,5 @@
|
||||||
name=DShotRMT
|
name=DShotRMT
|
||||||
version=0.6.1
|
version=0.6.3
|
||||||
author=derdoktor667
|
author=derdoktor667
|
||||||
maintainer=derdoktor667
|
maintainer=derdoktor667
|
||||||
sentence=DShotRMT Library supporting all DShot Types and speeds. Tested with BlHeli_S.
|
sentence=DShotRMT Library supporting all DShot Types and speeds. Tested with BlHeli_S.
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue