117 lines
3.1 KiB
C++
117 lines
3.1 KiB
C++
/**
|
|
* @file DShotRMT.cpp
|
|
* @brief Implementation of continuous DShot signal using ESP32 RMT encoder API with pause between frames
|
|
* @author Wastl Kraus
|
|
* @date 2025-06-11
|
|
* @license MIT
|
|
*/
|
|
|
|
#include <DShotRMT.h>
|
|
|
|
DShotRMT::DShotRMT(gpio_num_t gpio, dshot_mode_t mode, bool isBidirectional)
|
|
: _gpio(gpio), _mode(mode), _isBidirectional(isBidirectional) {}
|
|
|
|
void DShotRMT::begin()
|
|
{
|
|
rmt_tx_channel_config_t rmt_tx_channel_config = {
|
|
.gpio_num = _gpio,
|
|
.clk_src = RMT_CLK_SRC_DEFAULT,
|
|
.resolution_hz = DEFAULT_RES_HZ,
|
|
.mem_block_symbols = 64,
|
|
.trans_queue_depth = 1,
|
|
.flags = {
|
|
.invert_out = _isBidirectional,
|
|
.with_dma = false}};
|
|
|
|
rmt_new_tx_channel(&rmt_tx_channel_config, &_rmt_channel);
|
|
rmt_enable(_rmt_channel);
|
|
|
|
// Create encoder only once
|
|
if (!_encoder)
|
|
{
|
|
rmt_copy_encoder_config_t enc_cfg = {};
|
|
rmt_new_copy_encoder(&enc_cfg, &_encoder);
|
|
}
|
|
|
|
_transmit_config.loop_count = -1; // Infinite loop
|
|
_transmit_config.flags.eot_level = 0;
|
|
}
|
|
|
|
void DShotRMT::setThrottle(uint16_t throttle, bool telemetry)
|
|
{
|
|
// Send only new Throttle values
|
|
static uint16_t _lastThrottle = 0;
|
|
|
|
// Clamp to 11 bits
|
|
throttle &= 0x07FF;
|
|
|
|
if (throttle == _lastThrottle)
|
|
return;
|
|
|
|
_lastThrottle = throttle;
|
|
|
|
// Build 16-bit DShot packet
|
|
uint16_t packet = (throttle << 1) | (telemetry ? 1 : 0);
|
|
uint8_t crc = (packet ^ (packet >> 4) ^ (packet >> 8)) & 0x0F;
|
|
packet = (packet << 4) | crc;
|
|
|
|
// Build symbols
|
|
rmt_symbol_word_t symbols[32] = {};
|
|
size_t count = 0;
|
|
|
|
buildFrameSymbols(packet, symbols, count);
|
|
|
|
// Transmit
|
|
rmt_disable(_rmt_channel); // Ensure safe restart
|
|
rmt_enable(_rmt_channel);
|
|
|
|
rmt_transmit(_rmt_channel, _encoder, symbols, count * sizeof(rmt_symbol_word_t), &_transmit_config);
|
|
}
|
|
|
|
void DShotRMT::buildFrameSymbols(uint16_t dshot_packet, rmt_symbol_word_t *symbols, size_t &count)
|
|
{
|
|
uint32_t ticks_per_bit = 0;
|
|
uint32_t ticks_zero_high = 0;
|
|
uint32_t ticks_one_high = 0;
|
|
|
|
switch (_mode)
|
|
{
|
|
case DSHOT150:
|
|
ticks_per_bit = 67;
|
|
ticks_zero_high = 25;
|
|
ticks_one_high = 50;
|
|
break;
|
|
case DSHOT300:
|
|
ticks_per_bit = 33;
|
|
ticks_zero_high = 12;
|
|
ticks_one_high = 25;
|
|
break;
|
|
case DSHOT600:
|
|
ticks_per_bit = 17;
|
|
ticks_zero_high = 6;
|
|
ticks_one_high = 13;
|
|
break;
|
|
}
|
|
|
|
uint32_t ticks_zero_low = ticks_per_bit - ticks_zero_high;
|
|
uint32_t ticks_one_low = ticks_per_bit - ticks_one_high;
|
|
|
|
// Encode 16 bits
|
|
for (int i = 15; i >= 0; i--)
|
|
{
|
|
bool bit = (dshot_packet >> i) & 0x01;
|
|
symbols[count].level0 = 1;
|
|
symbols[count].duration0 = bit ? ticks_one_high : ticks_zero_high;
|
|
symbols[count].level1 = 0;
|
|
symbols[count].duration1 = bit ? ticks_one_low : ticks_zero_low;
|
|
count++;
|
|
}
|
|
|
|
// Add pause
|
|
symbols[count].level0 = 0;
|
|
symbols[count].duration0 = ticks_per_bit * PAUSE_BITS;
|
|
symbols[count].level1 = 0;
|
|
symbols[count].duration1 = 0;
|
|
count++;
|
|
}
|