Loading files
#include "EscDriverBase.hpp"
#include <Arduino.h>
const char * const * EscDriverBase::getProtocolNames()
{
static const char * const protocols[] = {
"PWM", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED",
"DSHOT150", "DSHOT300", "DSHOT600", "PROSHOT1000", "DISABLED",
nullptr
};
return protocols;
}
const char * const EscDriverBase::getProtocolName(EscProtocol protocol)
{
if(protocol >= ESC_PROTOCOL_COUNT) return "?";
return getProtocolNames()[protocol];
}
uint16_t IRAM_ATTR EscDriverBase::dshotConvert(uint16_t pulse)
{
return pulse > 1000 ? PWM_TO_DSHOT(pulse) : 0;
}
uint16_t IRAM_ATTR EscDriverBase::dshotEncode(uint16_t value, bool inverted)
{
value <<= 1;
// compute checksum
int csum = 0;
int csum_data = value;
for (int i = 0; i < 3; i++)
{
csum ^= csum_data; // xor
csum_data >>= 4;
}
if(inverted)
{
csum = ~csum;
}
csum &= 0xf;
return (value << 4) | csum;
}
uint32_t IRAM_ATTR EscDriverBase::durationToBitLen(uint32_t duration, uint32_t len)
{
return (duration + (len >> 1)) / len;
}
uint32_t IRAM_ATTR EscDriverBase::pushBits(uint32_t value, uint32_t bitVal, size_t bitLen)
{
while(bitLen--)
{
value <<= 1;
value |= bitVal;
}
return value;
}
/**
* @param data expected data layout (bits): duration0(15), level0(1), duration(15), level1(1)
* @param len number of data items
* @param bitLen duration of single bit
* @return uint32_t raw gcr value
*/
uint32_t IRAM_ATTR EscDriverBase::extractTelemetryGcr(uint32_t* data, size_t len, uint32_t bitLen)
{
int bitCount = 0;
uint32_t value = 0;
for(size_t i = 0; i < len; i++)
{
uint32_t duration0 = data[i] & 0x7fff;
if(!duration0) break;
uint32_t level0 = (data[i] >> 15) & 0x01;
uint32_t len0 = durationToBitLen(duration0, bitLen);
if(len0)
{
value = pushBits(value, level0, len0);
bitCount += len0;
}
uint32_t duration1 = (data[i] >> 16) & 0x7fff;
if(!duration1) break;
uint32_t level1 = (data[i] >> 31) & 0x01;
uint32_t len1 = durationToBitLen(duration1, bitLen);
if(len1)
{
value = pushBits(value, level1, len1);
bitCount += len1;
}
}
// fill missing bits with 1
if(bitCount < 21)
{
value = pushBits(value, 0x1, 21 - bitCount);
}
return value;
}
float IRAM_ATTR EscDriverBase::getErpmToHzRatio(int poles)
{
return ERPM_PER_LSB / SECONDS_PER_MINUTE / (poles / 2.0f);
}
uint32_t IRAM_ATTR EscDriverBase::convertToErpm(uint32_t value)
{
if(!value) return 0;
if(!value || value == INVALID_TELEMETRY_VALUE)
{
return INVALID_TELEMETRY_VALUE;
}
// Convert period to erpm * 100
return (1000000 * 60 / 100 + value / 2) / value;
}
uint32_t IRAM_ATTR EscDriverBase::convertToValue(uint32_t value)
{
// eRPM range
if(value == 0x0fff)
{
return 0;
}
// Convert value to 16 bit from the GCR telemetry format (eeem mmmm mmmm)
return (value & 0x01ff) << ((value & 0xfe00) >> 9);
}
uint32_t IRAM_ATTR EscDriverBase::gcrToRawValue(uint32_t value)
{
value = value ^ (value >> 1); // extract gcr
constexpr uint32_t iv = 0xffffffff; // invalid code
// First bit is start bit so discard it.
value &= 0xfffff;
static const uint32_t decode[32] = {
iv, iv, iv, iv, iv, iv, iv, iv, iv, 9, 10, 11, iv, 13, 14, 15,
iv, iv, 2, 3, iv, 5, 6, 7, iv, 0, 8, 1, iv, 4, 12, iv,
};
uint32_t decodedValue = decode[value & 0x1f];
decodedValue |= decode[(value >> 5) & 0x1f] << 4;
decodedValue |= decode[(value >> 10) & 0x1f] << 8;
decodedValue |= decode[(value >> 15) & 0x1f] << 12;
uint32_t csum = decodedValue;
csum = csum ^ (csum >> 8); // xor bytes
csum = csum ^ (csum >> 4); // xor nibbles
if((csum & 0xf) != 0xf || decodedValue > 0xffff)
{
return INVALID_TELEMETRY_VALUE;
}
value = decodedValue >> 4;
return value;
}
ee3ae5effThis file is pinned to the selected release. Download the original to open it in your design tool.