From 4f731edaead51a644270d2bf183a8a43cae33454 Mon Sep 17 00:00:00 2001 From: franchioping Date: Mon, 13 Apr 2026 15:03:58 +0100 Subject: [PATCH] fix non dma on esp32-s3. hacky way shouldnt be used. TODO - fix underlying issue with lack of termination of signal --- src/DShotRMT.cpp | 950 +++++++++++++++++++++++---------------------- src/DShotRMT.h | 208 +++++----- src/dshot_init.cpp | 132 ++++--- 3 files changed, 675 insertions(+), 615 deletions(-) diff --git a/src/DShotRMT.cpp b/src/DShotRMT.cpp index aca0667..a69631d 100644 --- a/src/DShotRMT.cpp +++ b/src/DShotRMT.cpp @@ -7,593 +7,623 @@ */ #include "DShotRMT.h" +#include "esp32-hal-gpio.h" +#include "esp32-hal.h" +#include "esp_log.h" +#include #include +#include // Constructor with GPIO number -DShotRMT::DShotRMT(gpio_num_t gpio, dshot_mode_t mode, bool is_bidirectional, uint16_t magnet_count) - : _gpio(gpio), - _mode(mode), - _is_bidirectional(is_bidirectional), - _motor_magnet_count(magnet_count), - _dshot_timing(DSHOT_TIMING_US[_mode]) -{ - // Pre-calculate timing and ratios for performance - _preCalculateRMTTicks(); - _percent_to_throttle_ratio = (static_cast(DSHOT_THROTTLE_MAX - DSHOT_THROTTLE_MIN)) / DSHOT_PERCENT_MAX; +DShotRMT::DShotRMT(gpio_num_t gpio, dshot_mode_t mode, bool is_bidirectional, + uint16_t magnet_count) + : _gpio(gpio), _mode(mode), _is_bidirectional(is_bidirectional), + _motor_magnet_count(magnet_count), _dshot_timing(DSHOT_TIMING_US[_mode]) { + // Pre-calculate timing and ratios for performance + _preCalculateRMTTicks(); + _percent_to_throttle_ratio = + (static_cast(DSHOT_THROTTLE_MAX - DSHOT_THROTTLE_MIN)) / + DSHOT_PERCENT_MAX; } // Constructor using pin number -DShotRMT::DShotRMT(uint16_t pin_nr, dshot_mode_t mode, bool is_bidirectional, uint16_t magnet_count) - : DShotRMT(static_cast(pin_nr), mode, is_bidirectional, magnet_count) -{ - // Delegates to primary constructor with type cast +DShotRMT::DShotRMT(uint16_t pin_nr, dshot_mode_t mode, bool is_bidirectional, + uint16_t magnet_count) + : DShotRMT(static_cast(pin_nr), mode, is_bidirectional, + magnet_count) { + // Delegates to primary constructor with type cast } // Destructor -DShotRMT::~DShotRMT() -{ - _cleanupRmtResources(); -} +DShotRMT::~DShotRMT() { _cleanupRmtResources(); } // Initialize DShotRMT -dshot_result_t DShotRMT::begin() -{ - dshot_result_t result = init_rmt_tx_channel(_gpio, &_rmt_tx_channel, _is_bidirectional); +dshot_result_t DShotRMT::begin() { + _tx_payload = (uint16_t *)heap_caps_malloc(sizeof(uint16_t), + MALLOC_CAP_8BIT | MALLOC_CAP_DMA); + dshot_result_t result = + init_rmt_tx_channel(_gpio, &_rmt_tx_channel, _is_bidirectional); - if (!result.success) - { - _cleanupRmtResources(); // Clean up any allocated resources on failure - return result; + if (!result.success) { + _cleanupRmtResources(); // Clean up any allocated resources on failure + return result; + } + + if (_is_bidirectional) { + result = init_rmt_rx_channel(_gpio, &_rmt_rx_channel, &_rx_event_callbacks, + this); + if (!result.success) { + _cleanupRmtResources(); // Clean up any allocated resources on failure + return result; } + } - if (_is_bidirectional) - { - result = init_rmt_rx_channel(_gpio, &_rmt_rx_channel, &_rx_event_callbacks, this); - if (!result.success) - { - _cleanupRmtResources(); // Clean up any allocated resources on failure - return result; - } - } + result = init_dshot_encoder(&_dshot_encoder, _rmt_ticks, _pulse_level, + _idle_level); - result = init_dshot_encoder(&_dshot_encoder, _rmt_ticks, _pulse_level, _idle_level); + if (!result.success) { + _cleanupRmtResources(); // Clean up any allocated resources on failure + return result; + } - if (!result.success) - { - _cleanupRmtResources(); // Clean up any allocated resources on failure - return result; - } - - return {true, DSHOT_INIT_SUCCESS}; + return {true, DSHOT_INIT_SUCCESS}; } // Send throttle value -dshot_result_t DShotRMT::sendThrottle(uint16_t throttle) -{ - // Per DShot specification, a throttle value of 0 is a disarm command. - if (throttle == 0) - { - _last_throttle = 0; - return sendCommand(DSHOT_CMD_MOTOR_STOP); - } +dshot_result_t DShotRMT::sendThrottle(uint16_t throttle) { + // Per DShot specification, a throttle value of 0 is a disarm command. + if (throttle == 0) { + _last_throttle = 0; + return sendCommand(DSHOT_CMD_MOTOR_STOP); + } - // Constrain throttle to the valid DShot range. - _last_throttle = constrain(throttle, DSHOT_THROTTLE_MIN, DSHOT_THROTTLE_MAX); + // Constrain throttle to the valid DShot range. + _last_throttle = constrain(throttle, DSHOT_THROTTLE_MIN, DSHOT_THROTTLE_MAX); - _packet = _buildDShotPacket(_last_throttle); - return _sendPacket(_packet); + _packet = _buildDShotPacket(_last_throttle); + return _sendPacket(_packet); } // Send throttle value as a percentage -dshot_result_t DShotRMT::sendThrottlePercent(float percent) -{ - if (percent < DSHOT_PERCENT_MIN || percent > DSHOT_PERCENT_MAX) - { - return {false, DSHOT_PERCENT_NOT_IN_RANGE}; - } +dshot_result_t DShotRMT::sendThrottlePercent(float percent) { + if (percent < DSHOT_PERCENT_MIN || percent > DSHOT_PERCENT_MAX) { + return {false, DSHOT_PERCENT_NOT_IN_RANGE}; + } - // Map percent to DShot throttle range using pre-calculated ratio. - uint16_t throttle = static_cast(DSHOT_THROTTLE_MIN + _percent_to_throttle_ratio * percent); - return sendThrottle(throttle); + // Map percent to DShot throttle range using pre-calculated ratio. + uint16_t throttle = static_cast( + DSHOT_THROTTLE_MIN + _percent_to_throttle_ratio * percent); + return sendThrottle(throttle); } // Sends a DShot command (0-47) to the ESC by accepting an integer value. -dshot_result_t DShotRMT::sendCommand(uint16_t command_value) -{ - // Validate the integer command value before casting. - if (command_value < DSHOT_CMD_MOTOR_STOP || command_value > DSHOT_CMD_MAX_VALUE) - { - return {false, DSHOT_COMMAND_NOT_VALID}; - } - return sendCommand(static_cast(command_value)); +dshot_result_t DShotRMT::sendCommand(uint16_t command_value) { + // Validate the integer command value before casting. + if (command_value < DSHOT_CMD_MOTOR_STOP || + command_value > DSHOT_CMD_MAX_VALUE) { + return {false, DSHOT_COMMAND_NOT_VALID}; + } + return sendCommand(static_cast(command_value)); } // Sends a DShot command (0-47) to the ESC. -dshot_result_t DShotRMT::sendCommand(dshotCommands_e command) -{ - uint16_t repeat_count = DEFAULT_CMD_REPEAT_COUNT; - uint16_t delay_us = DEFAULT_CMD_DELAY_US; +dshot_result_t DShotRMT::sendCommand(dshotCommands_e command) { + uint16_t repeat_count = DEFAULT_CMD_REPEAT_COUNT; + uint16_t delay_us = DEFAULT_CMD_DELAY_US; - // Certain commands require more repetitions to be reliably accepted by the ESC. - switch (command) - { - case DSHOT_CMD_MOTOR_STOP: - case DSHOT_CMD_SAVE_SETTINGS: - case DSHOT_CMD_SPIN_DIRECTION_NORMAL: - case DSHOT_CMD_SPIN_DIRECTION_REVERSED: - repeat_count = SETTINGS_COMMAND_REPEATS; - delay_us = SETTINGS_COMMAND_DELAY_US; - break; - default: - // For other commands, use default repeat and delay. - break; - } + // Certain commands require more repetitions to be reliably accepted by the + // ESC. + switch (command) { + case DSHOT_CMD_MOTOR_STOP: + case DSHOT_CMD_SAVE_SETTINGS: + case DSHOT_CMD_SPIN_DIRECTION_NORMAL: + case DSHOT_CMD_SPIN_DIRECTION_REVERSED: + repeat_count = SETTINGS_COMMAND_REPEATS; + delay_us = SETTINGS_COMMAND_DELAY_US; + break; + default: + // For other commands, use default repeat and delay. + break; + } - return sendCommand(command, repeat_count, delay_us); + return sendCommand(command, repeat_count, delay_us); } -// Sends a DShot command (0-47) to the ESC with a specified repeat count and delay. -dshot_result_t DShotRMT::sendCommand(dshotCommands_e command, uint16_t repeat_count, uint16_t delay_us) -{ - if (!_isValidCommand(command)) - { - return {false, DSHOT_INVALID_COMMAND}; - } - return _sendRepeatedCommand(static_cast(command), repeat_count, delay_us); +// Sends a DShot command (0-47) to the ESC with a specified repeat count and +// delay. +dshot_result_t DShotRMT::sendCommand(dshotCommands_e command, + uint16_t repeat_count, uint16_t delay_us) { + if (!_isValidCommand(command)) { + return {false, DSHOT_INVALID_COMMAND}; + } + return _sendRepeatedCommand(static_cast(command), repeat_count, + delay_us); } // Get telemetry data -dshot_result_t DShotRMT::getTelemetry() -{ - dshot_result_t result = {false, DSHOT_TELEMETRY_FAILED}; - - if (!_is_bidirectional) - { - result.result_code = DSHOT_BIDIR_NOT_ENABLED; - return result; - } - - // Prioritize checking for full telemetry data, as it is richer. - if (_full_telemetry_ready_flag_atomic) - { - _full_telemetry_ready_flag_atomic = false; // Reset the flag - result.telemetry_data = _last_telemetry_data_atomic; // Read the atomic variable - result.telemetry_available = true; - - // Also populate eRPM fields from the full telemetry data for consistency. - result.erpm = result.telemetry_data.rpm; - if (_motor_magnet_count >= MAGNETS_PER_POLE_PAIR) { - uint8_t pole_pairs = _motor_magnet_count / MAGNETS_PER_POLE_PAIR; - result.motor_rpm = result.telemetry_data.rpm / pole_pairs; - } - - result.success = true; - result.result_code = DSHOT_TELEMETRY_DATA_AVAILABLE; - return result; - } - - // If no full telemetry, check for eRPM-only data. - if (_telemetry_ready_flag_atomic) - { - _telemetry_ready_flag_atomic = false; // Reset the flag - uint16_t erpm = _last_erpm_atomic; // Read the atomic variable - - if (erpm != DSHOT_NULL_PACKET && _motor_magnet_count >= MAGNETS_PER_POLE_PAIR) - { - // Calculate motor RPM from eRPM and magnet count - uint8_t pole_pairs = _motor_magnet_count / MAGNETS_PER_POLE_PAIR; - result.erpm = erpm; - result.motor_rpm = (erpm / pole_pairs); - result.success = true; - result.result_code = DSHOT_TELEMETRY_SUCCESS; - } - } +dshot_result_t DShotRMT::getTelemetry() { + dshot_result_t result = {false, DSHOT_TELEMETRY_FAILED}; + if (!_is_bidirectional) { + result.result_code = DSHOT_BIDIR_NOT_ENABLED; return result; + } + + // Prioritize checking for full telemetry data, as it is richer. + if (_full_telemetry_ready_flag_atomic) { + _full_telemetry_ready_flag_atomic = false; // Reset the flag + result.telemetry_data = + _last_telemetry_data_atomic; // Read the atomic variable + result.telemetry_available = true; + + // Also populate eRPM fields from the full telemetry data for consistency. + result.erpm = result.telemetry_data.rpm; + if (_motor_magnet_count >= MAGNETS_PER_POLE_PAIR) { + uint8_t pole_pairs = _motor_magnet_count / MAGNETS_PER_POLE_PAIR; + result.motor_rpm = result.telemetry_data.rpm / pole_pairs; + } + + result.success = true; + result.result_code = DSHOT_TELEMETRY_DATA_AVAILABLE; + return result; + } + + // If no full telemetry, check for eRPM-only data. + if (_telemetry_ready_flag_atomic) { + _telemetry_ready_flag_atomic = false; // Reset the flag + uint16_t erpm = _last_erpm_atomic; // Read the atomic variable + + if (erpm != DSHOT_NULL_PACKET && + _motor_magnet_count >= MAGNETS_PER_POLE_PAIR) { + // Calculate motor RPM from eRPM and magnet count + uint8_t pole_pairs = _motor_magnet_count / MAGNETS_PER_POLE_PAIR; + result.erpm = erpm; + result.motor_rpm = (erpm / pole_pairs); + result.success = true; + result.result_code = DSHOT_TELEMETRY_SUCCESS; + } + } + + return result; } // Reverse motor direction directly -dshot_result_t DShotRMT::setMotorSpinDirection(bool reversed) -{ - dshotCommands_e command = reversed ? dshotCommands_e::DSHOT_CMD_SPIN_DIRECTION_REVERSED : dshotCommands_e::DSHOT_CMD_SPIN_DIRECTION_NORMAL; - return sendCommand(command, SETTINGS_COMMAND_REPEATS, SETTINGS_COMMAND_DELAY_US); +dshot_result_t DShotRMT::setMotorSpinDirection(bool reversed) { + dshotCommands_e command = + reversed ? dshotCommands_e::DSHOT_CMD_SPIN_DIRECTION_REVERSED + : dshotCommands_e::DSHOT_CMD_SPIN_DIRECTION_NORMAL; + return sendCommand(command, SETTINGS_COMMAND_REPEATS, + + SETTINGS_COMMAND_DELAY_US); } -dshot_result_t DShotRMT::sendCustomCommand(uint16_t command_value, uint16_t repeat_count, uint16_t delay_us) -{ - // Validate the integer command value. - if (command_value < DSHOT_CMD_MIN || command_value > DSHOT_CMD_MAX) - { - return {false, DSHOT_COMMAND_NOT_VALID}; - } - return _sendRepeatedCommand(command_value, repeat_count, delay_us); +dshot_result_t DShotRMT::sendCustomCommand(uint16_t command_value, + uint16_t repeat_count, + uint16_t delay_us) { + // Validate the integer command value. + if (command_value < DSHOT_CMD_MIN || command_value > DSHOT_CMD_MAX) { + return {false, DSHOT_COMMAND_NOT_VALID}; + } + return _sendRepeatedCommand(command_value, repeat_count, delay_us); } // Writes settings to the ESC's non-volatile memory; use with caution. -dshot_result_t DShotRMT::saveESCSettings() -{ - return sendCommand(dshotCommands_e::DSHOT_CMD_SAVE_SETTINGS, SETTINGS_COMMAND_REPEATS, SETTINGS_COMMAND_DELAY_US); +dshot_result_t DShotRMT::saveESCSettings() { + return sendCommand(dshotCommands_e::DSHOT_CMD_SAVE_SETTINGS, + SETTINGS_COMMAND_REPEATS, SETTINGS_COMMAND_DELAY_US); } // Private helper to send a command value multiple times. -dshot_result_t DShotRMT::_sendRepeatedCommand(uint16_t value, uint16_t repeat_count, uint16_t delay_us) -{ - bool all_successful = true; - dshot_result_t last_result = {true, DSHOT_COMMAND_SUCCESS}; +dshot_result_t DShotRMT::_sendRepeatedCommand(uint16_t value, + uint16_t repeat_count, + uint16_t delay_us) { + bool all_successful = true; + dshot_result_t last_result = {true, DSHOT_COMMAND_SUCCESS}; - for (uint16_t i = 0; i < repeat_count; i++) - { - last_result = _sendRawDshotFrame(value); + for (uint16_t i = 0; i < repeat_count; i++) { + last_result = _sendRawDshotFrame(value); - if (!last_result.success) - { - all_successful = false; - break; - } - - if (i < repeat_count - 1) - { - delayMicroseconds(delay_us); - } + if (!last_result.success) { + all_successful = false; + break; } - if (all_successful) - { - return {true, DSHOT_COMMAND_SUCCESS}; - } - else - { - // Return the result from the failed transmission. - return last_result; + if (i < repeat_count - 1) { + delayMicroseconds(delay_us); } + } + + if (all_successful) { + return {true, DSHOT_COMMAND_SUCCESS}; + } else { + // Return the result from the failed transmission. + return last_result; + } } // Simple check for valid command range. -bool DShotRMT::_isValidCommand(dshotCommands_e command) const -{ - return (command >= dshotCommands_e::DSHOT_CMD_MOTOR_STOP && command <= DSHOT_CMD_MAX); +bool DShotRMT::_isValidCommand(dshotCommands_e command) const { + return (command >= dshotCommands_e::DSHOT_CMD_MOTOR_STOP && + command <= DSHOT_CMD_MAX); } -dshot_result_t DShotRMT::_sendRawDshotFrame(uint16_t value) -{ - _packet = _buildDShotPacket(value); - return _sendPacket(_packet); +dshot_result_t DShotRMT::_sendRawDshotFrame(uint16_t value) { + _packet = _buildDShotPacket(value); + return _sendPacket(_packet); } // Private Packet Management Functions -dshot_packet_t DShotRMT::_buildDShotPacket(const uint16_t &value) const -{ - dshot_packet_t packet = {}; +dshot_packet_t DShotRMT::_buildDShotPacket(const uint16_t &value) const { + dshot_packet_t packet = {}; - packet.throttle_value = value & DSHOT_THROTTLE_MAX; - packet.telemetric_request = _is_bidirectional ? 1 : 0; + packet.throttle_value = value & DSHOT_THROTTLE_MAX; + packet.telemetric_request = _is_bidirectional ? 1 : 0; - // The data for CRC calculation includes the 11-bit value and the 1-bit telemetry flag. - uint16_t data_for_crc = (packet.throttle_value << 1) | packet.telemetric_request; - packet.checksum = _calculateCRC(data_for_crc); + // The data for CRC calculation includes the 11-bit value and the 1-bit + // telemetry flag. + uint16_t data_for_crc = + (packet.throttle_value << 1) | packet.telemetric_request; + packet.checksum = _calculateCRC(data_for_crc); - return packet; + return packet; } -uint16_t DShotRMT::_buildDShotFrameValue(const dshot_packet_t &packet) const -{ - // Combine throttle, telemetry bit, and CRC into a single 16-bit frame. - uint16_t data_and_telemetry = (packet.throttle_value << 1) | packet.telemetric_request; - return (data_and_telemetry << 4) | packet.checksum; +uint16_t DShotRMT::_buildDShotFrameValue(const dshot_packet_t &packet) const { + // Combine throttle, telemetry bit, and CRC into a single 16-bit frame. + uint16_t data_and_telemetry = + (packet.throttle_value << 1) | packet.telemetric_request; + return (data_and_telemetry << 4) | packet.checksum; } -uint16_t DShotRMT::_calculateCRC(const uint16_t &data) const -{ - // Standard DShot CRC calculation using XOR. - uint16_t crc = (data ^ (data >> 4) ^ (data >> 8)) & DSHOT_CRC_MASK; +uint16_t DShotRMT::_calculateCRC(const uint16_t &data) const { + // Standard DShot CRC calculation using XOR. + uint16_t crc = (data ^ (data >> 4) ^ (data >> 8)) & DSHOT_CRC_MASK; - // For bidirectional DShot, the CRC is inverted per specification. - if (_is_bidirectional) - { - crc = (~crc) & DSHOT_CRC_MASK; + // For bidirectional DShot, the CRC is inverted per specification. + if (_is_bidirectional) { + crc = (~crc) & DSHOT_CRC_MASK; + } + return crc; +} + +uint8_t DShotRMT::_calculateTelemetryCRC(const uint8_t *data, + size_t len) const { + uint8_t crc = 0; + for (size_t i = 0; i < len; ++i) { + crc ^= data[i]; + for (uint8_t j = 0; j < 8; ++j) { + if (crc & 0x80) { + crc = (crc << 1) ^ + 0x07; // DSHOT telemetry uses CRC-8 with polynomial 0x07. + } else { + crc <<= 1; + } } - return crc; + } + return crc; } -uint8_t DShotRMT::_calculateTelemetryCRC(const uint8_t *data, size_t len) const -{ - uint8_t crc = 0; - for (size_t i = 0; i < len; ++i) - { - crc ^= data[i]; - for (uint8_t j = 0; j < 8; ++j) - { - if (crc & 0x80) - { - crc = (crc << 1) ^ 0x07; // DSHOT telemetry uses CRC-8 with polynomial 0x07. - } - else - { - crc <<= 1; - } - } - } - return crc; +void DShotRMT::_extractTelemetryData( + const uint8_t *raw_telemetry_bytes, + dshot_telemetry_data_t &telemetry_data) const { + // Ensure the telemetry_data struct is cleared before filling. + memset(&telemetry_data, 0, sizeof(dshot_telemetry_data_t)); + + // Telemetry data is typically ordered as: + // Byte 0: Temperature (signed 8-bit) + // Byte 1-2: Voltage (16-bit, MSB first) + // Byte 3-4: Current (16-bit, MSB first) + // Byte 5-6: Consumption (16-bit, MSB first) + // Byte 7-8: RPM (16-bit, MSB first) + // Byte 9: CRC (8-bit) - checked separately + + telemetry_data.temperature = static_cast(raw_telemetry_bytes[0]); + telemetry_data.voltage = + (static_cast(raw_telemetry_bytes[1]) << 8) | + raw_telemetry_bytes[2]; + telemetry_data.current = + (static_cast(raw_telemetry_bytes[3]) << 8) | + raw_telemetry_bytes[4]; + telemetry_data.consumption = + (static_cast(raw_telemetry_bytes[5]) << 8) | + raw_telemetry_bytes[6]; + telemetry_data.rpm = (static_cast(raw_telemetry_bytes[7]) << 8) | + raw_telemetry_bytes[8]; } -void DShotRMT::_extractTelemetryData(const uint8_t *raw_telemetry_bytes, dshot_telemetry_data_t &telemetry_data) const -{ - // Ensure the telemetry_data struct is cleared before filling. - memset(&telemetry_data, 0, sizeof(dshot_telemetry_data_t)); +void DShotRMT::_preCalculateRMTTicks() { + // Pre-calculate all timing values in RMT ticks to save CPU cycles during + // operation. + _rmt_ticks.bit_length_ticks = + static_cast(_dshot_timing.bit_length_us * RMT_TICKS_PER_US); + _rmt_ticks.t1h_ticks = + static_cast(_dshot_timing.t1h_lenght_us * RMT_TICKS_PER_US); + _rmt_ticks.t0h_ticks = _rmt_ticks.t1h_ticks >> + 1; // High time for a '0' bit is half of a '1' bit. + _rmt_ticks.t1l_ticks = _rmt_ticks.bit_length_ticks - _rmt_ticks.t1h_ticks; + _rmt_ticks.t0l_ticks = _rmt_ticks.bit_length_ticks - _rmt_ticks.t0h_ticks; - // Telemetry data is typically ordered as: - // Byte 0: Temperature (signed 8-bit) - // Byte 1-2: Voltage (16-bit, MSB first) - // Byte 3-4: Current (16-bit, MSB first) - // Byte 5-6: Consumption (16-bit, MSB first) - // Byte 7-8: RPM (16-bit, MSB first) - // Byte 9: CRC (8-bit) - checked separately + // Calculate the minimum time required between frames to prevent signal + // collision. + _frame_timer_us = + (static_cast(_dshot_timing.bit_length_us * DSHOT_BITS_PER_FRAME) + << 1) + + DSHOT_PADDING_US; - telemetry_data.temperature = static_cast(raw_telemetry_bytes[0]); - telemetry_data.voltage = (static_cast(raw_telemetry_bytes[1]) << 8) | raw_telemetry_bytes[2]; - telemetry_data.current = (static_cast(raw_telemetry_bytes[3]) << 8) | raw_telemetry_bytes[4]; - telemetry_data.consumption = (static_cast(raw_telemetry_bytes[5]) << 8) | raw_telemetry_bytes[6]; - telemetry_data.rpm = (static_cast(raw_telemetry_bytes[7]) << 8) | raw_telemetry_bytes[8]; -} - -void DShotRMT::_preCalculateRMTTicks() -{ - // Pre-calculate all timing values in RMT ticks to save CPU cycles during operation. - _rmt_ticks.bit_length_ticks = static_cast(_dshot_timing.bit_length_us * RMT_TICKS_PER_US); - _rmt_ticks.t1h_ticks = static_cast(_dshot_timing.t1h_lenght_us * RMT_TICKS_PER_US); - _rmt_ticks.t0h_ticks = _rmt_ticks.t1h_ticks >> 1; // High time for a '0' bit is half of a '1' bit. - _rmt_ticks.t1l_ticks = _rmt_ticks.bit_length_ticks - _rmt_ticks.t1h_ticks; - _rmt_ticks.t0l_ticks = _rmt_ticks.bit_length_ticks - _rmt_ticks.t0h_ticks; - - // Calculate the minimum time required between frames to prevent signal collision. - _frame_timer_us = (static_cast(_dshot_timing.bit_length_us * DSHOT_BITS_PER_FRAME) << 1) + DSHOT_PADDING_US; - - if (_is_bidirectional) - { - _frame_timer_us = (_frame_timer_us << 1); - } + if (_is_bidirectional) { + _frame_timer_us = (_frame_timer_us << 1); + } } // Private Frame Processing Functions -dshot_result_t DShotRMT::_sendPacket(const dshot_packet_t &packet) -{ - // Ensure enough time has passed since the last transmission. - if (!_isFrameIntervalElapsed()) - { - return {true, DSHOT_NONE}; +dshot_result_t DShotRMT::_sendPacket(const dshot_packet_t &packet) { + // Ensure enough time has passed since the last transmission. + if (!_isFrameIntervalElapsed()) { + return {true, DSHOT_NONE}; + } + + if (_is_bidirectional) { + // Start the RMT receiver to wait for the ESC's telemetry response. + rmt_symbol_word_t rx_symbols[DSHOT_TELEMETRY_FULL_GCR_BITS]; + size_t rx_size_bytes = + DSHOT_TELEMETRY_FULL_GCR_BITS * sizeof(rmt_symbol_word_t); + + rmt_receive_config_t rmt_rx_config = { + .signal_range_min_ns = DSHOT_PULSE_MIN_NS, + .signal_range_max_ns = DSHOT_PULSE_MAX_NS, + }; + + if (rmt_receive(_rmt_rx_channel, rx_symbols, rx_size_bytes, + &rmt_rx_config) != DSHOT_OK) { + return {false, DSHOT_RECEIVER_FAILED}; } + } - if (_is_bidirectional) - { - // Start the RMT receiver to wait for the ESC's telemetry response. - rmt_symbol_word_t rx_symbols[DSHOT_TELEMETRY_FULL_GCR_BITS]; - size_t rx_size_bytes = DSHOT_TELEMETRY_FULL_GCR_BITS * sizeof(rmt_symbol_word_t); + _encoded_frame_value = _buildDShotFrameValue(packet); - rmt_receive_config_t rmt_rx_config = { - .signal_range_min_ns = DSHOT_PULSE_MIN_NS, - .signal_range_max_ns = DSHOT_PULSE_MAX_NS, - }; + // Byte-swap the 16-bit value for correct transmission order. + // The RMT bytes encoder sends MSB of each byte first. + // uint16_t swapped_value = __builtin_bswap16(_encoded_frame_value); + *_tx_payload = __builtin_bswap16(_encoded_frame_value); - if (rmt_receive(_rmt_rx_channel, rx_symbols, rx_size_bytes, &rmt_rx_config) != DSHOT_OK) - { - return {false, DSHOT_RECEIVER_FAILED}; - } + // The DShot frame is 16 bits, which is 2 bytes. + // size_t tx_size_bytes = sizeof(swapped_value); + + rmt_transmit_config_t tx_config = {.loop_count = 0, + .flags = { + .eot_level = 0, + }}; // No automatic loops. + + // ESP_LOGE("DSHOT", "BEFORE TRANSMIT"); + // In bidirectional mode, the RMT RX channel must be disabled before + // transmitting to prevent the receiver from picking up the outgoing signal + // (loopback). + if (_is_bidirectional) { + if (rmt_disable(_rmt_rx_channel) != DSHOT_OK) { + return {false, DSHOT_RECEIVER_FAILED}; } + } - _encoded_frame_value = _buildDShotFrameValue(packet); + if (rmt_transmit(_rmt_tx_channel, _dshot_encoder, _tx_payload, 2, + &tx_config) != DSHOT_OK) { + return {false, DSHOT_TRANSMISSION_FAILED}; + } + delayMicroseconds(50); + rmt_disable(_rmt_tx_channel); + rmt_enable(_rmt_tx_channel); - // Byte-swap the 16-bit value for correct transmission order. - // The RMT bytes encoder sends MSB of each byte first. - uint16_t swapped_value = __builtin_bswap16(_encoded_frame_value); + // rmt_tx_wait_all_done(_rmt_tx_channel, pdMS_TO_TICKS(100)); - // The DShot frame is 16 bits, which is 2 bytes. - size_t tx_size_bytes = sizeof(swapped_value); + // ESP_LOGE("DSHOT", "AFTER TRANSMIT"); - rmt_transmit_config_t tx_config = {.loop_count = 0}; // No automatic loops. - - // In bidirectional mode, the RMT RX channel must be disabled before transmitting - // to prevent the receiver from picking up the outgoing signal (loopback). - if (_is_bidirectional) - { - if (rmt_disable(_rmt_rx_channel) != DSHOT_OK) - { - return {false, DSHOT_RECEIVER_FAILED}; - } + // Re-enable RMT RX immediately after transmission to catch the response. + if (_is_bidirectional) { + if (rmt_enable(_rmt_rx_channel) != DSHOT_OK) { + return {false, DSHOT_RECEIVER_FAILED}; } + } - if (rmt_transmit(_rmt_tx_channel, _dshot_encoder, &swapped_value, tx_size_bytes, &tx_config) != DSHOT_OK) - { - return {false, DSHOT_TRANSMISSION_FAILED}; - } + _recordFrameTransmissionTime(); // Reset the timer for the next frame. - // Re-enable RMT RX immediately after transmission to catch the response. - if (_is_bidirectional) - { - if (rmt_enable(_rmt_rx_channel) != DSHOT_OK) - { - return {false, DSHOT_RECEIVER_FAILED}; - } - } - - _recordFrameTransmissionTime(); // Reset the timer for the next frame. - - return {true, DSHOT_TRANSMISSION_SUCCESS}; + return {true, DSHOT_TRANSMISSION_SUCCESS}; } // This function is placed in IRAM for high performance, as it may be // called from an ISR context depending on RMT driver implementation. -uint16_t IRAM_ATTR DShotRMT::_decodeDShotFrame(const rmt_symbol_word_t *symbols) const -{ - uint32_t gcr_value = 0; +uint16_t IRAM_ATTR +DShotRMT::_decodeDShotFrame(const rmt_symbol_word_t *symbols) const { + uint32_t gcr_value = 0; - // Decode RMT symbols into a 21-bit GCR (Group Code Recording) value. - // The ESC sends back a signal where the duration of high vs. low determines the bit value. - for (size_t i = 0; i < DSHOT_ERPM_FRAME_GCR_BITS; ++i) - { - bool bit_is_one = symbols[i].duration0 > symbols[i].duration1; - gcr_value = (gcr_value << 1) | bit_is_one; - } + // Decode RMT symbols into a 21-bit GCR (Group Code Recording) value. + // The ESC sends back a signal where the duration of high vs. low determines + // the bit value. + for (size_t i = 0; i < DSHOT_ERPM_FRAME_GCR_BITS; ++i) { + bool bit_is_one = symbols[i].duration0 > symbols[i].duration1; + gcr_value = (gcr_value << 1) | bit_is_one; + } - // Perform GCR decoding (GCR = Value ^ (Value >> 1)). - uint32_t decoded_frame = gcr_value ^ (gcr_value >> 1); + // Perform GCR decoding (GCR = Value ^ (Value >> 1)). + uint32_t decoded_frame = gcr_value ^ (gcr_value >> 1); - // Extract the 16-bit DShot-like frame from the decoded data. - uint16_t data_and_crc = (decoded_frame & DSHOT_FULL_PACKET); + // Extract the 16-bit DShot-like frame from the decoded data. + uint16_t data_and_crc = (decoded_frame & DSHOT_FULL_PACKET); - // The eRPM telemetry frame consists of 12 data bits and 4 CRC bits. - uint16_t received_data = data_and_crc >> DSHOT_CRC_BIT_SHIFT; - uint16_t received_crc = data_and_crc & DSHOT_CRC_MASK; + // The eRPM telemetry frame consists of 12 data bits and 4 CRC bits. + uint16_t received_data = data_and_crc >> DSHOT_CRC_BIT_SHIFT; + uint16_t received_crc = data_and_crc & DSHOT_CRC_MASK; - // Calculate and validate the CRC for the received data. - uint16_t calculated_crc = _calculateCRC(received_data); - if (received_crc != calculated_crc) - { - return DSHOT_NULL_PACKET; - } + // Calculate and validate the CRC for the received data. + uint16_t calculated_crc = _calculateCRC(received_data); + if (received_crc != calculated_crc) { + return DSHOT_NULL_PACKET; + } - // Return the eRPM value (the first 11 bits of the data). - return received_data & DSHOT_THROTTLE_MAX; + // Return the eRPM value (the first 11 bits of the data). + return received_data & DSHOT_THROTTLE_MAX; } // Timing Control Functions -bool IRAM_ATTR DShotRMT::_isFrameIntervalElapsed() const -{ - // Check if the minimum interval between frames has passed. - uint64_t current_time = esp_timer_get_time(); - uint64_t elapsed = current_time - _last_transmission_time_us; - return elapsed >= _frame_timer_us; +bool IRAM_ATTR DShotRMT::_isFrameIntervalElapsed() const { + // Check if the minimum interval between frames has passed. + uint64_t current_time = esp_timer_get_time(); + uint64_t elapsed = current_time - _last_transmission_time_us; + return elapsed >= _frame_timer_us; } -void DShotRMT::_recordFrameTransmissionTime() -{ - // Record the time of the current transmission. - _last_transmission_time_us = esp_timer_get_time(); +void DShotRMT::_recordFrameTransmissionTime() { + // Record the time of the current transmission. + _last_transmission_time_us = esp_timer_get_time(); } // Static Callback Functions // Processes a full telemetry frame from the RMT RX ISR. -void IRAM_ATTR DShotRMT::_processFullTelemetryFrame(const rmt_symbol_word_t *symbols, size_t num_symbols) -{ - if (num_symbols != DSHOT_TELEMETRY_FULL_GCR_BITS) - { - return; // Incorrect number of symbols for a full telemetry frame. +void IRAM_ATTR DShotRMT::_processFullTelemetryFrame( + const rmt_symbol_word_t *symbols, size_t num_symbols) { + if (num_symbols != DSHOT_TELEMETRY_FULL_GCR_BITS) { + return; // Incorrect number of symbols for a full telemetry frame. + } + + uint8_t gcr_decoded_bytes[DSHOT_TELEMETRY_FRAME_LENGTH_BYTES + + 1]; // 10 data bytes + 1 CRC byte. + memset(gcr_decoded_bytes, 0, sizeof(gcr_decoded_bytes)); + + uint8_t data_bit_idx = 0; + for (size_t i = 0; i < DSHOT_TELEMETRY_FULL_GCR_BITS; i += 5) { + uint8_t gcr_group_5bits = 0; + for (size_t j = 0; j < 5; ++j) { + if (i + j < DSHOT_TELEMETRY_FULL_GCR_BITS) { + gcr_group_5bits = + (gcr_group_5bits << 1) | + ((symbols[i + j].duration0 > symbols[i + j].duration1) ? 1 : 0); + } } - uint8_t gcr_decoded_bytes[DSHOT_TELEMETRY_FRAME_LENGTH_BYTES + 1]; // 10 data bytes + 1 CRC byte. - memset(gcr_decoded_bytes, 0, sizeof(gcr_decoded_bytes)); - - uint8_t data_bit_idx = 0; - for (size_t i = 0; i < DSHOT_TELEMETRY_FULL_GCR_BITS; i += 5) - { - uint8_t gcr_group_5bits = 0; - for (size_t j = 0; j < 5; ++j) - { - if (i + j < DSHOT_TELEMETRY_FULL_GCR_BITS) - { - gcr_group_5bits = (gcr_group_5bits << 1) | ((symbols[i + j].duration0 > symbols[i + j].duration1) ? 1 : 0); - } - } - - uint8_t decoded_nibble; // 4 data bits. - switch (gcr_group_5bits) - { - case 0b11110: decoded_nibble = 0b0000; break; - case 0b01001: decoded_nibble = 0b0001; break; - case 0b10100: decoded_nibble = 0b0010; break; - case 0b10101: decoded_nibble = 0b0011; break; - case 0b01010: decoded_nibble = 0b0100; break; - case 0b01011: decoded_nibble = 0b0101; break; - case 0b01110: decoded_nibble = 0b0110; break; - case 0b01111: decoded_nibble = 0b0111; break; - case 0b10010: decoded_nibble = 0b1000; break; - case 0b10011: decoded_nibble = 0b1001; break; - case 0b10110: decoded_nibble = 0b1010; break; - case 0b10111: decoded_nibble = 0b1011; break; - case 0b11010: decoded_nibble = 0b1100; break; - case 0b11011: decoded_nibble = 0b1101; break; - case 0b11100: decoded_nibble = 0b1110; break; - case 0b11101: decoded_nibble = 0b1111; break; - default: return; // Invalid GCR group, discard frame. - } - - // Place the 4 decoded bits into the data_bytes array. - for (int k = 3; k >= 0; --k) - { - if (data_bit_idx < (DSHOT_TELEMETRY_FRAME_LENGTH_BITS + DSHOT_TELEMETRY_CRC_LENGTH_BITS)) - { - size_t byte_idx = data_bit_idx / 8; - size_t bit_pos = data_bit_idx % 8; - if (byte_idx < sizeof(gcr_decoded_bytes)) - { - gcr_decoded_bytes[byte_idx] |= ((decoded_nibble >> k) & 1) << (7 - bit_pos); - } - data_bit_idx++; - } - } + uint8_t decoded_nibble; // 4 data bits. + switch (gcr_group_5bits) { + case 0b11110: + decoded_nibble = 0b0000; + break; + case 0b01001: + decoded_nibble = 0b0001; + break; + case 0b10100: + decoded_nibble = 0b0010; + break; + case 0b10101: + decoded_nibble = 0b0011; + break; + case 0b01010: + decoded_nibble = 0b0100; + break; + case 0b01011: + decoded_nibble = 0b0101; + break; + case 0b01110: + decoded_nibble = 0b0110; + break; + case 0b01111: + decoded_nibble = 0b0111; + break; + case 0b10010: + decoded_nibble = 0b1000; + break; + case 0b10011: + decoded_nibble = 0b1001; + break; + case 0b10110: + decoded_nibble = 0b1010; + break; + case 0b10111: + decoded_nibble = 0b1011; + break; + case 0b11010: + decoded_nibble = 0b1100; + break; + case 0b11011: + decoded_nibble = 0b1101; + break; + case 0b11100: + decoded_nibble = 0b1110; + break; + case 0b11101: + decoded_nibble = 0b1111; + break; + default: + return; // Invalid GCR group, discard frame. } - // The gcr_decoded_bytes array now contains the 10 telemetry bytes + 1 CRC byte. - // Perform CRC validation. - uint8_t received_crc = gcr_decoded_bytes[DSHOT_TELEMETRY_FRAME_LENGTH_BYTES]; - uint8_t calculated_crc = _calculateTelemetryCRC(gcr_decoded_bytes, DSHOT_TELEMETRY_FRAME_LENGTH_BYTES); - - if (received_crc == calculated_crc) - { - dshot_telemetry_data_t telemetry_data; - // Extract from the first 10 bytes (excluding the CRC byte). - _extractTelemetryData(gcr_decoded_bytes, telemetry_data); - - _last_telemetry_data_atomic.store(telemetry_data); - _full_telemetry_ready_flag_atomic.store(true); + // Place the 4 decoded bits into the data_bytes array. + for (int k = 3; k >= 0; --k) { + if (data_bit_idx < (DSHOT_TELEMETRY_FRAME_LENGTH_BITS + + DSHOT_TELEMETRY_CRC_LENGTH_BITS)) { + size_t byte_idx = data_bit_idx / 8; + size_t bit_pos = data_bit_idx % 8; + if (byte_idx < sizeof(gcr_decoded_bytes)) { + gcr_decoded_bytes[byte_idx] |= ((decoded_nibble >> k) & 1) + << (7 - bit_pos); + } + data_bit_idx++; + } } + } + + // The gcr_decoded_bytes array now contains the 10 telemetry bytes + 1 CRC + // byte. Perform CRC validation. + uint8_t received_crc = gcr_decoded_bytes[DSHOT_TELEMETRY_FRAME_LENGTH_BYTES]; + uint8_t calculated_crc = _calculateTelemetryCRC( + gcr_decoded_bytes, DSHOT_TELEMETRY_FRAME_LENGTH_BYTES); + + if (received_crc == calculated_crc) { + dshot_telemetry_data_t telemetry_data; + // Extract from the first 10 bytes (excluding the CRC byte). + _extractTelemetryData(gcr_decoded_bytes, telemetry_data); + + _last_telemetry_data_atomic.store(telemetry_data); + _full_telemetry_ready_flag_atomic.store(true); + } } // This function is called by the RMT driver's ISR when a frame is received. -bool IRAM_ATTR DShotRMT::_on_rx_done(rmt_channel_handle_t rmt_rx_channel, const rmt_rx_done_event_data_t *edata, void *user_data) -{ - DShotRMT *instance = static_cast(user_data); +bool IRAM_ATTR DShotRMT::_on_rx_done(rmt_channel_handle_t rmt_rx_channel, + const rmt_rx_done_event_data_t *edata, + void *user_data) { + DShotRMT *instance = static_cast(user_data); - if (edata) - { - if (edata->num_symbols == DSHOT_TELEMETRY_FULL_GCR_BITS) - { - instance->_processFullTelemetryFrame(edata->received_symbols, edata->num_symbols); - } - else if (edata->num_symbols == DSHOT_ERPM_FRAME_GCR_BITS) - { - uint16_t erpm = instance->_decodeDShotFrame(edata->received_symbols); + if (edata) { + if (edata->num_symbols == DSHOT_TELEMETRY_FULL_GCR_BITS) { + instance->_processFullTelemetryFrame(edata->received_symbols, + edata->num_symbols); + } else if (edata->num_symbols == DSHOT_ERPM_FRAME_GCR_BITS) { + uint16_t erpm = instance->_decodeDShotFrame(edata->received_symbols); - if (erpm != DSHOT_NULL_PACKET) - { - // Atomically store the new eRPM value and set the flag. - instance->_last_erpm_atomic.store(erpm); - instance->_telemetry_ready_flag_atomic.store(true); - } - } + if (erpm != DSHOT_NULL_PACKET) { + // Atomically store the new eRPM value and set the flag. + instance->_last_erpm_atomic.store(erpm); + instance->_telemetry_ready_flag_atomic.store(true); + } } + } - return false; + return false; } -void DShotRMT::_cleanupRmtResources() -{ - if (_rmt_tx_channel) - { - rmt_disable(_rmt_tx_channel); - rmt_del_channel(_rmt_tx_channel); - _rmt_tx_channel = nullptr; - } +void DShotRMT::_cleanupRmtResources() { + if (_rmt_tx_channel) { + rmt_disable(_rmt_tx_channel); + rmt_del_channel(_rmt_tx_channel); - if (_rmt_rx_channel) - { - rmt_disable(_rmt_rx_channel); - rmt_del_channel(_rmt_rx_channel); - _rmt_rx_channel = nullptr; - } + // digitalWrite(14, 0); + _rmt_tx_channel = nullptr; + } - if (_dshot_encoder) - { - rmt_del_encoder(_dshot_encoder); - _dshot_encoder = nullptr; - } + if (_rmt_rx_channel) { + rmt_disable(_rmt_rx_channel); + rmt_del_channel(_rmt_rx_channel); + _rmt_rx_channel = nullptr; + } + + if (_dshot_encoder) { + rmt_del_encoder(_dshot_encoder); + _dshot_encoder = nullptr; + } } diff --git a/src/DShotRMT.h b/src/DShotRMT.h index 217a004..0a9ec75 100644 --- a/src/DShotRMT.h +++ b/src/DShotRMT.h @@ -8,8 +8,8 @@ #pragma once -#include #include +#include #include #include @@ -27,115 +27,147 @@ static constexpr uint8_t DSHOTRMT_PATCH_VERSION = 5; static constexpr auto DSHOT_THROTTLE_FAILSAFE = 0; // DShotRMT class for generating DShot signals and receiving telemetry. -class DShotRMT -{ +class DShotRMT { public: - // Constructs a new DShotRMT object using a GPIO pin. - DShotRMT(gpio_num_t gpio, dshot_mode_t mode = DSHOT300, bool is_bidirectional = false, uint16_t magnet_count = DEFAULT_MOTOR_MAGNET_COUNT); + // Constructs a new DShotRMT object using a GPIO pin. + DShotRMT(gpio_num_t gpio, dshot_mode_t mode = DSHOT300, + bool is_bidirectional = false, + uint16_t magnet_count = DEFAULT_MOTOR_MAGNET_COUNT); - // Constructs a new DShotRMT object using an Arduino-style integer pin number. - DShotRMT(uint16_t pin_nr, dshot_mode_t mode, bool is_bidirectional = false, uint16_t magnet_count = DEFAULT_MOTOR_MAGNET_COUNT); + // Constructs a new DShotRMT object using an Arduino-style integer pin number. + DShotRMT(uint16_t pin_nr, dshot_mode_t mode, bool is_bidirectional = false, + uint16_t magnet_count = DEFAULT_MOTOR_MAGNET_COUNT); - // Destructor - ~DShotRMT(); + // Destructor + ~DShotRMT(); - // Initialize DShotRMT - dshot_result_t begin(); + // Initialize DShotRMT + dshot_result_t begin(); - // Sends a raw throttle value to the ESC. - dshot_result_t sendThrottle(uint16_t throttle); + // Sends a raw throttle value to the ESC. + dshot_result_t sendThrottle(uint16_t throttle); - // Sends a throttle value as a percentage to the ESC. - dshot_result_t sendThrottlePercent(float percent); + // Sends a throttle value as a percentage to the ESC. + dshot_result_t sendThrottlePercent(float percent); - // Sends a DShot command to the ESC by accepting an integer value. - dshot_result_t sendCommand(uint16_t command_value); + // Sends a DShot command to the ESC by accepting an integer value. + dshot_result_t sendCommand(uint16_t command_value); - // Sends a DShot command to the ESC. - dshot_result_t sendCommand(dshotCommands_e command); + // Sends a DShot command to the ESC. + dshot_result_t sendCommand(dshotCommands_e command); - // Sends a DShot command to the ESC with a specified repeat count and delay. - dshot_result_t sendCommand(dshotCommands_e command, uint16_t repeat_count, uint16_t delay_us); + // Sends a DShot command to the ESC with a specified repeat count and delay. + dshot_result_t sendCommand(dshotCommands_e command, uint16_t repeat_count, + uint16_t delay_us); - /** - * @brief Sends a custom DShot command to the ESC. Advanced feature, use with caution. - * @param command_value The raw command value (0-47). - * @param repeat_count The number of times to send the command. - * @param delay_us The delay in microseconds between repetitions. - * @return dshot_result_t The result of the operation. - */ - dshot_result_t sendCustomCommand(uint16_t command_value, uint16_t repeat_count, uint16_t delay_us); + /** + * @brief Sends a custom DShot command to the ESC. Advanced feature, use with + * caution. + * @param command_value The raw command value (0-47). + * @param repeat_count The number of times to send the command. + * @param delay_us The delay in microseconds between repetitions. + * @return dshot_result_t The result of the operation. + */ + dshot_result_t sendCustomCommand(uint16_t command_value, + uint16_t repeat_count, uint16_t delay_us); - // Retrieves telemetry data from the ESC. - dshot_result_t getTelemetry(); + // Retrieves telemetry data from the ESC. + dshot_result_t getTelemetry(); - // Sets the motor spin direction. - dshot_result_t setMotorSpinDirection(bool reversed); + // Sets the motor spin direction. + dshot_result_t setMotorSpinDirection(bool reversed); - // Sends a command to the ESC to save its current settings. - dshot_result_t saveESCSettings(); + // Sends a command to the ESC to save its current settings. + dshot_result_t saveESCSettings(); - // Getters for DShot info - dshot_mode_t getMode() const { return _mode; } - bool isBidirectional() const { return _is_bidirectional; } - uint16_t getThrottleValue() const { return _last_throttle; } - uint16_t getEncodedFrameValue() const { return _encoded_frame_value; } + // Getters for DShot info + dshot_mode_t getMode() const { return _mode; } + bool isBidirectional() const { return _is_bidirectional; } + uint16_t getThrottleValue() const { return _last_throttle; } + uint16_t getEncodedFrameValue() const { return _encoded_frame_value; } + + rmt_channel_handle_t _rmt_tx_channel = nullptr; // RMT transmit channel handle + + // Static Callback Function for RMT RX Events + void _cleanupRmtResources(); private: - dshot_result_t _sendRawDshotFrame(uint16_t value); - static bool IRAM_ATTR _on_rx_done(rmt_channel_handle_t rmt_rx_channel, const rmt_rx_done_event_data_t *edata, void *user_data); + uint16_t *_tx_payload; + dshot_result_t _sendRawDshotFrame(uint16_t value); + static bool IRAM_ATTR _on_rx_done(rmt_channel_handle_t rmt_rx_channel, + const rmt_rx_done_event_data_t *edata, + void *user_data); - // DShot Configuration Parameters - gpio_num_t _gpio; // GPIO pin used for DShot communication - dshot_mode_t _mode; // DShot mode (e.g., DSHOT300, DSHOT600) - bool _is_bidirectional; // True if bidirectional DShot is enabled - uint16_t _motor_magnet_count; // Number of magnets in the motor for RPM calculation - dshot_timing_us_t _dshot_timing; // DShot timing parameters in microseconds + // DShot Configuration Parameters + gpio_num_t _gpio; // GPIO pin used for DShot communication + dshot_mode_t _mode; // DShot mode (e.g., DSHOT300, DSHOT600) + bool _is_bidirectional; // True if bidirectional DShot is enabled + uint16_t + _motor_magnet_count; // Number of magnets in the motor for RPM calculation + dshot_timing_us_t _dshot_timing; // DShot timing parameters in microseconds - // RMT Hardware Handles and Configuration - rmt_channel_handle_t _rmt_tx_channel = nullptr; // RMT transmit channel handle - rmt_channel_handle_t _rmt_rx_channel = nullptr; // RMT receive channel handle - rmt_encoder_handle_t _dshot_encoder = nullptr; // DShot RMT encoder handle - rmt_ticks_t _rmt_ticks; // Pre-calculated RMT timing ticks - uint16_t _pulse_level = 1; // Output level for a pulse (typically high) - uint16_t _idle_level = 0; // Output level for idle (typically low) + // RMT Hardware Handles and Configuration + rmt_channel_handle_t _rmt_rx_channel = nullptr; // RMT receive channel handle + rmt_encoder_handle_t _dshot_encoder = nullptr; // DShot RMT encoder handle + rmt_ticks_t _rmt_ticks; // Pre-calculated RMT timing ticks + uint16_t _pulse_level = 1; // Output level for a pulse (typically high) + uint16_t _idle_level = 0; // Output level for idle (typically low) - // DShot Frame Timing and State Variables - uint64_t _last_transmission_time_us = 0; // Timestamp of the last DShot frame transmission - uint64_t _frame_timer_us = 0; // Minimum time required between DShot frames - float _percent_to_throttle_ratio = 0.0f; // Pre-calculated ratio for throttle percentage conversion - uint16_t _last_throttle = 0; // Last transmitted throttle value - dshot_packet_t _packet; // Current DShot packet being processed - uint16_t _encoded_frame_value = 0; // Last encoded 16-bit DShot frame value + // DShot Frame Timing and State Variables + uint64_t _last_transmission_time_us = + 0; // Timestamp of the last DShot frame transmission + uint64_t _frame_timer_us = 0; // Minimum time required between DShot frames + float _percent_to_throttle_ratio = + 0.0f; // Pre-calculated ratio for throttle percentage conversion + uint16_t _last_throttle = 0; // Last transmitted throttle value + dshot_packet_t _packet; // Current DShot packet being processed + uint16_t _encoded_frame_value = 0; // Last encoded 16-bit DShot frame value - // Telemetry Related Variables - std::atomic _last_erpm_atomic = 0; // Atomically stored last received eRPM value - std::atomic _telemetry_ready_flag_atomic = false; // Atomically stored flag indicating new telemetry data - std::atomic _last_telemetry_data_atomic = {}; // Atomically stored last received full telemetry data - std::atomic _full_telemetry_ready_flag_atomic = false; // Atomically stored flag indicating new full telemetry data - rmt_rx_event_callbacks_t _rx_event_callbacks = { - // RMT receive event callbacks - .on_recv_done = _on_rx_done, - }; + // Telemetry Related Variables + std::atomic _last_erpm_atomic = + 0; // Atomically stored last received eRPM value + std::atomic _telemetry_ready_flag_atomic = + false; // Atomically stored flag indicating new telemetry data + std::atomic _last_telemetry_data_atomic = + {}; // Atomically stored last received full telemetry data + std::atomic _full_telemetry_ready_flag_atomic = + false; // Atomically stored flag indicating new full telemetry data + rmt_rx_event_callbacks_t _rx_event_callbacks = { + // RMT receive event callbacks + .on_recv_done = _on_rx_done, + }; - // Private Helper Functions for DShot Protocol Logic - bool _isValidCommand(dshotCommands_e command) const; // Checks if a given DShot command is valid - dshot_packet_t _buildDShotPacket(const uint16_t &value) const; // Builds a DShot packet from a value (throttle or command) - uint16_t _buildDShotFrameValue(const dshot_packet_t &packet) const; // Combines packet data into a 16-bit DShot frame value - uint16_t _calculateCRC(const uint16_t &data) const; // Calculates the 4-bit CRC for a DShot frame - uint8_t _calculateTelemetryCRC(const uint8_t *data, size_t len) const; // Calculates the 8-bit CRC for telemetry data - void _extractTelemetryData(const uint8_t *raw_telemetry_bytes, dshot_telemetry_data_t &telemetry_data) const; // Extracts telemetry data from raw bytes - void _preCalculateRMTTicks(); // Pre-calculates RMT timing ticks for the selected DShot mode - dshot_result_t _sendPacket(const dshot_packet_t &packet); // Sends a DShot frame via RMT TX channel - uint16_t IRAM_ATTR _decodeDShotFrame(const rmt_symbol_word_t *symbols) const; // Decodes a received RMT symbol array into an eRPM value - void IRAM_ATTR _processFullTelemetryFrame(const rmt_symbol_word_t *symbols, size_t num_symbols); // Processes a full telemetry frame - bool IRAM_ATTR _isFrameIntervalElapsed() const; // Checks if enough time has passed since the last frame transmission - void _recordFrameTransmissionTime(); // Records the current time as the last frame transmission time + // Private Helper Functions for DShot Protocol Logic + bool _isValidCommand(dshotCommands_e command) + const; // Checks if a given DShot command is valid + dshot_packet_t _buildDShotPacket(const uint16_t &value) + const; // Builds a DShot packet from a value (throttle or command) + uint16_t _buildDShotFrameValue(const dshot_packet_t &packet) + const; // Combines packet data into a 16-bit DShot frame value + uint16_t _calculateCRC( + const uint16_t &data) const; // Calculates the 4-bit CRC for a DShot frame + uint8_t _calculateTelemetryCRC(const uint8_t *data, size_t len) + const; // Calculates the 8-bit CRC for telemetry data + void _extractTelemetryData(const uint8_t *raw_telemetry_bytes, + dshot_telemetry_data_t &telemetry_data) + const; // Extracts telemetry data from raw bytes + void _preCalculateRMTTicks(); // Pre-calculates RMT timing ticks for the + // selected DShot mode + dshot_result_t _sendPacket( + const dshot_packet_t &packet); // Sends a DShot frame via RMT TX channel + uint16_t IRAM_ATTR _decodeDShotFrame(const rmt_symbol_word_t *symbols) + const; // Decodes a received RMT symbol array into an eRPM value + void IRAM_ATTR _processFullTelemetryFrame( + const rmt_symbol_word_t *symbols, + size_t num_symbols); // Processes a full telemetry frame + bool IRAM_ATTR + _isFrameIntervalElapsed() const; // Checks if enough time has passed since the + // last frame transmission + void _recordFrameTransmissionTime(); // Records the current time as the last + // frame transmission time - dshot_result_t _sendRepeatedCommand(uint16_t value, uint16_t repeat_count, uint16_t delay_us); - - // Static Callback Function for RMT RX Events - void _cleanupRmtResources(); + dshot_result_t _sendRepeatedCommand(uint16_t value, uint16_t repeat_count, + uint16_t delay_us); }; #include "dshot_utils.h" // Include for helper functions diff --git a/src/dshot_init.cpp b/src/dshot_init.cpp index 5395cb4..bf05cf7 100644 --- a/src/dshot_init.cpp +++ b/src/dshot_init.cpp @@ -9,87 +9,85 @@ #include "dshot_init.h" // Function to initialize the RMT TX channel -dshot_result_t init_rmt_tx_channel(gpio_num_t gpio, rmt_channel_handle_t *out_channel, bool is_bidirectional) -{ - rmt_tx_channel_config_t tx_channel_config = { - .gpio_num = gpio, - .clk_src = DSHOT_CLOCK_SRC_DEFAULT, - .resolution_hz = DSHOT_RMT_RESOLUTION, - .mem_block_symbols = RMT_TX_BUFFER_SYMBOLS, - .trans_queue_depth = RMT_QUEUE_DEPTH, - .flags = { - .invert_out = static_cast(is_bidirectional ? 1 : 0), - .init_level = 0}}; +dshot_result_t init_rmt_tx_channel(gpio_num_t gpio, + rmt_channel_handle_t *out_channel, + bool is_bidirectional) { + rmt_tx_channel_config_t tx_channel_config = { + .gpio_num = gpio, + .clk_src = DSHOT_CLOCK_SRC_DEFAULT, + .resolution_hz = DSHOT_RMT_RESOLUTION, + .mem_block_symbols = 48, + .trans_queue_depth = RMT_QUEUE_DEPTH, + .flags = {.invert_out = static_cast(is_bidirectional ? 1 : 0), + .init_level = 0, + .with_dma = 0}}; - rmt_transmit_config_t rmt_tx_config = {}; // Initialize all members to zero - rmt_tx_config.loop_count = 0; // No automatic loops - real-time calculation + if (rmt_new_tx_channel(&tx_channel_config, out_channel) != DSHOT_OK) { + return {false, DSHOT_TX_INIT_FAILED}; + } - if (rmt_new_tx_channel(&tx_channel_config, out_channel) != DSHOT_OK) - { - return {false, DSHOT_TX_INIT_FAILED}; - } + if (rmt_enable(*out_channel) != DSHOT_OK) { + return {false, DSHOT_TX_INIT_FAILED}; + } - if (rmt_enable(*out_channel) != DSHOT_OK) - { - return {false, DSHOT_TX_INIT_FAILED}; - } - - return {true, DSHOT_TX_INIT_SUCCESS}; + return {true, DSHOT_TX_INIT_SUCCESS}; } // Function to initialize the RMT RX channel -dshot_result_t init_rmt_rx_channel(gpio_num_t gpio, rmt_channel_handle_t *out_channel, rmt_rx_event_callbacks_t *rx_event_callbacks, void *user_data) -{ - rmt_rx_channel_config_t rx_channel_config = { - .gpio_num = gpio, - .clk_src = DSHOT_CLOCK_SRC_DEFAULT, - .resolution_hz = DSHOT_RMT_RESOLUTION, - .mem_block_symbols = RMT_RX_BUFFER_SYMBOLS, - }; +dshot_result_t init_rmt_rx_channel(gpio_num_t gpio, + rmt_channel_handle_t *out_channel, + rmt_rx_event_callbacks_t *rx_event_callbacks, + void *user_data) { + rmt_rx_channel_config_t rx_channel_config = { + .gpio_num = gpio, + .clk_src = DSHOT_CLOCK_SRC_DEFAULT, + .resolution_hz = DSHOT_RMT_RESOLUTION, + .mem_block_symbols = RMT_RX_BUFFER_SYMBOLS, + }; - if (rmt_new_rx_channel(&rx_channel_config, out_channel) != DSHOT_OK) - { - return {false, DSHOT_RX_INIT_FAILED}; - } + if (rmt_new_rx_channel(&rx_channel_config, out_channel) != DSHOT_OK) { + return {false, DSHOT_RX_INIT_FAILED}; + } - if (rmt_rx_register_event_callbacks(*out_channel, rx_event_callbacks, user_data) != DSHOT_OK) - { - return {false, DSHOT_CALLBACK_REGISTERING_FAILED}; - } + if (rmt_rx_register_event_callbacks(*out_channel, rx_event_callbacks, + user_data) != DSHOT_OK) { + return {false, DSHOT_CALLBACK_REGISTERING_FAILED}; + } - if (rmt_enable(*out_channel) != DSHOT_OK) - { - return {false, DSHOT_RX_INIT_FAILED}; - } + if (rmt_enable(*out_channel) != DSHOT_OK) { + return {false, DSHOT_RX_INIT_FAILED}; + } - return {true, DSHOT_RX_INIT_SUCCESS}; + return {true, DSHOT_RX_INIT_SUCCESS}; } // Function to initialize the DShot RMT encoder -dshot_result_t init_dshot_encoder(rmt_encoder_handle_t *out_encoder, const rmt_ticks_t &rmt_ticks, uint16_t pulse_level, uint16_t idle_level) -{ - rmt_bytes_encoder_config_t encoder_config = { - .bit0 = { - .duration0 = rmt_ticks.t0h_ticks, - .level0 = pulse_level, - .duration1 = rmt_ticks.t0l_ticks, - .level1 = idle_level, - }, - .bit1 = { - .duration0 = rmt_ticks.t1h_ticks, - .level0 = pulse_level, - .duration1 = rmt_ticks.t1l_ticks, - .level1 = idle_level, - }, +dshot_result_t init_dshot_encoder(rmt_encoder_handle_t *out_encoder, + const rmt_ticks_t &rmt_ticks, + uint16_t pulse_level, uint16_t idle_level) { + rmt_bytes_encoder_config_t encoder_config = { + .bit0 = + { + .duration0 = rmt_ticks.t0h_ticks, + .level0 = pulse_level, + .duration1 = rmt_ticks.t0l_ticks, + .level1 = idle_level, + }, + .bit1 = + { + .duration0 = rmt_ticks.t1h_ticks, + .level0 = pulse_level, + .duration1 = rmt_ticks.t1l_ticks, + .level1 = idle_level, + }, - .flags = { - .msb_first = 1 // DShot is MSB first - }}; + .flags = { + .msb_first = 1 // DShot is MSB first + }}; - if (rmt_new_bytes_encoder(&encoder_config, out_encoder) != DSHOT_OK) - { - return {false, DSHOT_ENCODER_INIT_FAILED}; - } + if (rmt_new_bytes_encoder(&encoder_config, out_encoder) != DSHOT_OK) { + return {false, DSHOT_ENCODER_INIT_FAILED}; + } - return {true, DSHOT_ENCODER_INIT_SUCCESS}; + return {true, DSHOT_ENCODER_INIT_SUCCESS}; }