Cansat
/
DShotRMT
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Readability changes

This commit is contained in:
Wastl Kraus 2023-04-15 07:45:14 +02:00
parent a8c2ccaaea
commit 4e84b2e5e7
2 changed files with 102 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

142
DShotRMT.cpp
View File

@ -6,27 +6,29 @@
#include <DShotRMT.h> #include <DShotRMT.h>
// Constructor that takes gpio and rmtChannel as arguments
DShotRMT::DShotRMT(gpio_num_t gpio, rmt_channel_t rmtChannel) DShotRMT::DShotRMT(gpio_num_t gpio, rmt_channel_t rmtChannel)
{ {
// Initialize the configuration structure // Initialize the dshot_config structure with the arguments passed to the constructor
dshot_config.gpio_num = gpio; dshot_config.gpio_num = gpio;
dshot_config.pin_num = uint8_t(gpio); dshot_config.pin_num = static_cast<uint8_t>(gpio);
dshot_config.rmt_channel = rmtChannel; dshot_config.rmt_channel = rmtChannel;
dshot_config.mem_block_num = uint8_t(RMT_CHANNEL_MAX - uint8_t(rmtChannel)); dshot_config.mem_block_num = static_cast<uint8_t>(RMT_CHANNEL_MAX - static_cast<uint8_t>(rmtChannel));
// ...create empty packet // Create an empty packet using the DSHOT_NULL_PACKET and the buildTxRmtItem function
buildTxRmtItem(DSHOT_NULL_PACKET); buildTxRmtItem(DSHOT_NULL_PACKET);
} }
// Constructor that takes pin and channel as arguments
DShotRMT::DShotRMT(uint8_t pin, uint8_t channel) DShotRMT::DShotRMT(uint8_t pin, uint8_t channel)
{ {
// Initialize the configuration structure // Initialize the dshot_config structure with the arguments passed to the constructor
dshot_config.gpio_num = gpio_num_t(pin); dshot_config.gpio_num = static_cast<gpio_num_t>(pin);
dshot_config.pin_num = pin; dshot_config.pin_num = pin;
dshot_config.rmt_channel = rmt_channel_t(channel); dshot_config.rmt_channel = static_cast<rmt_channel_t>(channel);
dshot_config.mem_block_num = (RMT_CHANNEL_MAX - channel); dshot_config.mem_block_num = RMT_CHANNEL_MAX - channel;
// ...create empty packet // Create an empty packet using the DSHOT_NULL_PACKET and the buildTxRmtItem function
buildTxRmtItem(DSHOT_NULL_PACKET); buildTxRmtItem(DSHOT_NULL_PACKET);
} }
@ -43,59 +45,62 @@ DShotRMT::DShotRMT(DShotRMT const &)
bool DShotRMT::begin(dshot_mode_t dshot_mode, bool is_bidirectional) bool DShotRMT::begin(dshot_mode_t dshot_mode, bool is_bidirectional)
{ {
// Set DShot configuration parameters based on input parameters
dshot_config.mode = dshot_mode; dshot_config.mode = dshot_mode;
dshot_config.clk_div = DSHOT_CLK_DIVIDER; dshot_config.clk_div = DSHOT_CLK_DIVIDER;
dshot_config.name_str = dshot_mode_name[dshot_mode]; dshot_config.name_str = dshot_mode_name[dshot_mode];
dshot_config.is_bidirectional = is_bidirectional; dshot_config.is_bidirectional = is_bidirectional;
// Set timing parameters based on selected DShot mode
switch (dshot_config.mode) switch (dshot_config.mode)
{ {
case DSHOT150: case DSHOT150:
dshot_config.ticks_per_bit = 64; // ...Bit Period Time 6.67 us dshot_config.ticks_per_bit = 64;
dshot_config.ticks_zero_high = 24; // ...zero time 2.50 us dshot_config.ticks_zero_high = 24;
dshot_config.ticks_one_high = 48; // ...one time 5.00 us dshot_config.ticks_one_high = 48;
break; break;
case DSHOT300: case DSHOT300:
dshot_config.ticks_per_bit = 32; // ...Bit Period Time 3.33 us dshot_config.ticks_per_bit = 32;
dshot_config.ticks_zero_high = 12; // ...zero time 1.25 us dshot_config.ticks_zero_high = 12;
dshot_config.ticks_one_high = 24; // ...one time 2.50 us dshot_config.ticks_one_high = 24;
break; break;
case DSHOT600: case DSHOT600:
dshot_config.ticks_per_bit = 16; // ...Bit Period Time 1.67 us dshot_config.ticks_per_bit = 16;
dshot_config.ticks_zero_high = 6; // ...zero time 0.625 us dshot_config.ticks_zero_high = 6;
dshot_config.ticks_one_high = 12; // ...one time 1.25 us dshot_config.ticks_one_high = 12;
break; break;
case DSHOT1200: case DSHOT1200:
dshot_config.ticks_per_bit = 8; // ...Bit Period Time 0.83 us dshot_config.ticks_per_bit = 8;
dshot_config.ticks_zero_high = 3; // ...zero time 0.313 us dshot_config.ticks_zero_high = 3;
dshot_config.ticks_one_high = 6; // ...one time 0.625 us dshot_config.ticks_one_high = 6;
break; break;
// ...because having a default is "good style" // Default case to handle invalid input
default: default:
dshot_config.ticks_per_bit = 0; // ...Bit Period Time endless dshot_config.ticks_per_bit = 0;
dshot_config.ticks_zero_high = 0; // ...no bits, no time dshot_config.ticks_zero_high = 0;
dshot_config.ticks_one_high = 0; // ......no bits, no time dshot_config.ticks_one_high = 0;
break; break;
} }
// ...calc low signal timing // Calculate low signal timing
dshot_config.ticks_zero_low = (dshot_config.ticks_per_bit - dshot_config.ticks_zero_high); dshot_config.ticks_zero_low = (dshot_config.ticks_per_bit - dshot_config.ticks_zero_high);
dshot_config.ticks_one_low = (dshot_config.ticks_per_bit - dshot_config.ticks_one_high); dshot_config.ticks_one_low = (dshot_config.ticks_per_bit - dshot_config.ticks_one_high);
// Set up RMT configuration for DShot transmission
dshot_tx_rmt_config.rmt_mode = RMT_MODE_TX; dshot_tx_rmt_config.rmt_mode = RMT_MODE_TX;
dshot_tx_rmt_config.channel = dshot_config.rmt_channel; dshot_tx_rmt_config.channel = dshot_config.rmt_channel;
dshot_tx_rmt_config.gpio_num = dshot_config.gpio_num; dshot_tx_rmt_config.gpio_num = dshot_config.gpio_num;
dshot_tx_rmt_config.mem_block_num = dshot_config.mem_block_num; dshot_tx_rmt_config.mem_block_num = dshot_config.mem_block_num;
dshot_tx_rmt_config.clk_div = dshot_config.clk_div; dshot_tx_rmt_config.clk_div = dshot_config.clk_div;
dshot_tx_rmt_config.tx_config.loop_en = false; dshot_tx_rmt_config.tx_config.loop_en = false;
dshot_tx_rmt_config.tx_config.carrier_en = false; dshot_tx_rmt_config.tx_config.carrier_en = false;
dshot_tx_rmt_config.tx_config.idle_output_en = true; dshot_tx_rmt_config.tx_config.idle_output_en = true;
// Set idle level for RMT transmission based on input parameter
if (dshot_config.is_bidirectional) if (dshot_config.is_bidirectional)
{ {
dshot_tx_rmt_config.tx_config.idle_level = RMT_IDLE_LEVEL_HIGH; dshot_tx_rmt_config.tx_config.idle_level = RMT_IDLE_LEVEL_HIGH;
@ -105,87 +110,94 @@ bool DShotRMT::begin(dshot_mode_t dshot_mode, bool is_bidirectional)
dshot_tx_rmt_config.tx_config.idle_level = RMT_IDLE_LEVEL_LOW; dshot_tx_rmt_config.tx_config.idle_level = RMT_IDLE_LEVEL_LOW;
} }
// ...setup selected dshot mode // Set up selected DShot mode
rmt_config(&dshot_tx_rmt_config); rmt_config(&dshot_tx_rmt_config);
// ...essential step, return the result // Install RMT driver and return result
return rmt_driver_install(dshot_tx_rmt_config.channel, 0, 0); return rmt_driver_install(dshot_tx_rmt_config.channel, 0, 0);
} }
// ...the config part is done, now the calculating and sending part // Define a function to send a DShot command over an RMT interface to control a brushless motor's speed.
// void DShotRMT::sendThrottleValue(uint16_t throttle_value, telemetric_request_t telemetric_request)
void DShotRMT::sendThrottleValue(uint16_t throttle_value) void DShotRMT::sendThrottleValue(uint16_t throttle_value)
{ {
dshot_packet_t dshot_rmt_packet = {}; dshot_packet_t dshot_rmt_packet = {};
// Check if the throttle value is less than the minimum allowed value for the DShot protocol.
if (throttle_value < DSHOT_THROTTLE_MIN) if (throttle_value < DSHOT_THROTTLE_MIN)
{ {
throttle_value = DSHOT_THROTTLE_MIN; throttle_value = DSHOT_THROTTLE_MIN;
} }
// Check if the throttle value is greater than the maximum allowed value for the DShot protocol.
if (throttle_value > DSHOT_THROTTLE_MAX) if (throttle_value > DSHOT_THROTTLE_MAX)
{ {
throttle_value = DSHOT_THROTTLE_MAX; throttle_value = DSHOT_THROTTLE_MAX;
} }
// ...packets are the same for bidirectional mode
dshot_rmt_packet.throttle_value = throttle_value; dshot_rmt_packet.throttle_value = throttle_value;
// dshot_rmt_packet.telemetric_request = telemetric_request;
// Telemetric using additional pin on the ESC is not supported.
dshot_rmt_packet.telemetric_request = NO_TELEMETRIC; dshot_rmt_packet.telemetric_request = NO_TELEMETRIC;
// Calculate the checksum for the DShot packet using the calculateCRC function.
dshot_rmt_packet.checksum = calculateCRC(dshot_rmt_packet); dshot_rmt_packet.checksum = calculateCRC(dshot_rmt_packet);
// Send the DShot packet over the RMT interface to control the motor's speed.
sendRmtPaket(dshot_rmt_packet); sendRmtPaket(dshot_rmt_packet);
} }
// This method builds the RMT data transmission sequence for the DShot protocol
rmt_item32_t *DShotRMT::buildTxRmtItem(uint16_t parsed_packet) rmt_item32_t *DShotRMT::buildTxRmtItem(uint16_t parsed_packet)
{ {
// ...for bidirectional mode // Check if DShot is set to bidirectional mode
if (dshot_config.is_bidirectional) if (dshot_config.is_bidirectional)
{ {
// ..."invert" the high / low bits // If bidirectional, invert the high/low bits
for (int i = 0; i < DSHOT_PAUSE_BIT; i++, parsed_packet <<= 1) for (int i = 0; i < DSHOT_PAUSE_BIT; i++, parsed_packet <<= 1)
{ {
if (parsed_packet & 0b1000000000000000) if (parsed_packet & 0b1000000000000000)
{ {
// set one // Set RMT item for a logic high signal
dshot_tx_rmt_item[i].duration0 = dshot_config.ticks_one_low; dshot_tx_rmt_item[i].duration0 = dshot_config.ticks_one_low;
dshot_tx_rmt_item[i].duration1 = dshot_config.ticks_one_high; dshot_tx_rmt_item[i].duration1 = dshot_config.ticks_one_high;
} }
else else
{ {
// set zero // Set RMT item for a logic low signal
dshot_tx_rmt_item[i].duration0 = dshot_config.ticks_zero_low; dshot_tx_rmt_item[i].duration0 = dshot_config.ticks_zero_low;
dshot_tx_rmt_item[i].duration1 = dshot_config.ticks_zero_high; dshot_tx_rmt_item[i].duration1 = dshot_config.ticks_zero_high;
} }
// Set level of RMT item
dshot_tx_rmt_item[i].level0 = 0; dshot_tx_rmt_item[i].level0 = 0;
dshot_tx_rmt_item[i].level1 = 1; dshot_tx_rmt_item[i].level1 = 1;
} }
} }
// ..."normal" DShot mode / "bidirectional" mode OFF
else else
{ {
// If not bidirectional, set the RMT items as usual
for (int i = 0; i < DSHOT_PAUSE_BIT; i++, parsed_packet <<= 1) for (int i = 0; i < DSHOT_PAUSE_BIT; i++, parsed_packet <<= 1)
{ {
if (parsed_packet & 0b1000000000000000) if (parsed_packet & 0b1000000000000000)
{ {
// set one // Set RMT item for a logic high signal
dshot_tx_rmt_item[i].duration0 = dshot_config.ticks_one_high; dshot_tx_rmt_item[i].duration0 = dshot_config.ticks_one_high;
dshot_tx_rmt_item[i].duration1 = dshot_config.ticks_one_low; dshot_tx_rmt_item[i].duration1 = dshot_config.ticks_one_low;
} }
else else
{ {
// set zero // Set RMT item for a logic low signal
dshot_tx_rmt_item[i].duration0 = dshot_config.ticks_zero_high; dshot_tx_rmt_item[i].duration0 = dshot_config.ticks_zero_high;
dshot_tx_rmt_item[i].duration1 = dshot_config.ticks_zero_low; dshot_tx_rmt_item[i].duration1 = dshot_config.ticks_zero_low;
} }
// Set level of RMT item
dshot_tx_rmt_item[i].level0 = 1; dshot_tx_rmt_item[i].level0 = 1;
dshot_tx_rmt_item[i].level1 = 0; dshot_tx_rmt_item[i].level1 = 0;
} }
} }
// Set end marker for each frame
if (dshot_config.is_bidirectional) if (dshot_config.is_bidirectional)
{ {
dshot_tx_rmt_item[DSHOT_PAUSE_BIT].level0 = 1; dshot_tx_rmt_item[DSHOT_PAUSE_BIT].level0 = 1;
@ -197,39 +209,37 @@ rmt_item32_t *DShotRMT::buildTxRmtItem(uint16_t parsed_packet)
dshot_tx_rmt_item[DSHOT_PAUSE_BIT].level1 = 1; dshot_tx_rmt_item[DSHOT_PAUSE_BIT].level1 = 1;
} }
// ...end marker added to each frame // Add packet seperator aka DShot Pause.
dshot_tx_rmt_item[DSHOT_PAUSE_BIT].duration1 = DSHOT_PAUSE; dshot_tx_rmt_item[DSHOT_PAUSE_BIT].duration1 = DSHOT_PAUSE;
// Return the rmt_item
return dshot_tx_rmt_item; return dshot_tx_rmt_item;
} }
// Legacy CRC calculation // Calculates a CRC value for a DShot digital control signal packet
// uint16_t DShotRMT::calculateCRC(const dshot_packet_t &dshot_packet)
// {
// uint16_t packet = DSHOT_NULL_PACKET;
// uint16_t crc = DSHOT_NULL_PACKET;
// // Same initial 11 bits for both bidirectional and normal mode
// packet = (dshot_packet.throttle_value << 1) | dshot_packet.telemetric_request;
// if (dshot_config.is_bidirectional)
// {
// // Calculate checksum in inverted/bidirectional mode
// crc = (~(packet ^ (packet >> 4) ^ (packet >> 8))) & 0x0F;
// }
// else
// {
// // Calculate checksum in normal mode
// crc = (packet ^ (packet >> 4) ^ (packet >> 8)) & 0x0F;
// }
// return crc;
// }
// New way of calculating CRC
uint16_t DShotRMT::calculateCRC(const dshot_packet_t &dshot_packet) uint16_t DShotRMT::calculateCRC(const dshot_packet_t &dshot_packet)
{ {
uint16_t crc;
// Combine the throttle value and telemetric request flag into a 16-bit packet value
const uint16_t packet = (dshot_packet.throttle_value << 1) | dshot_packet.telemetric_request; const uint16_t packet = (dshot_packet.throttle_value << 1) | dshot_packet.telemetric_request;
const uint16_t crc = dshot_config.is_bidirectional
? (~(packet ^ (packet >> 4) ^ (packet >> 8))) & 0x0F // Calculate the CRC value using different bitwise operations depending on the DShot configuration
: (packet ^ (packet >> 4) ^ (packet >> 8)) & 0x0F; if (dshot_config.is_bidirectional)
{
// Bidirectional configuration: perform a bitwise negation of the result of XORing the packet with its right-shifted values by 4 and 8 bits,
// and then bitwise AND the result with 0x0F
const uint16_t intermediate_result = packet ^ (packet >> 4) ^ (packet >> 8);
crc = (~intermediate_result) & 0x0F;
}
else
{
// Unidirectional configuration: XOR the packet with its right-shifted values by 4 and 8 bits,
// and then bitwise AND the result with 0x0F
crc = (packet ^ (packet >> 4) ^ (packet >> 8)) & 0x0F;
}
// Return the calculated CRC value as a 16-bit unsigned integer
return crc; return crc;
} }

51
DShotRMT.h
View File

@ -86,31 +86,32 @@ typedef struct dshot_config_s
} dshot_config_t; } dshot_config_t;
// The official DShot Commands // The official DShot Commands
typedef enum dshot_cmd_e { typedef enum dshot_cmd_e
DSHOT_CMD_MOTOR_STOP = 0, // Currently not implemented - STOP Motors {
DSHOT_CMD_BEEP1, // Wait at least length of beep (380ms) before next command DSHOT_CMD_MOTOR_STOP = 0, // Currently not implemented - STOP Motors
DSHOT_CMD_BEEP2, // Wait at least length of beep (380ms) before next command DSHOT_CMD_BEEP1, // Wait at least length of beep (380ms) before next command
DSHOT_CMD_BEEP3, // Wait at least length of beep (400ms) before next command DSHOT_CMD_BEEP2, // Wait at least length of beep (380ms) before next command
DSHOT_CMD_BEEP4, // Wait at least length of beep (400ms) before next command DSHOT_CMD_BEEP3, // Wait at least length of beep (400ms) before next command
DSHOT_CMD_BEEP5, // Wait at least length of beep (400ms) before next command DSHOT_CMD_BEEP4, // Wait at least length of beep (400ms) before next command
DSHOT_CMD_ESC_INFO, // Currently not implemented DSHOT_CMD_BEEP5, // Wait at least length of beep (400ms) before next command
DSHOT_CMD_SPIN_DIRECTION_1, // Need 6x, no wait required DSHOT_CMD_ESC_INFO, // Currently not implemented
DSHOT_CMD_SPIN_DIRECTION_2, // Need 6x, no wait required DSHOT_CMD_SPIN_DIRECTION_1, // Need 6x, no wait required
DSHOT_CMD_3D_MODE_OFF, // Need 6x, no wait required DSHOT_CMD_SPIN_DIRECTION_2, // Need 6x, no wait required
DSHOT_CMD_3D_MODE_ON, // Need 6x, no wait required DSHOT_CMD_3D_MODE_OFF, // Need 6x, no wait required
DSHOT_CMD_SETTINGS_REQUEST, // Currently not implemented DSHOT_CMD_3D_MODE_ON, // Need 6x, no wait required
DSHOT_CMD_SAVE_SETTINGS, // Need 6x, wait at least 12ms before next command DSHOT_CMD_SETTINGS_REQUEST, // Currently not implemented
DSHOT_CMD_SPIN_DIRECTION_NORMAL, // Need 6x, no wait required DSHOT_CMD_SAVE_SETTINGS, // Need 6x, wait at least 12ms before next command
DSHOT_CMD_SPIN_DIRECTION_REVERSED, // Need 6x, no wait required DSHOT_CMD_SPIN_DIRECTION_NORMAL, // Need 6x, no wait required
DSHOT_CMD_LED0_ON, // Currently not implemented DSHOT_CMD_SPIN_DIRECTION_REVERSED, // Need 6x, no wait required
DSHOT_CMD_LED1_ON, // Currently not implemented DSHOT_CMD_LED0_ON, // Currently not implemented
DSHOT_CMD_LED2_ON, // Currently not implemented DSHOT_CMD_LED1_ON, // Currently not implemented
DSHOT_CMD_LED3_ON, // Currently not implemented DSHOT_CMD_LED2_ON, // Currently not implemented
DSHOT_CMD_LED0_OFF, // Currently not implemented DSHOT_CMD_LED3_ON, // Currently not implemented
DSHOT_CMD_LED1_OFF, // Currently not implemented DSHOT_CMD_LED0_OFF, // Currently not implemented
DSHOT_CMD_LED2_OFF, // Currently not implemented DSHOT_CMD_LED1_OFF, // Currently not implemented
DSHOT_CMD_LED3_OFF, // Currently not implemented DSHOT_CMD_LED2_OFF, // Currently not implemented
DSHOT_CMD_MAX = 47 DSHOT_CMD_LED3_OFF, // Currently not implemented
DSHOT_CMD_MAX = 47
} dshot_cmd_t; } dshot_cmd_t;
// The main DShotRMT class // The main DShotRMT class