...added getMotorRPM Function
This commit is contained in:
parent
c2f1bf84d4
commit
8557930d3e
105
DShotRMT.cpp
105
DShotRMT.cpp
|
|
@ -97,59 +97,76 @@ void DShotRMT::setThrottle(uint16_t throttle)
|
||||||
// Receives and decodes a response frame from ESC containing eRPM info
|
// Receives and decodes a response frame from ESC containing eRPM info
|
||||||
uint32_t DShotRMT::getERPM()
|
uint32_t DShotRMT::getERPM()
|
||||||
{
|
{
|
||||||
static size_t rx_size = sizeof(_rx_symbols);
|
if (_isBidirectional)
|
||||||
|
|
||||||
if (_rmt_rx_channel == nullptr)
|
|
||||||
return _last_erpm;
|
|
||||||
|
|
||||||
// Attempt to receive a new frame
|
|
||||||
if (!rmt_receive(_rmt_rx_channel, _rx_symbols, rx_size, &_receive_config))
|
|
||||||
return _last_erpm;
|
|
||||||
|
|
||||||
uint16_t received_bits = 0;
|
|
||||||
_received_packet = 0;
|
|
||||||
|
|
||||||
// Decode raw RMT encoded bits
|
|
||||||
for (int i = 0; i < DSHOT_BITS_PER_FRAME; ++i)
|
|
||||||
{
|
{
|
||||||
rmt_symbol_word_t symbols = _rx_symbols[i];
|
static size_t rx_size = sizeof(_rx_symbols);
|
||||||
|
|
||||||
// Validate signal polarity
|
if (_rmt_rx_channel == nullptr)
|
||||||
if (symbols.level0 != 1 || symbols.level1 != 0)
|
return _last_erpm;
|
||||||
break;
|
|
||||||
|
|
||||||
uint32_t total_ticks = symbols.duration0 + symbols.duration1;
|
// Attempt to receive a new frame
|
||||||
bool bit = (symbols.duration0 > (total_ticks / 2));
|
if (!rmt_receive(_rmt_rx_channel, _rx_symbols, rx_size, &_receive_config))
|
||||||
|
return _last_erpm;
|
||||||
|
|
||||||
_received_packet <<= 1;
|
uint16_t received_bits = 0;
|
||||||
_received_packet |= bit ? 1 : 0;
|
_received_packet = 0;
|
||||||
|
|
||||||
received_bits++;
|
// Decode raw RMT encoded bits
|
||||||
|
for (int i = 0; i < DSHOT_BITS_PER_FRAME; ++i)
|
||||||
|
{
|
||||||
|
rmt_symbol_word_t symbols = _rx_symbols[i];
|
||||||
|
|
||||||
|
// Validate signal polarity
|
||||||
|
if (symbols.level0 != 1 || symbols.level1 != 0)
|
||||||
|
break;
|
||||||
|
|
||||||
|
uint32_t total_ticks = symbols.duration0 + symbols.duration1;
|
||||||
|
bool bit = (symbols.duration0 > (total_ticks / 2));
|
||||||
|
|
||||||
|
_received_packet <<= 1;
|
||||||
|
_received_packet |= bit ? 1 : 0;
|
||||||
|
|
||||||
|
received_bits++;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (received_bits < 16)
|
||||||
|
return _last_erpm;
|
||||||
|
|
||||||
|
// Extract data & checksum from packet
|
||||||
|
uint16_t packet_data = _received_packet >> 4;
|
||||||
|
uint8_t recalc_packet_crc = (packet_data ^ (packet_data >> 4) ^ (packet_data >> 8)) & 0x0F;
|
||||||
|
uint8_t packet_crc = _received_packet & 0x0F;
|
||||||
|
|
||||||
|
if (recalc_packet_crc != packet_crc)
|
||||||
|
return _last_erpm;
|
||||||
|
|
||||||
|
// Assume received value is DShot eRPM
|
||||||
|
uint16_t throttle = packet_data >> 1;
|
||||||
|
|
||||||
|
// Filter noise values
|
||||||
|
if (throttle < DSHOT_THROTTLE_MIN || throttle > DSHOT_THROTTLE_MAX)
|
||||||
|
return _last_erpm;
|
||||||
|
|
||||||
|
// Approximate eRPM (ESC dependent, scale factor can be tuned)
|
||||||
|
_last_erpm = throttle * 100;
|
||||||
|
return _last_erpm;
|
||||||
}
|
}
|
||||||
|
// Nothing to do here
|
||||||
if (received_bits < 16)
|
|
||||||
return _last_erpm;
|
|
||||||
|
|
||||||
// Extract data & checksum from packet
|
|
||||||
uint16_t packet_data = _received_packet >> 4;
|
|
||||||
uint8_t recalc_packet_crc = (packet_data ^ (packet_data >> 4) ^ (packet_data >> 8)) & 0x0F;
|
|
||||||
uint8_t packet_crc = _received_packet & 0x0F;
|
|
||||||
|
|
||||||
if (recalc_packet_crc != packet_crc)
|
|
||||||
return _last_erpm;
|
|
||||||
|
|
||||||
// Assume received value is DShot eRPM
|
|
||||||
uint16_t throttle = packet_data >> 1;
|
|
||||||
|
|
||||||
// Filter noise values
|
|
||||||
if (throttle < DSHOT_THROTTLE_MIN || throttle > DSHOT_THROTTLE_MAX)
|
|
||||||
return _last_erpm;
|
|
||||||
|
|
||||||
// Approximate eRPM (ESC dependent, scale factor can be tuned)
|
|
||||||
_last_erpm = throttle * 100;
|
|
||||||
return _last_erpm;
|
return _last_erpm;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Translate eRPM value to RPM taking magnet count as parameter
|
||||||
|
uint32_t DShotRMT::getMotorRPM(uint8_t magnet_count)
|
||||||
|
{
|
||||||
|
uint8_t pole_count = magnet_count / 2;
|
||||||
|
|
||||||
|
if (pole_count == 0)
|
||||||
|
pole_count = 1;
|
||||||
|
|
||||||
|
uint32_t rpm = getERPM() / pole_count;
|
||||||
|
return rpm;
|
||||||
|
}
|
||||||
|
|
||||||
// --- Encode DShot TX Frame ---
|
// --- Encode DShot TX Frame ---
|
||||||
// Converts a 16-bit packet into a valid DShot Frame for RMT
|
// Converts a 16-bit packet into a valid DShot Frame for RMT
|
||||||
void DShotRMT::encodeDShotTX(uint16_t dshot_packet, rmt_symbol_word_t *symbols, size_t &count)
|
void DShotRMT::encodeDShotTX(uint16_t dshot_packet, rmt_symbol_word_t *symbols, size_t &count)
|
||||||
|
|
|
||||||
11
DShotRMT.h
11
DShotRMT.h
|
|
@ -55,19 +55,17 @@ public:
|
||||||
// Initializes the RMT TX and RX channels
|
// Initializes the RMT TX and RX channels
|
||||||
void begin();
|
void begin();
|
||||||
|
|
||||||
// Sets a new throttle value (0-2047) and sends it repeatedly
|
// Sets a new throttle value (48-2047) and sends it repeatedly
|
||||||
void setThrottle(uint16_t throttle);
|
void setThrottle(uint16_t throttle);
|
||||||
|
|
||||||
// Receives and decodes the latest eRPM value from ESC, if available
|
// Receives and decodes the latest value from ESC, if available
|
||||||
uint32_t getERPM();
|
uint32_t getERPM();
|
||||||
|
uint32_t getMotorRPM(uint8_t magnet_count);
|
||||||
|
|
||||||
// Accessors for GPIO and DShot mode
|
// Accessors for GPIO and DShot mode
|
||||||
gpio_num_t getGPIO() const { return _gpio; }
|
gpio_num_t getGPIO() const { return _gpio; }
|
||||||
dshot_mode_t getDShotMode() const { return _mode; }
|
dshot_mode_t getDShotMode() const { return _mode; }
|
||||||
|
|
||||||
// Stores the last valid eRPM received from the ESC
|
|
||||||
uint32_t _last_erpm = 0;
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// Converts a 16-bit DShot packet into RMT symbols and appends pause
|
// Converts a 16-bit DShot packet into RMT symbols and appends pause
|
||||||
void encodeDShotTX(uint16_t dshot_packet, rmt_symbol_word_t *symbols, size_t &count);
|
void encodeDShotTX(uint16_t dshot_packet, rmt_symbol_word_t *symbols, size_t &count);
|
||||||
|
|
@ -96,4 +94,7 @@ private:
|
||||||
// --- RMT Symbol Buffer ---
|
// --- RMT Symbol Buffer ---
|
||||||
rmt_symbol_word_t _rx_symbols[RX_BUFFER_SIZE] = {};
|
rmt_symbol_word_t _rx_symbols[RX_BUFFER_SIZE] = {};
|
||||||
rmt_symbol_word_t _tx_symbols[TX_BUFFER_SIZE] = {};
|
rmt_symbol_word_t _tx_symbols[TX_BUFFER_SIZE] = {};
|
||||||
|
|
||||||
|
// Stores the last valid eRPM received from the ESC
|
||||||
|
uint32_t _last_erpm = 0;
|
||||||
};
|
};
|
||||||
|
|
|
||||||
|
|
@ -20,6 +20,9 @@ constexpr auto DSHOT_MODE = DSHOT300;
|
||||||
// BiDirectional DShot Support (default: false)
|
// BiDirectional DShot Support (default: false)
|
||||||
constexpr auto IS_BIDIRECTIONAL = true;
|
constexpr auto IS_BIDIRECTIONAL = true;
|
||||||
|
|
||||||
|
// Motor Magnet count for RPM calculation
|
||||||
|
constexpr auto MOTOR01_MAGNET_COUNT = 14;
|
||||||
|
|
||||||
// Setup Motor Pin, DShot Mode and optional BiDirectional Support
|
// Setup Motor Pin, DShot Mode and optional BiDirectional Support
|
||||||
DShotRMT motor01(MOTOR01_PIN, DSHOT_MODE, IS_BIDIRECTIONAL);
|
DShotRMT motor01(MOTOR01_PIN, DSHOT_MODE, IS_BIDIRECTIONAL);
|
||||||
|
|
||||||
|
|
@ -37,7 +40,7 @@ void setup()
|
||||||
//
|
//
|
||||||
USB_Serial.println("**********************");
|
USB_Serial.println("**********************");
|
||||||
USB_Serial.println("DShotRMT Demo started.");
|
USB_Serial.println("DShotRMT Demo started.");
|
||||||
USB_Serial.println("Enter a throttle value (48 – 2047):");
|
USB_Serial.println("Enter a throttle value (48–2047):");
|
||||||
}
|
}
|
||||||
|
|
||||||
void loop()
|
void loop()
|
||||||
|
|
@ -58,12 +61,12 @@ void loop()
|
||||||
{
|
{
|
||||||
last_print_time = now;
|
last_print_time = now;
|
||||||
|
|
||||||
uint32_t erpm = motor01.getERPM();
|
uint32_t rpm = motor01.getMotorRPM(MOTOR01_MAGNET_COUNT);
|
||||||
|
|
||||||
USB_Serial.print("Sent Throttle: ");
|
USB_Serial.print("Throttle: ");
|
||||||
USB_Serial.print(throttle_input);
|
USB_Serial.print(throttle_input);
|
||||||
USB_Serial.print(" | eRPM: ");
|
USB_Serial.print(" | RPM: ");
|
||||||
USB_Serial.println(erpm);
|
USB_Serial.println(rpm);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -86,7 +89,7 @@ int readSerialThrottle()
|
||||||
USB_Serial.print("Throttle set to: ");
|
USB_Serial.print("Throttle set to: ");
|
||||||
USB_Serial.println(last_throttle);
|
USB_Serial.println(last_throttle);
|
||||||
USB_Serial.println("***********************************");
|
USB_Serial.println("***********************************");
|
||||||
USB_Serial.println("Enter a throttle value (48 – 2047):");
|
USB_Serial.println("Enter a throttle value (48–2047):");
|
||||||
}
|
}
|
||||||
|
|
||||||
return last_throttle;
|
return last_throttle;
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue