parent
0e77a05039
commit
eaab163836
113
DShotRMT.cpp
113
DShotRMT.cpp
|
|
@ -40,8 +40,7 @@ DShotRMT::DShotRMT(gpio_num_t gpio, dshot_mode_t mode, bool is_bidirectional)
|
||||||
_tx_channel_config{},
|
_tx_channel_config{},
|
||||||
_rx_channel_config{},
|
_rx_channel_config{},
|
||||||
_transmit_config{},
|
_transmit_config{},
|
||||||
_receive_config{},
|
_receive_config{}
|
||||||
_result{false, UNKNOWN_ERROR}
|
|
||||||
{
|
{
|
||||||
// Calculate frame timing including switch/pause time
|
// Calculate frame timing including switch/pause time
|
||||||
_frame_timer_us = _timing_config.frame_length_us + DSHOT_PAUSE_US;
|
_frame_timer_us = _timing_config.frame_length_us + DSHOT_PAUSE_US;
|
||||||
|
|
@ -99,39 +98,31 @@ dshot_result_t DShotRMT::begin()
|
||||||
{
|
{
|
||||||
if (!_initRXChannel().success)
|
if (!_initRXChannel().success)
|
||||||
{
|
{
|
||||||
_result.msg = RX_INIT_FAILED;
|
return {false, RX_INIT_FAILED};
|
||||||
return _result;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Init TX channel
|
// Init TX channel
|
||||||
if (!_initTXChannel().success)
|
if (!_initTXChannel().success)
|
||||||
{
|
{
|
||||||
_result.msg = TX_INIT_FAILED;
|
return {false, TX_INIT_FAILED};
|
||||||
return _result;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Init DShot encoder
|
// Init DShot encoder
|
||||||
if (!_initDShotEncoder().success)
|
if (!_initDShotEncoder().success)
|
||||||
{
|
{
|
||||||
_result.msg = ENCODER_INIT_FAILED;
|
return {false, ENCODER_INIT_FAILED};
|
||||||
return _result;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Bit positions precalculation
|
// Bit positions precalculation
|
||||||
_preCalculateBitPositions();
|
_preCalculateBitPositions();
|
||||||
|
|
||||||
_result.success = true;
|
return {true, INIT_SUCCESS};
|
||||||
_result.msg = INIT_SUCCESS;
|
|
||||||
|
|
||||||
return _result;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Init RMT TX channel
|
// Init RMT TX channel
|
||||||
dshot_result_t DShotRMT::_initTXChannel()
|
dshot_result_t DShotRMT::_initTXChannel()
|
||||||
{
|
{
|
||||||
_result = {false, TX_INIT_FAILED};
|
|
||||||
|
|
||||||
// Configure TX channel
|
// 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;
|
||||||
|
|
@ -146,25 +137,21 @@ dshot_result_t DShotRMT::_initTXChannel()
|
||||||
// Create 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)
|
||||||
{
|
{
|
||||||
return _result;
|
return {false, TX_INIT_FAILED};
|
||||||
}
|
}
|
||||||
|
|
||||||
//
|
//
|
||||||
if (rmt_enable(_rmt_tx_channel) != DSHOT_OK)
|
if (rmt_enable(_rmt_tx_channel) != DSHOT_OK)
|
||||||
{
|
{
|
||||||
return _result;
|
return {false, TX_INIT_FAILED};
|
||||||
}
|
}
|
||||||
|
|
||||||
_result.success = true;
|
return {true, TX_INIT_SUCCESS};
|
||||||
_result.msg = TX_INIT_SUCCESS;
|
|
||||||
|
|
||||||
return _result;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Init RMT RX channel
|
// Init RMT RX channel
|
||||||
dshot_result_t DShotRMT::_initRXChannel()
|
dshot_result_t DShotRMT::_initRXChannel()
|
||||||
{
|
{
|
||||||
_result = {false, RX_INIT_FAILED};
|
|
||||||
|
|
||||||
// Direct RMT symbol processing - Performance optimized
|
// Direct RMT symbol processing - Performance optimized
|
||||||
_rx_event_callbacks.on_recv_done = _rmt_rx_done_callback;
|
_rx_event_callbacks.on_recv_done = _rmt_rx_done_callback;
|
||||||
|
|
@ -182,19 +169,16 @@ dshot_result_t DShotRMT::_initRXChannel()
|
||||||
// Create RMT RX 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)
|
||||||
{
|
{
|
||||||
return _result;
|
return {false, RX_INIT_FAILED};
|
||||||
}
|
}
|
||||||
|
|
||||||
//
|
//
|
||||||
if (rmt_enable(_rmt_rx_channel) != DSHOT_OK)
|
if (rmt_enable(_rmt_rx_channel) != DSHOT_OK)
|
||||||
{
|
{
|
||||||
return _result;
|
return {false, RX_INIT_FAILED};
|
||||||
}
|
}
|
||||||
|
|
||||||
_result.success = true;
|
return {true, RX_INIT_SUCCESS};
|
||||||
_result.msg = RX_INIT_SUCCESS;
|
|
||||||
|
|
||||||
return _result;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Callback for RMT RX
|
// Callback for RMT RX
|
||||||
|
|
@ -202,17 +186,16 @@ bool IRAM_ATTR DShotRMT::_rmt_rx_done_callback(rmt_channel_handle_t rmt_rx_chann
|
||||||
{
|
{
|
||||||
DShotRMT *instance = static_cast<DShotRMT *>(user_data);
|
DShotRMT *instance = static_cast<DShotRMT *>(user_data);
|
||||||
|
|
||||||
// Minimale ISR-Verarbeitung: Nur bei gültigen Daten
|
// ISR check for valid data
|
||||||
if (edata && edata->num_symbols >= GCR_BITS_PER_FRAME &&
|
if (edata && edata->num_symbols >= GCR_BITS_PER_FRAME && edata->num_symbols <= GCR_BITS_PER_FRAME)
|
||||||
edata->num_symbols <= GCR_BITS_PER_FRAME)
|
|
||||||
{
|
{
|
||||||
|
|
||||||
// Direkte Dekodierung in der ISR (schnell!)
|
// Direct decoding
|
||||||
uint16_t erpm = instance->_decodeDShotFrame(edata->received_symbols);
|
uint16_t erpm = instance->_decodeDShotFrame(edata->received_symbols);
|
||||||
|
|
||||||
if (erpm != DSHOT_NULL_PACKET)
|
if (erpm != DSHOT_NULL_PACKET)
|
||||||
{
|
{
|
||||||
// Atomic writes - thread-safe ohne Mutex
|
// Atomic writes - thread-safe
|
||||||
instance->_last_erpm_atomic = erpm;
|
instance->_last_erpm_atomic = erpm;
|
||||||
instance->_telemetry_ready_flag = true;
|
instance->_telemetry_ready_flag = true;
|
||||||
}
|
}
|
||||||
|
|
@ -224,21 +207,16 @@ bool IRAM_ATTR DShotRMT::_rmt_rx_done_callback(rmt_channel_handle_t rmt_rx_chann
|
||||||
// Initialize DShot encoder
|
// Initialize DShot encoder
|
||||||
dshot_result_t DShotRMT::_initDShotEncoder()
|
dshot_result_t DShotRMT::_initDShotEncoder()
|
||||||
{
|
{
|
||||||
_result = {false, ENCODER_INIT_FAILED};
|
|
||||||
|
|
||||||
// Create copy encoder configuration
|
// Create copy encoder configuration
|
||||||
rmt_copy_encoder_config_t encoder_config = {};
|
rmt_copy_encoder_config_t encoder_config = {};
|
||||||
|
|
||||||
// Create encoder instance
|
// 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)
|
||||||
{
|
{
|
||||||
return _result;
|
return {false, ENCODER_INIT_FAILED};
|
||||||
}
|
}
|
||||||
|
|
||||||
_result.success = true;
|
return {true, TX_INIT_SUCCESS};
|
||||||
_result.msg = ENCODER_INIT_SUCCESS;
|
|
||||||
|
|
||||||
return _result;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Send throttle value
|
// Send throttle value
|
||||||
|
|
@ -250,17 +228,12 @@ dshot_result_t DShotRMT::sendThrottle(uint16_t throttle)
|
||||||
return sendCommand(DSHOT_CMD_MOTOR_STOP);
|
return sendCommand(DSHOT_CMD_MOTOR_STOP);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Log only if throttle is out of range and different from last time
|
|
||||||
if ((throttle < DSHOT_THROTTLE_MIN || throttle > DSHOT_THROTTLE_MAX) && throttle != _last_throttle)
|
|
||||||
{
|
|
||||||
_result.msg = THROTTLE_NOT_IN_RANGE;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Always store the original throttle value
|
// Always store the original throttle value
|
||||||
_last_throttle = throttle;
|
_last_throttle = throttle;
|
||||||
|
|
||||||
// Constrain throttle for transmission and send
|
// Constrain throttle for transmission and send
|
||||||
uint16_t new_throttle = constrain(throttle, DSHOT_THROTTLE_MIN, DSHOT_THROTTLE_MAX);
|
uint16_t new_throttle = constrain(throttle, DSHOT_THROTTLE_MIN, DSHOT_THROTTLE_MAX);
|
||||||
|
|
||||||
_packet = _buildDShotPacket(new_throttle);
|
_packet = _buildDShotPacket(new_throttle);
|
||||||
|
|
||||||
return _sendDShotFrame(_packet);
|
return _sendDShotFrame(_packet);
|
||||||
|
|
@ -272,8 +245,7 @@ dshot_result_t DShotRMT::sendCommand(uint16_t command)
|
||||||
// Validate command is within DShot specification range
|
// 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)
|
||||||
{
|
{
|
||||||
_result.msg = COMMAND_NOT_VALID;
|
return {false, COMMAND_NOT_VALID};
|
||||||
return _result;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Build packet and transmit
|
// Build packet and transmit
|
||||||
|
|
@ -295,12 +267,14 @@ dshot_telemetry_result_t DShotRMT::getTelemetry(uint16_t magnet_count)
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
//
|
||||||
if (_telemetry_ready_flag)
|
if (_telemetry_ready_flag)
|
||||||
{
|
{
|
||||||
_telemetry_ready_flag = false;
|
_telemetry_ready_flag = false;
|
||||||
|
|
||||||
uint16_t erpm = _last_erpm_atomic;
|
uint16_t erpm = _last_erpm_atomic;
|
||||||
|
|
||||||
|
//
|
||||||
if (erpm != DSHOT_NULL_PACKET && magnet_count >= 1)
|
if (erpm != DSHOT_NULL_PACKET && magnet_count >= 1)
|
||||||
{
|
{
|
||||||
uint8_t pole_pairs = max(MIN_POLE_PAIRS, (magnet_count / MAGNETS_PER_POLE_PAIR));
|
uint8_t pole_pairs = max(MIN_POLE_PAIRS, (magnet_count / MAGNETS_PER_POLE_PAIR));
|
||||||
|
|
@ -378,25 +352,24 @@ void DShotRMT::_preCalculateBitPositions()
|
||||||
// Transmit DShot packet via RMT
|
// Transmit DShot packet via RMT
|
||||||
dshot_result_t DShotRMT::_sendDShotFrame(const dshot_packet_t &packet)
|
dshot_result_t DShotRMT::_sendDShotFrame(const dshot_packet_t &packet)
|
||||||
{
|
{
|
||||||
_result = {false, UNKNOWN_ERROR};
|
|
||||||
|
|
||||||
// Check timing requirements
|
// Check timing requirements
|
||||||
if (!_timer_signal())
|
if (!_timer_signal())
|
||||||
{
|
{
|
||||||
_result.msg = TIMING_CORRECTION;
|
return {false, TIMING_CORRECTION};
|
||||||
return _result;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Enable RMT RX before RMT TX
|
// Enable RMT RX before RMT TX
|
||||||
if (_is_bidirectional)
|
if (_is_bidirectional)
|
||||||
{
|
{
|
||||||
|
// Calculate transmission data size
|
||||||
|
size_t rx_size_bytes = GCR_BITS_PER_FRAME * sizeof(rmt_symbol_word_t);
|
||||||
|
|
||||||
// Performance reasons
|
// Performance reasons
|
||||||
rmt_symbol_word_t rx_symbols[DSHOT_BITS_PER_FRAME];
|
rmt_symbol_word_t rx_symbols[DSHOT_BITS_PER_FRAME];
|
||||||
|
|
||||||
if (rmt_receive(_rmt_rx_channel, rx_symbols, sizeof(rx_symbols), &_receive_config) != DSHOT_OK)
|
if (rmt_receive(_rmt_rx_channel, rx_symbols, rx_size_bytes, &_receive_config) != DSHOT_OK)
|
||||||
{
|
{
|
||||||
_result.msg = RECEIVER_FAILED;
|
return {false, RECEIVER_FAILED};
|
||||||
return _result;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -415,16 +388,14 @@ dshot_result_t DShotRMT::_sendDShotFrame(const dshot_packet_t &packet)
|
||||||
// Disable RMT RX for sending
|
// Disable RMT RX for sending
|
||||||
if (rmt_disable(_rmt_rx_channel) != DSHOT_OK)
|
if (rmt_disable(_rmt_rx_channel) != DSHOT_OK)
|
||||||
{
|
{
|
||||||
_result.msg = RECEIVER_FAILED;
|
return {false, RECEIVER_FAILED};
|
||||||
return _result;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Perform RMT transmission
|
// Perform RMT transmission
|
||||||
if (rmt_transmit(_rmt_tx_channel, _dshot_encoder, tx_symbols, tx_size_bytes, &_transmit_config) != DSHOT_OK)
|
if (rmt_transmit(_rmt_tx_channel, _dshot_encoder, tx_symbols, tx_size_bytes, &_transmit_config) != DSHOT_OK)
|
||||||
{
|
{
|
||||||
_result.msg = TRANSMISSION_FAILED;
|
return {false, TRANSMISSION_FAILED};
|
||||||
return _result;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Re-enable RMT RX
|
// Re-enable RMT RX
|
||||||
|
|
@ -432,18 +403,14 @@ dshot_result_t DShotRMT::_sendDShotFrame(const dshot_packet_t &packet)
|
||||||
{
|
{
|
||||||
if (rmt_enable(_rmt_rx_channel) != DSHOT_OK)
|
if (rmt_enable(_rmt_rx_channel) != DSHOT_OK)
|
||||||
{
|
{
|
||||||
_result.msg = RECEIVER_FAILED;
|
return {false, RECEIVER_FAILED};
|
||||||
return _result;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Update timestamp and calculate execution time
|
// Update timestamp and calculate execution time
|
||||||
_timer_reset();
|
_timer_reset();
|
||||||
|
|
||||||
_result.success = true;
|
return {true, TRANSMISSION_SUCCESS};
|
||||||
_result.msg = TRANSMISSION_SUCCESS;
|
|
||||||
|
|
||||||
return _result;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Encode DShot packet into RMT symbol format (placed in IRAM for performance)
|
// Encode DShot packet into RMT symbol format (placed in IRAM for performance)
|
||||||
|
|
@ -494,7 +461,7 @@ uint16_t DShotRMT::_decodeDShotFrame(const rmt_symbol_word_t *symbols)
|
||||||
// Masking CRC
|
// Masking CRC
|
||||||
uint16_t received_crc = data_and_crc & DSHOT_CRC_MASK;
|
uint16_t received_crc = data_and_crc & DSHOT_CRC_MASK;
|
||||||
|
|
||||||
// Telemetry request bit is always 1.
|
// Telemetry request bit has to be 1
|
||||||
if (!(received_data & (1 << 11)))
|
if (!(received_data & (1 << 11)))
|
||||||
{
|
{
|
||||||
return DSHOT_NULL_PACKET;
|
return DSHOT_NULL_PACKET;
|
||||||
|
|
@ -529,6 +496,7 @@ bool IRAM_ATTR DShotRMT::_timer_signal()
|
||||||
bool DShotRMT::_timer_reset()
|
bool DShotRMT::_timer_reset()
|
||||||
{
|
{
|
||||||
_last_transmission_time = esp_timer_get_time();
|
_last_transmission_time = esp_timer_get_time();
|
||||||
|
|
||||||
return DSHOT_OK;
|
return DSHOT_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -547,9 +515,6 @@ void DShotRMT::printDShotInfo(Stream &output) const
|
||||||
|
|
||||||
output.printf("Bidirectional: %s\n", _is_bidirectional ? "YES" : "NO");
|
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
|
// Packet Info
|
||||||
output.printf("Current Packet: ");
|
output.printf("Current Packet: ");
|
||||||
|
|
||||||
|
|
@ -585,15 +550,7 @@ void DShotRMT::printCpuInfo(Stream &output) const
|
||||||
// --- HELPERS ---
|
// --- HELPERS ---
|
||||||
void printDShotResult(dshot_result_t &result, Stream &output)
|
void printDShotResult(dshot_result_t &result, Stream &output)
|
||||||
{
|
{
|
||||||
if (result.success)
|
output.printf("Status: %s - %s\n", result.success ? "SUCCESS" : "FAILED", result.msg);
|
||||||
{
|
|
||||||
output.printf("Staus: SUCCESS - %s\n", result.msg);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
output.printf("Status: FAILED - %s\n", result.msg);
|
|
||||||
}
|
|
||||||
|
|
||||||
output.println(" ");
|
output.println(" ");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -602,7 +559,7 @@ void printDShotTelemetry(dshot_telemetry_result_t &result, Stream &output)
|
||||||
{
|
{
|
||||||
if (result.success)
|
if (result.success)
|
||||||
{
|
{
|
||||||
output.printf("Telemetry: eRPM=%u, Motor RPM=%u \n", result.erpm, result.motor_rpm);
|
output.printf("Telemetry: eRPM=%u, Motor RPM=%u\n", result.erpm, result.motor_rpm);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
|
|
|
||||||
|
|
@ -32,7 +32,7 @@ static constexpr auto NO_DSHOT_ERPM = 0;
|
||||||
static constexpr auto NO_DSHOT_RPM = 0;
|
static constexpr auto NO_DSHOT_RPM = 0;
|
||||||
|
|
||||||
// RMT Configuration Constants
|
// RMT Configuration Constants
|
||||||
constexpr auto DSHOT_CLOCK_SRC_DEFAULT = RMT_CLK_SRC_APB;
|
constexpr auto DSHOT_CLOCK_SRC_DEFAULT = RMT_CLK_SRC_DEFAULT;
|
||||||
constexpr auto DSHOT_RMT_RESOLUTION = 10 * 1000 * 1000; // 10 MHz resolution
|
constexpr auto DSHOT_RMT_RESOLUTION = 10 * 1000 * 1000; // 10 MHz resolution
|
||||||
constexpr auto RMT_BUFFER_SIZE = DSHOT_BITS_PER_FRAME;
|
constexpr auto RMT_BUFFER_SIZE = DSHOT_BITS_PER_FRAME;
|
||||||
constexpr auto RMT_BUFFER_SYMBOLS = 64;
|
constexpr auto RMT_BUFFER_SYMBOLS = 64;
|
||||||
|
|
@ -205,9 +205,6 @@ private:
|
||||||
volatile bool _telemetry_ready_flag;
|
volatile bool _telemetry_ready_flag;
|
||||||
static bool IRAM_ATTR _rmt_rx_done_callback(rmt_channel_handle_t rmt_rx_channel, const rmt_rx_done_event_data_t *edata, void *user_data);
|
static bool IRAM_ATTR _rmt_rx_done_callback(rmt_channel_handle_t rmt_rx_channel, const rmt_rx_done_event_data_t *edata, void *user_data);
|
||||||
|
|
||||||
// --- DSHOT RESULT HANDLER ---
|
|
||||||
dshot_result_t _result;
|
|
||||||
|
|
||||||
// --- DSHOT DEFAULTS ---
|
// --- DSHOT DEFAULTS ---
|
||||||
static constexpr auto const DSHOT_TELEMETRY_INVALID = (0xffff);
|
static constexpr auto const DSHOT_TELEMETRY_INVALID = (0xffff);
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -171,6 +171,7 @@ void handleSerialInput(const String &input, uint16_t &throttle, bool &continuous
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
|
USB_SERIAL.println(" ");
|
||||||
USB_SERIAL.printf("Invalid input: '%s'\n", input);
|
USB_SERIAL.printf("Invalid input: '%s'\n", input);
|
||||||
USB_SERIAL.printf("Valid throttle range: %d - %d\n", DSHOT_THROTTLE_MIN, DSHOT_THROTTLE_MAX);
|
USB_SERIAL.printf("Valid throttle range: %d - %d\n", DSHOT_THROTTLE_MIN, DSHOT_THROTTLE_MAX);
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue