...update result
This commit is contained in:
parent
16ab6a6ef2
commit
0e77a05039
70
DShotRMT.cpp
70
DShotRMT.cpp
|
|
@ -44,7 +44,7 @@ DShotRMT::DShotRMT(gpio_num_t gpio, dshot_mode_t mode, bool is_bidirectional)
|
||||||
_result{false, UNKNOWN_ERROR}
|
_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_SWITCH_TIME;
|
_frame_timer_us = _timing_config.frame_length_us + DSHOT_PAUSE_US;
|
||||||
|
|
||||||
// Double frame time for bidirectional mode (includes response time)
|
// Double frame time for bidirectional mode (includes response time)
|
||||||
if (_is_bidirectional)
|
if (_is_bidirectional)
|
||||||
|
|
@ -130,8 +130,7 @@ dshot_result_t DShotRMT::begin()
|
||||||
// Init RMT TX channel
|
// Init RMT TX channel
|
||||||
dshot_result_t DShotRMT::_initTXChannel()
|
dshot_result_t DShotRMT::_initTXChannel()
|
||||||
{
|
{
|
||||||
// Result container
|
_result = {false, TX_INIT_FAILED};
|
||||||
dshot_result_t result = {false, TX_INIT_FAILED};
|
|
||||||
|
|
||||||
// Configure TX channel
|
// Configure TX channel
|
||||||
_tx_channel_config.gpio_num = _gpio;
|
_tx_channel_config.gpio_num = _gpio;
|
||||||
|
|
@ -147,26 +146,25 @@ 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 _result;
|
||||||
}
|
}
|
||||||
|
|
||||||
//
|
//
|
||||||
if (rmt_enable(_rmt_tx_channel) != DSHOT_OK)
|
if (rmt_enable(_rmt_tx_channel) != DSHOT_OK)
|
||||||
{
|
{
|
||||||
return result;
|
return _result;
|
||||||
}
|
}
|
||||||
|
|
||||||
result.success = true;
|
_result.success = true;
|
||||||
result.msg = TX_INIT_SUCCESS;
|
_result.msg = TX_INIT_SUCCESS;
|
||||||
|
|
||||||
return result;
|
return _result;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Init RMT RX channel
|
// Init RMT RX channel
|
||||||
dshot_result_t DShotRMT::_initRXChannel()
|
dshot_result_t DShotRMT::_initRXChannel()
|
||||||
{
|
{
|
||||||
// Result container
|
_result = {false, RX_INIT_FAILED};
|
||||||
dshot_result_t 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;
|
||||||
|
|
@ -184,19 +182,19 @@ 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 _result;
|
||||||
}
|
}
|
||||||
|
|
||||||
//
|
//
|
||||||
if (rmt_enable(_rmt_rx_channel) != DSHOT_OK)
|
if (rmt_enable(_rmt_rx_channel) != DSHOT_OK)
|
||||||
{
|
{
|
||||||
return result;
|
return _result;
|
||||||
}
|
}
|
||||||
|
|
||||||
result.success = true;
|
_result.success = true;
|
||||||
result.msg = RX_INIT_SUCCESS;
|
_result.msg = RX_INIT_SUCCESS;
|
||||||
|
|
||||||
return result;
|
return _result;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Callback for RMT RX
|
// Callback for RMT RX
|
||||||
|
|
@ -226,6 +224,8 @@ 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 = {};
|
||||||
|
|
||||||
|
|
@ -355,12 +355,12 @@ uint16_t DShotRMT::_parseDShotPacket(const dshot_packet_t &packet)
|
||||||
uint16_t DShotRMT::_calculateCRC(const uint16_t data)
|
uint16_t DShotRMT::_calculateCRC(const uint16_t data)
|
||||||
{
|
{
|
||||||
// DShot CRC
|
// DShot CRC
|
||||||
uint16_t crc = (data ^ (data >> 4) ^ (data >> 8)) & 0b0000000000001111;
|
uint16_t crc = (data ^ (data >> 4) ^ (data >> 8)) & DSHOT_CRC_MASK;
|
||||||
|
|
||||||
// Invert CRC for bidirectional DShot mode
|
// Invert CRC for bidirectional DShot mode
|
||||||
if (_is_bidirectional)
|
if (_is_bidirectional)
|
||||||
{
|
{
|
||||||
crc = (~crc) & 0b0000000000001111;
|
crc = (~crc) & DSHOT_CRC_MASK;
|
||||||
}
|
}
|
||||||
|
|
||||||
return crc;
|
return crc;
|
||||||
|
|
@ -378,13 +378,13 @@ 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)
|
||||||
{
|
{
|
||||||
dshot_result_t result = {false, UNKNOWN_ERROR};
|
_result = {false, UNKNOWN_ERROR};
|
||||||
|
|
||||||
// Check timing requirements
|
// Check timing requirements
|
||||||
if (!_timer_signal())
|
if (!_timer_signal())
|
||||||
{
|
{
|
||||||
result.msg = TIMING_CORRECTION;
|
_result.msg = TIMING_CORRECTION;
|
||||||
return result;
|
return _result;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Enable RMT RX before RMT TX
|
// Enable RMT RX before RMT TX
|
||||||
|
|
@ -395,8 +395,8 @@ dshot_result_t DShotRMT::_sendDShotFrame(const dshot_packet_t &packet)
|
||||||
|
|
||||||
if (rmt_receive(_rmt_rx_channel, rx_symbols, sizeof(rx_symbols), &_receive_config) != DSHOT_OK)
|
if (rmt_receive(_rmt_rx_channel, rx_symbols, sizeof(rx_symbols), &_receive_config) != DSHOT_OK)
|
||||||
{
|
{
|
||||||
result.msg = RECEIVER_FAILED;
|
_result.msg = RECEIVER_FAILED;
|
||||||
return result;
|
return _result;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -415,16 +415,16 @@ 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;
|
_result.msg = RECEIVER_FAILED;
|
||||||
return result;
|
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;
|
_result.msg = TRANSMISSION_FAILED;
|
||||||
return result;
|
return _result;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Re-enable RMT RX
|
// Re-enable RMT RX
|
||||||
|
|
@ -432,18 +432,18 @@ 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;
|
_result.msg = RECEIVER_FAILED;
|
||||||
return result;
|
return _result;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Update timestamp and calculate execution time
|
// Update timestamp and calculate execution time
|
||||||
_timer_reset();
|
_timer_reset();
|
||||||
|
|
||||||
result.success = true;
|
_result.success = true;
|
||||||
result.msg = TRANSMISSION_SUCCESS;
|
_result.msg = TRANSMISSION_SUCCESS;
|
||||||
|
|
||||||
return result;
|
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)
|
||||||
|
|
@ -486,13 +486,13 @@ uint16_t DShotRMT::_decodeDShotFrame(const rmt_symbol_word_t *symbols)
|
||||||
|
|
||||||
// Extract 16 data bits and 4 CRC bits from 20-bit frame.
|
// Extract 16 data bits and 4 CRC bits from 20-bit frame.
|
||||||
// The first bit of the GCR frame is a start bit and is discarded.
|
// The first bit of the GCR frame is a start bit and is discarded.
|
||||||
uint16_t data_and_crc = (decoded_frame & 0xFFFF);
|
uint16_t data_and_crc = (decoded_frame & DSHOT_FULL_PACKET);
|
||||||
|
|
||||||
// Cutting 4 bits?
|
// Cutting 4 bits?
|
||||||
uint16_t received_data = data_and_crc >> 4;
|
uint16_t received_data = data_and_crc >> 4;
|
||||||
|
|
||||||
// Masking CRC
|
// Masking CRC
|
||||||
uint16_t received_crc = data_and_crc & 0b0000000000001111;
|
uint16_t received_crc = data_and_crc & DSHOT_CRC_MASK;
|
||||||
|
|
||||||
// Telemetry request bit is always 1.
|
// Telemetry request bit is always 1.
|
||||||
if (!(received_data & (1 << 11)))
|
if (!(received_data & (1 << 11)))
|
||||||
|
|
@ -511,7 +511,7 @@ uint16_t DShotRMT::_decodeDShotFrame(const rmt_symbol_word_t *symbols)
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the eRPM value (first 11 bits of received data).
|
// Return the eRPM value (first 11 bits of received data).
|
||||||
return received_data & 0b0000011111111111;
|
return received_data & DSHOT_THROTTLE_MAX;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Check if enough time has passed for next transmission
|
// Check if enough time has passed for next transmission
|
||||||
|
|
|
||||||
|
|
@ -19,8 +19,10 @@ static constexpr auto DSHOT_THROTTLE_FAILSAFE = 0;
|
||||||
static constexpr auto DSHOT_THROTTLE_MIN = 48;
|
static constexpr auto DSHOT_THROTTLE_MIN = 48;
|
||||||
static constexpr auto DSHOT_THROTTLE_MAX = 2047;
|
static constexpr auto DSHOT_THROTTLE_MAX = 2047;
|
||||||
static constexpr auto DSHOT_BITS_PER_FRAME = 16;
|
static constexpr auto DSHOT_BITS_PER_FRAME = 16;
|
||||||
static constexpr auto DSHOT_SWITCH_TIME = 30; // Additional time in us for bidir switching
|
static constexpr auto DSHOT_PAUSE_US = 30; // Additional frame pause time
|
||||||
static constexpr auto DSHOT_NULL_PACKET = 0b0000000000000000;
|
static constexpr auto DSHOT_NULL_PACKET = 0b0000000000000000;
|
||||||
|
static constexpr auto DSHOT_FULL_PACKET = 0b1111111111111111;
|
||||||
|
static constexpr auto DSHOT_CRC_MASK = 0b0000000000001111;
|
||||||
static constexpr auto DSHOT_RX_TIMEOUT_MS = 2; // Never reached, just a timeeout
|
static constexpr auto DSHOT_RX_TIMEOUT_MS = 2; // Never reached, just a timeeout
|
||||||
static constexpr auto GCR_BITS_PER_FRAME = 21; // Number of GCR bits in a DShot answer frame (1 start + 16 data + 4 CRC)
|
static constexpr auto GCR_BITS_PER_FRAME = 21; // Number of GCR bits in a DShot answer frame (1 start + 16 data + 4 CRC)
|
||||||
static constexpr auto DEFAULT_MOTOR_MAGNET_COUNT = 14;
|
static constexpr auto DEFAULT_MOTOR_MAGNET_COUNT = 14;
|
||||||
|
|
@ -30,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_DEFAULT;
|
constexpr auto DSHOT_CLOCK_SRC_DEFAULT = RMT_CLK_SRC_APB;
|
||||||
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;
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue