From 9e68c06fbc4e48ac86f38f2aa5d034098fec9e17 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Wed, 21 Feb 2024 23:44:17 +0100 Subject: [PATCH] tune dshot telemetry + more tests --- docs/dshot_gcr_dump.txt | 4 +- lib/EscDriver/src/EscDriver.h | 24 +++-- lib/EscDriver/src/EscDriverEsp32.cpp | 67 ++++++------ lib/EscDriver/src/EscDriverEsp32.h | 4 +- lib/Espfc/src/Output/Mixer.h | 9 +- test/test_esc/test_esc.cpp | 152 ++++++++++++++++++++++----- 6 files changed, 178 insertions(+), 82 deletions(-) diff --git a/docs/dshot_gcr_dump.txt b/docs/dshot_gcr_dump.txt index 10c04dfb..4ca7f050 100644 --- a/docs/dshot_gcr_dump.txt +++ b/docs/dshot_gcr_dump.txt @@ -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 @@ -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 diff --git a/lib/EscDriver/src/EscDriver.h b/lib/EscDriver/src/EscDriver.h index f68765e0..5e0673ee 100644 --- a/lib/EscDriver/src/EscDriver.h +++ b/lib/EscDriver/src/EscDriver.h @@ -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; } @@ -143,7 +150,8 @@ class EscDriverBase static inline uint32_t convertToValue(uint32_t value) { // eRPM range - if(value == 0x0fff) { + if(value == 0x0fff) + { return 0; } @@ -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; } diff --git a/lib/EscDriver/src/EscDriverEsp32.cpp b/lib/EscDriver/src/EscDriverEsp32.cpp index 4e0cbd59..b1f237b6 100644 --- a/lib/EscDriver/src/EscDriverEsp32.cpp +++ b/lib/EscDriver/src/EscDriverEsp32.cpp @@ -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 @@ -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; } } @@ -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); @@ -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; @@ -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) { @@ -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); } @@ -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; @@ -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); @@ -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; } @@ -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: @@ -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: @@ -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: diff --git a/lib/EscDriver/src/EscDriverEsp32.h b/lib/EscDriver/src/EscDriverEsp32.h index 231561ca..c79b110c 100644 --- a/lib/EscDriver/src/EscDriverEsp32.h +++ b/lib/EscDriver/src/EscDriverEsp32.h @@ -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; }; @@ -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; diff --git a/lib/Espfc/src/Output/Mixer.h b/lib/Espfc/src/Output/Mixer.h index 5300eb12..f995cf92 100644 --- a/lib/Espfc/src/Output/Mixer.h +++ b/lib/Espfc/src/Output/Mixer.h @@ -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 { @@ -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; @@ -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) diff --git a/test/test_esc/test_esc.cpp b/test/test_esc/test_esc.cpp index 087afaa3..e437b7a4 100644 --- a/test/test_esc/test_esc.cpp +++ b/test/test_esc/test_esc.cpp @@ -61,34 +61,33 @@ void test_esc_gcr_convert_to_value() void test_esc_gcr_convert_to_erpm() { - TEST_ASSERT_EQUAL_UINT32(185, EscDriver::convertToErpm(EscDriver::convertToValue(1942))); - TEST_ASSERT_EQUAL_UINT32(184, EscDriver::convertToErpm(EscDriver::convertToValue(1944))); - TEST_ASSERT_EQUAL_UINT32(182, EscDriver::convertToErpm(EscDriver::convertToValue(1947))); - TEST_ASSERT_EQUAL_UINT32(182, EscDriver::convertToErpm(EscDriver::convertToValue(1948))); - TEST_ASSERT_EQUAL_UINT32(182, EscDriver::convertToErpm(EscDriver::convertToValue(1949))); - TEST_ASSERT_EQUAL_UINT32(181, EscDriver::convertToErpm(EscDriver::convertToValue(1950))); + TEST_ASSERT_EQUAL_UINT32( 60, EscDriver::convertToErpm(10000)); + TEST_ASSERT_EQUAL_UINT32( 120, EscDriver::convertToErpm( 5000)); + TEST_ASSERT_EQUAL_UINT32( 300, EscDriver::convertToErpm( 2000)); + TEST_ASSERT_EQUAL_UINT32( 600, EscDriver::convertToErpm( 1000)); + TEST_ASSERT_EQUAL_UINT32(1200, EscDriver::convertToErpm( 500)); + TEST_ASSERT_EQUAL_UINT32(3000, EscDriver::convertToErpm( 200)); + TEST_ASSERT_EQUAL_UINT32(6000, EscDriver::convertToErpm( 100)); } void test_esc_duration_to_bitlen() { - uint32_t bit_len = 178; - TEST_ASSERT_EQUAL_UINT32(0, EscDriver::durationToBitLen(80, bit_len)); + uint32_t bit_len = 213; // dshot300: 2.666us/12.5, dshot600: 1.333us/12.5 + TEST_ASSERT_EQUAL_UINT32(0, EscDriver::durationToBitLen( 80, bit_len)); TEST_ASSERT_EQUAL_UINT32(1, EscDriver::durationToBitLen(150, bit_len)); TEST_ASSERT_EQUAL_UINT32(1, EscDriver::durationToBitLen(160, bit_len)); TEST_ASSERT_EQUAL_UINT32(1, EscDriver::durationToBitLen(177, bit_len)); - TEST_ASSERT_EQUAL_UINT32(2, EscDriver::durationToBitLen(300, bit_len)); TEST_ASSERT_EQUAL_UINT32(2, EscDriver::durationToBitLen(373, bit_len)); TEST_ASSERT_EQUAL_UINT32(2, EscDriver::durationToBitLen(383, bit_len)); - TEST_ASSERT_EQUAL_UINT32(3, EscDriver::durationToBitLen(450, bit_len)); - TEST_ASSERT_EQUAL_UINT32(3, EscDriver::durationToBitLen(498, bit_len)); TEST_ASSERT_EQUAL_UINT32(3, EscDriver::durationToBitLen(595, bit_len)); TEST_ASSERT_EQUAL_UINT32(3, EscDriver::durationToBitLen(607, bit_len)); TEST_ASSERT_EQUAL_UINT32(3, EscDriver::durationToBitLen(610, bit_len)); + TEST_ASSERT_EQUAL_UINT32(3, EscDriver::durationToBitLen(640, bit_len)); - TEST_ASSERT_EQUAL_UINT32(4, EscDriver::durationToBitLen(664, bit_len)); + TEST_ASSERT_EQUAL_UINT32(4, EscDriver::durationToBitLen(850, bit_len)); } constexpr uint32_t make_item(uint32_t duration0, uint32_t level0, uint32_t duration1, uint32_t level1) @@ -96,7 +95,7 @@ constexpr uint32_t make_item(uint32_t duration0, uint32_t level0, uint32_t durat return (duration0 & 0x07fff) | (level0 & 0x1) << 15 | (duration1 & 0x07fff) << 16 | (level1 & 0x1) << 31; } -void test_esc_extract_telemetry_gcr_real() +void test_esc_extract_telemetry_gcr_synth() { // 0b 0 11 0 1 00 111 0 11 0 11 00 1 0 1 // 24 0:100 1:200 0:100 1:100 0:200 1:300 0:100 1:200 0:100 1:200 0:200 1:100 0:100 @@ -114,8 +113,40 @@ void test_esc_extract_telemetry_gcr_real() TEST_ASSERT_EQUAL_HEX32(exp, EscDriver::extractTelemetryGcr(data, data_len, bit_len)); } +void test_esc_extract_telemetry_gcr_synth_idle() +{ + // 0b 00 1 0 1 00 1 0 1 00 1 0 1 0 1 000 1 + // 32 0:200 1:100 0:100 1:100 0:200 1:100 0:100 1:100 0:200 1:100 0:100 1:100 0:100 1:100 0:300 + uint32_t data[] = { + make_item(200, 0, 100, 1), make_item(100, 0, 100, 1), make_item(200, 0, 100, 1), + make_item(100, 0, 100, 1), make_item(200, 0, 100, 1), make_item(100, 0, 100, 1), + make_item(100, 0, 100, 1), make_item(300, 0, 0, 1), + }; + uint32_t bit_len = 100; + uint32_t data_len = sizeof(data); + TEST_ASSERT_EQUAL_UINT32(100, bit_len); + TEST_ASSERT_EQUAL_UINT32(32, data_len); + uint32_t gcr = 0b001010010100101010001; + TEST_ASSERT_EQUAL_HEX32(0x52951, gcr); + TEST_ASSERT_EQUAL_HEX32(gcr, EscDriver::extractTelemetryGcr(data, data_len, bit_len)); + + uint32_t value = EscDriver::gcrToRawValue(gcr); + TEST_ASSERT_EQUAL_HEX32(0x0fff, value); + + value = EscDriver::convertToValue(value); + TEST_ASSERT_EQUAL_HEX32(0x0, value); -void test_esc_extract_telemetry_gcr1() + uint32_t erpm = EscDriver::convertToErpm(value); + TEST_ASSERT_EQUAL_UINT32(0, erpm); + + float erpmToHz = EscDriver::getErpmToHzRatio(14); + float freq = erpmToHz * erpm; + TEST_ASSERT_FLOAT_WITHIN(0.01f, 0.0f, freq); + float rpm = freq * 60; + TEST_ASSERT_FLOAT_WITHIN(0.01f, 0.0f, rpm); +} + +void test_esc_extract_telemetry_dshot300_sample() { // 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? // 00 111 0 111 0 11 00 111 0 1 0 @@ -123,16 +154,31 @@ void test_esc_extract_telemetry_gcr1() make_item(373, 0, 605, 1), make_item(163, 0, 605, 1), make_item(174, 0, 376, 1), make_item(379, 0, 599, 1), make_item(177, 0, 163, 1), make_item(173, 0, 0, 1), }; - uint32_t bit_len = 2136 / (1000 / 80); + uint32_t bit_len = 2667 / (1000.0 / 80); uint32_t data_len = sizeof(data); - TEST_ASSERT_EQUAL_UINT32(178, bit_len); + TEST_ASSERT_EQUAL_UINT32(213, bit_len); TEST_ASSERT_EQUAL_UINT32(24, data_len); - uint32_t exp = 0b001110111011001110101; // +1 - TEST_ASSERT_EQUAL_HEX32(0x77675, exp); - TEST_ASSERT_EQUAL_HEX32(exp, EscDriver::extractTelemetryGcr(data, data_len, bit_len)); + uint32_t gcr = 0b001110111011001110101; // +1 + TEST_ASSERT_EQUAL_HEX32(0x77675, gcr); + TEST_ASSERT_EQUAL_HEX32(gcr, EscDriver::extractTelemetryGcr(data, data_len, bit_len)); + + uint32_t value = EscDriver::gcrToRawValue(gcr); + TEST_ASSERT_EQUAL_HEX32(0x093a, value); + + value = EscDriver::convertToValue(value); + TEST_ASSERT_EQUAL_HEX32(0x13a0, value); + + uint32_t erpm = EscDriver::convertToErpm(value); + TEST_ASSERT_EQUAL_UINT32(119, erpm); + + float erpmToHz = EscDriver::getErpmToHzRatio(14); + float freq = erpmToHz * erpm; + TEST_ASSERT_FLOAT_WITHIN(0.01f, 28.33f, freq); + float rpm = freq * 60; + TEST_ASSERT_FLOAT_WITHIN(0.01f, 1700.0f, rpm); } -void test_esc_extract_telemetry_gcr2() +void test_esc_extract_telemetry_dshot300_running() { // 28 0:177 1:379 0:177 1:167 0:173 1:164 0:379 1:177 0:373 1:379 0:380 1:608 0:164 D499E D499E? // 0 11 0 1 0 1 00 1 00 11 00 111 0 @@ -141,13 +187,61 @@ void test_esc_extract_telemetry_gcr2() make_item(379, 0, 177, 1), make_item(373, 0, 379, 1), make_item(380, 0, 608, 1), make_item(164, 0, 0, 0), }; - uint32_t bit_len = 2136 / (1000 / 80); + uint32_t bit_len = 2667 / (1000.0 / 80); uint32_t data_len = sizeof(data); - uint32_t exp = 0b011010100100110011101; // +1 - TEST_ASSERT_EQUAL_UINT32(178, bit_len); + uint32_t gcr = 0b011010100100110011101; // +1 + TEST_ASSERT_EQUAL_UINT32(213, bit_len); TEST_ASSERT_EQUAL_UINT32(28, data_len); - TEST_ASSERT_EQUAL_HEX32(0xD499D, exp); - TEST_ASSERT_EQUAL_HEX32(exp, EscDriver::extractTelemetryGcr(data, data_len, bit_len)); + TEST_ASSERT_EQUAL_HEX32(0xD499D, gcr); + TEST_ASSERT_EQUAL_HEX32(gcr, EscDriver::extractTelemetryGcr(data, data_len, bit_len)); + + uint32_t value = EscDriver::gcrToRawValue(gcr); + TEST_ASSERT_EQUAL_HEX32(0x071a, value); + + value = EscDriver::convertToValue(value); + TEST_ASSERT_EQUAL_HEX32(0x08d0, value); + + uint32_t erpm = EscDriver::convertToErpm(value); + TEST_ASSERT_EQUAL_UINT32(266, erpm); + + float erpmToHz = EscDriver::getErpmToHzRatio(14); + float freq = erpmToHz * erpm; + TEST_ASSERT_FLOAT_WITHIN(0.01f, 63.33f, freq); + float rpm = freq * 60; + TEST_ASSERT_FLOAT_WITHIN(0.01f, 3800.0f, rpm); +} + +void test_esc_extract_telemetry_dshot300_idle() +{ + // 32 0:380 1:172 0:175 1:172 0:371 1:172 0:175 1:172 0:371 1:172 0:174 1:162 0:172 1:172 0:606 52950 52950 + // 00 1 0 1 00 1 0 1 00 1 0 1 0 1 000 1 + uint32_t data[] = { + make_item(380, 0, 172, 1), make_item(175, 0, 172, 1), make_item(371, 0, 172, 1), + make_item(175, 0, 172, 1), make_item(371, 0, 172, 1), make_item(174, 0, 162, 1), + make_item(172, 0, 172, 1), make_item(606, 0, 0, 1), + }; + uint32_t bit_len = 2667 / (1000.0 / 80); + uint32_t data_len = sizeof(data); + TEST_ASSERT_EQUAL_UINT32(213, bit_len); + TEST_ASSERT_EQUAL_UINT32(32, data_len); + uint32_t gcr = 0b001010010100101010001; // +1 + TEST_ASSERT_EQUAL_HEX32(0x52951, gcr); + TEST_ASSERT_EQUAL_HEX32(gcr, EscDriver::extractTelemetryGcr(data, data_len, bit_len)); + + uint32_t value = EscDriver::gcrToRawValue(gcr); + TEST_ASSERT_EQUAL_HEX32(0x0fff, value); + + value = EscDriver::convertToValue(value); + TEST_ASSERT_EQUAL_HEX32(0x0, value); + + uint32_t erpm = EscDriver::convertToErpm(value); + TEST_ASSERT_EQUAL_UINT32(0, erpm); + + float erpmToHz = EscDriver::getErpmToHzRatio(14); + float freq = erpmToHz * erpm; + TEST_ASSERT_FLOAT_WITHIN(0.01f, 0.0f, freq); + float rpm = freq * 60; + TEST_ASSERT_FLOAT_WITHIN(0.01f, 0.0f, rpm); } int main(int argc, char **argv) @@ -160,9 +254,11 @@ int main(int argc, char **argv) RUN_TEST(test_esc_gcr_convert_to_value); RUN_TEST(test_esc_gcr_convert_to_erpm); RUN_TEST(test_esc_duration_to_bitlen); - RUN_TEST(test_esc_extract_telemetry_gcr_real); - RUN_TEST(test_esc_extract_telemetry_gcr1); - RUN_TEST(test_esc_extract_telemetry_gcr2); + RUN_TEST(test_esc_extract_telemetry_gcr_synth); + RUN_TEST(test_esc_extract_telemetry_gcr_synth_idle); + RUN_TEST(test_esc_extract_telemetry_dshot300_sample); + RUN_TEST(test_esc_extract_telemetry_dshot300_running); + RUN_TEST(test_esc_extract_telemetry_dshot300_idle); UNITY_END(); return 0;