Skip to content

Commit

Permalink
tune dshot telemetry + more tests
Browse files Browse the repository at this point in the history
  • Loading branch information
@rtlopez
rtlopez committed Feb 21, 2024
1 parent 931bf7e commit 9e68c06
Show file tree
Hide file tree
Showing 6 changed files with 178 additions and 82 deletions.
4 changes: 2 additions & 2 deletions docs/dshot_gcr_dump.txt
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
// bluejay esc, dshot 300

// power off
// power on
32 0:375 1:168 0:10368 0:14 0:381 1:162 0:172 1:172 0:384 1:159 0:178 1:168 0:166 1:168 0:603 2950 2950
32 0:381 1:165 0:172 1:172 0:377 1:176 0:165 1:171 0:381 1:169 0:168 1:172 0:175 1:172 0:593 52950 52950
32 0:381 1:172 0:171 1:165 0:385 1:168 0:168 1:169 0:381 1:169 0:178 1:162 0:175 1:171 0:607 52950 52950
Expand All @@ -11,7 +11,7 @@
32 0:379 1:167 0:170 1:174 0:376 1:176 0:164 1:173 0:380 1:170 0:167 1:173 0:173 1:174 0:592 52950 52950
32 0:380 1:173 0:177 1:170 0:382 1:174 0:173 1:163 0:380 1:173 0:174 1:160 0:177 1:170 0:608 52950 52950

// power on
// running
24 0:373 1:605 0:163 1:605 0:174 1:376 0:379 1:599 0:177 1:163 0:173 1EF67A 1EF67A
24 0:379 1:599 0:170 1:654 0:170 1:170 0:184 1:605 0:173 1:379 0:380 1EF5EC 1EF5EC
24 0:383 1:605 0:163 1:602 0:177 1:163 0:383 1:170 0:170 1:167 0:605 F7A50 F7A50
Expand Down
24 changes: 16 additions & 8 deletions lib/EscDriver/src/EscDriver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -121,18 +121,25 @@ class EscDriverBase
{
value = pushBits(value, 0x1, 21 - bitCount);
}
//value = value ^ (value >> 1); // extract gcr

return value;
}

static const uint32_t INVALID_TELEMETRY_VALUE = 0xffff;
static constexpr uint32_t INVALID_TELEMETRY_VALUE = 0xffff;
static constexpr int SECONDS_PER_MINUTE = 60;
static constexpr float ERPM_PER_LSB = 100.0f;

static inline float getErpmToHzRatio(int poles)
{
return ERPM_PER_LSB / SECONDS_PER_MINUTE / (poles / 2.0f);
}

static inline uint32_t convertToErpm(uint32_t value)
{
if(!value) return 0;

if(!value || value == INVALID_TELEMETRY_VALUE) {
if(!value || value == INVALID_TELEMETRY_VALUE)
{
return INVALID_TELEMETRY_VALUE;
}

Expand All @@ -143,7 +150,8 @@ class EscDriverBase
static inline uint32_t convertToValue(uint32_t value)
{
// eRPM range
if(value == 0x0fff) {
if(value == 0x0fff)
{
return 0;
}

Expand Down Expand Up @@ -172,11 +180,11 @@ class EscDriverBase
csum = csum ^ (csum >> 8); // xor bytes
csum = csum ^ (csum >> 4); // xor nibbles

if((csum & 0xf) != 0xf || decodedValue > 0xffff) {
value = INVALID_TELEMETRY_VALUE;
} else {
value = decodedValue >> 4;
if((csum & 0xf) != 0xf || decodedValue > 0xffff)
{
return INVALID_TELEMETRY_VALUE;
}
value = decodedValue >> 4;

return value;
}
Expand Down
67 changes: 32 additions & 35 deletions lib/EscDriver/src/EscDriverEsp32.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,8 @@ IRAM_ATTR static esp_err_t _rmt_fill_tx_items(rmt_channel_t channel, const rmt_i
}
IRAM_ATTR static esp_err_t _rmt_tx_start(rmt_channel_t channel, bool tx_idx_rst)
{
if(tx_idx_rst) {
if(tx_idx_rst)
{
rmt_ll_tx_reset_pointer(&RMT, channel); // BC
}
rmt_ll_rx_set_mem_owner(&RMT, channel, RMT_MEM_OWNER_TX); // moved to _rmt_fill_tx_items
Expand All @@ -53,7 +54,8 @@ IRAM_ATTR static esp_err_t _rmt_tx_start(rmt_channel_t channel, bool tx_idx_rst)
static void IRAM_ATTR _rmt_zero_mem(rmt_channel_t channel, size_t len)
{
volatile rmt_item32_t *data = (rmt_item32_t *)RMTMEM.chan[channel].data32;
for (size_t idx = 0; idx < len; idx++) {
for (size_t idx = 0; idx < len; idx++)
{
data[idx].val = 0;
}
}
Expand Down Expand Up @@ -140,7 +142,7 @@ void EscDriverEsp32::initChannel(int i, gpio_num_t pin, int pulse)
_channel[i].divider = getClockDivider();
_channel[i].pulse_min = getPulseMin();
_channel[i].pulse_max = getPulseMax();
_channel[i].pulse_space = getPulseInterval();
_channel[i].pulse_space = nsToTicks(_interval * 1000);

// specification 0:37%, 1:75% of 1670ns for dshot600
//_channel[i].dshot_t0h = getDshotPulse(625);
Expand All @@ -149,10 +151,11 @@ void EscDriverEsp32::initChannel(int i, gpio_num_t pin, int pulse)
//_channel[i].dshot_t1l = getDshotPulse(420);

// betaflight 0:35%, 1:70% of 1670ns for dshot600
_channel[i].dshot_t0h = getDshotPulse(584 - 16);
_channel[i].dshot_t0l = getDshotPulse(1086 - 32);
_channel[i].dshot_t1h = getDshotPulse(1170 - 32);
_channel[i].dshot_t1l = getDshotPulse(500 - 16);
_channel[i].dshot_t0h = getDshotPulse(584);
_channel[i].dshot_t0l = getDshotPulse(1086);
_channel[i].dshot_t1h = getDshotPulse(1170);
_channel[i].dshot_t1l = getDshotPulse(500);
_channel[i].dshot_tlm_bit_len = (_channel[i].dshot_t0h + _channel[i].dshot_t0l) * 4 / 5;

instances[i] = this;

Expand All @@ -164,12 +167,12 @@ void EscDriverEsp32::initChannel(int i, gpio_num_t pin, int pulse)

// add RX specifics
int rx_ch = RMT_ENCODE_RX_CHANNEL(i);
rmt_ll_rx_set_idle_thres(&RMT, rx_ch, getAnalogPulse(30000)); // max bit len, (30us)
rmt_ll_rx_set_filter_thres(&RMT, rx_ch, 30); // min bit len, 80 ticks = 1us for div = 1, max value = 255
rmt_ll_rx_enable_filter(&RMT, rx_ch, 1);
rmt_ll_rx_set_idle_thres(&RMT, rx_ch, nsToTicks(30000)); // max bit len, (30us)
rmt_ll_rx_set_filter_thres(&RMT, rx_ch, 40); // min bit len, 80 ticks = 1us for div = 1, max value = 255
rmt_ll_rx_enable_filter(&RMT, rx_ch, true);
_rmt_zero_mem((rmt_channel_t)rx_ch, RMT_MEM_ITEM_NUM);

#if SOC_RMT_SUPPORT_TX_SYNCHRO
#if 0 && SOC_RMT_SUPPORT_TX_SYNCHRO
// sync all channels
if(!_async)
{
Expand Down Expand Up @@ -248,7 +251,7 @@ void EscDriverEsp32::enableRx(rmt_channel_t channel)
rmt_ll_rx_reset_pointer(&RMT, rx_ch);
rmt_ll_clear_rx_end_interrupt(&RMT, rx_ch);
rmt_ll_enable_rx_end_interrupt(&RMT, rx_ch, true);
_rmt_zero_mem(rx_ch, 1); // clear first item of rx buffer to avoid reading tx items
_rmt_zero_mem(rx_ch, 2); // clear first item of rx buffer to avoid reading tx items
rmt_ll_rx_enable(&RMT, rx_ch, true);
}

Expand Down Expand Up @@ -316,7 +319,6 @@ void EscDriverEsp32::transmitAll()
void EscDriverEsp32::readTelemetry()
{
//PIN_DEBUG(HIGH);
uint32_t bit_len = getAnalogPulse(2136);
for (size_t i = 0; i < ESC_CHANNEL_COUNT; i++)
{
if(!_digital || !_dshot_tlm) continue;
Expand Down Expand Up @@ -346,7 +348,7 @@ void EscDriverEsp32::readTelemetry()
if(s) s->print(':');
if(s) s->print(data[j].duration1);
}*/
uint32_t value = extractTelemetryGcr((uint32_t*)data, rmt_len >> 2, bit_len);
uint32_t value = extractTelemetryGcr((uint32_t*)data, rmt_len >> 2, _channel[i].dshot_tlm_bit_len);

//if(s) s->print(' ');
//if(s) s->print(value, HEX);
Expand Down Expand Up @@ -426,7 +428,7 @@ uint32_t EscDriverEsp32::getClockDivider() const
case ESC_PROTOCOL_PWM: return 24;
case ESC_PROTOCOL_ONESHOT125: return 4;
case ESC_PROTOCOL_ONESHOT42: return 2;
case ESC_PROTOCOL_MULTISHOT: return 2;
case ESC_PROTOCOL_MULTISHOT: return 1;
case ESC_PROTOCOL_BRUSHED: return 8;
default: return 1;
}
Expand All @@ -436,10 +438,10 @@ uint32_t EscDriverEsp32::getPulseMin() const
{
switch (_protocol)
{
case ESC_PROTOCOL_PWM: return getAnalogPulse(1000 * 1000);
case ESC_PROTOCOL_ONESHOT125: return getAnalogPulse(125 * 1000);
case ESC_PROTOCOL_ONESHOT42: return getAnalogPulse(42 * 1000);
case ESC_PROTOCOL_MULTISHOT: return getAnalogPulse(5 * 1000);
case ESC_PROTOCOL_PWM: return nsToTicks(1000 * 1000);
case ESC_PROTOCOL_ONESHOT125: return nsToTicks(125 * 1000);
case ESC_PROTOCOL_ONESHOT42: return nsToTicks(42 * 1000);
case ESC_PROTOCOL_MULTISHOT: return nsToTicks(5 * 1000);
case ESC_PROTOCOL_BRUSHED: return 0;
case ESC_PROTOCOL_DSHOT150:
case ESC_PROTOCOL_DSHOT300:
Expand All @@ -453,11 +455,11 @@ uint32_t EscDriverEsp32::getPulseMax() const
{
switch (_protocol)
{
case ESC_PROTOCOL_PWM: return getAnalogPulse(2000 * 1000);
case ESC_PROTOCOL_ONESHOT125: return getAnalogPulse(250 * 1000);
case ESC_PROTOCOL_ONESHOT42: return getAnalogPulse(84 * 1000);
case ESC_PROTOCOL_MULTISHOT: return getAnalogPulse(25 * 1000);
case ESC_PROTOCOL_BRUSHED: return getAnalogPulse(_interval * 1000);
case ESC_PROTOCOL_PWM: return nsToTicks(2000 * 1000);
case ESC_PROTOCOL_ONESHOT125: return nsToTicks(250 * 1000);
case ESC_PROTOCOL_ONESHOT42: return nsToTicks(84 * 1000);
case ESC_PROTOCOL_MULTISHOT: return nsToTicks(25 * 1000);
case ESC_PROTOCOL_BRUSHED: return nsToTicks(_interval * 1000);
case ESC_PROTOCOL_DSHOT150:
case ESC_PROTOCOL_DSHOT300:
case ESC_PROTOCOL_DSHOT600:
Expand All @@ -466,25 +468,20 @@ uint32_t EscDriverEsp32::getPulseMax() const
}
}

uint32_t EscDriverEsp32::getPulseInterval() const
{
return getAnalogPulse(_interval * 1000);
}

uint32_t EscDriverEsp32::getAnalogPulse(int32_t ns) const
uint32_t EscDriverEsp32::nsToTicks(uint32_t ns) const
{
int div = getClockDivider();
return ns / ((div * DURATION_CLOCK) / 2);
// multiply by 2 as DURATION_CLOCK is aleredy doubled, and round to nearest integer
return (ns * 2 + ((div * DURATION_CLOCK) >> 1)) / div / DURATION_CLOCK;
}

uint16_t EscDriverEsp32::getDshotPulse(uint32_t width) const
{
int div = getClockDivider();
switch (_protocol)
{
case ESC_PROTOCOL_DSHOT150: return ((width * 4) / (div * DURATION_CLOCK / 2));
case ESC_PROTOCOL_DSHOT300: return ((width * 2) / (div * DURATION_CLOCK / 2));
case ESC_PROTOCOL_DSHOT600: return ((width * 1) / (div * DURATION_CLOCK / 2));
case ESC_PROTOCOL_DSHOT150: return nsToTicks(width * 4);
case ESC_PROTOCOL_DSHOT300: return nsToTicks(width * 2);
case ESC_PROTOCOL_DSHOT600: return nsToTicks(width * 1);
case ESC_PROTOCOL_BRUSHED:
case ESC_PROTOCOL_PWM:
case ESC_PROTOCOL_ONESHOT125:
Expand Down
4 changes: 2 additions & 2 deletions lib/EscDriver/src/EscDriverEsp32.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,7 @@ class EscDriverEsp32: public EscDriverBase
uint16_t dshot_t0l;
uint16_t dshot_t1h;
uint16_t dshot_t1l;
uint16_t dshot_tlm_bit_len;
uint32_t telemetryValue;
};

Expand Down Expand Up @@ -104,8 +105,7 @@ class EscDriverEsp32: public EscDriverBase
uint32_t getClockDivider() const;
uint32_t getPulseMin() const;
uint32_t getPulseMax() const;
uint32_t getPulseInterval() const;
uint32_t getAnalogPulse(int32_t ns) const;
uint32_t nsToTicks(uint32_t ns) const;
uint16_t getDshotPulse(uint32_t width) const;
bool isDigital(EscProtocol protocol) const;

Expand Down
9 changes: 2 additions & 7 deletions lib/Espfc/src/Output/Mixer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,9 +6,6 @@
#include "EscDriver.h"
#include "platform.h"

#define ERPM_PER_LSB 100.0f
#define SECONDS_PER_MINUTE 60

namespace Espfc {

namespace Output {
Expand Down Expand Up @@ -44,7 +41,7 @@ class Mixer
_model.state.escServo = _servo = &escServo;
_model.logger.info().log(F("SERVO CONF")).log(ESC_PROTOCOL_PWM).log(true).logln(_model.config.output.servoRate).logln(ESC_DRIVER_SERVO_TIMER);
}
_erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE / (_model.config.output.motorPoles / 2.0f);
_erpmToHz = EscDriver::getErpmToHzRatio(_model.config.output.motorPoles);
_statsCounterMax = _model.state.mixerTimer.rate / 2;
_statsCounter = 0;

Expand Down Expand Up @@ -373,14 +370,12 @@ class Mixer

float inline erpmToHz(float erpm)
{
// rpm = (erpm * ERPM_PER_LSB) / (_model.config.output.motorPoles / 2)
// _erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE / (_model.config.output.motorPoles / 2.0f);
return _erpmToHz * erpm;
}

float inline erpmToRpm(float erpm)
{
return erpmToHz(erpm) * SECONDS_PER_MINUTE;
return erpmToHz(erpm) * EscDriver::SECONDS_PER_MINUTE;
}

bool _stop(void)
Expand Down
Loading

0 comments on commit 9e68c06

Please sign in to comment.