Skip to content

Commit

Permalink
decode gcr + erpm + extended telemetry + esc tests
Browse files Browse the repository at this point in the history
  • Loading branch information
@rtlopez
rtlopez committed Feb 16, 2024
1 parent d2eaf0b commit cf7c223
Show file tree
Hide file tree
Showing 9 changed files with 366 additions and 26 deletions.
47 changes: 47 additions & 0 deletions docs/dshot_gcr_dump.txt
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,47 @@
// bluejay esc, dshot 300

// power off
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
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
32 0:380 1:170 0:170 1:173 0:376 1:173 0:171 1:170 0:389 1:170 0:173 1:177 0:160 1:174 0:598 52950 52950
32 0:380 1:173 0:173 1:174 0:369 1:174 0:173 1:173 0:370 1:173 0:174 1:163 0:170 1:173 0:606 52950 52950
32 0:380 1:163 0:170 1:174 0:382 1:160 0:177 1:170 0:373 1:170 0:174 1:176 0:160 1:174 0:598 52950 52950
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
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
24 0:370 1:601 0:174 1:605 0:595 1:173 0:174 1:163 0:380 1:379 0:174 1EF0A6 1EF0A6
28 0:379 1:595 0:174 1:605 0:164 1:173 0:173 1:379 0:164 1:173 0:174 1:163 0:380 F7AD4 F7AD4
24 0:383 1:605 0:167 1:608 0:161 1:379 0:608 1:164 0:173 1:170 0:167 F7B0A F7B0A

// armed
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
24 0:173 1:379 0:167 1:173 0:167 1:605 0:386 1:164 0:379 1:376 0:602 D7930 D7930
24 0:183 1:380 0:173 1:173 0:164 1:605 0:380 1:604 0:164 1:379 0:664 35E7B0 35E7B0
24 0:174 1:379 0:173 1:164 0:226 1:601 0:383 1:599 0:376 1:180 0:373 D79E4 D79E4
28 0:173 1:380 0:163 1:173 0:174 1:602 0:595 1:173 0:380 1:173 0:380 1:163 0:173 1AF092 1AF092
28 0:167 1:379 0:170 1:173 0:164 1:602 0:602 1:173 0:173 1:380 0:163 1:173 0:380 1AF0B4 1AF0B4
28 0:164 1:379 0:174 1:173 0:164 1:605 0:605 1:163 0:174 1:379 0:174 1:163 0:380 1AF0B4 1AF0B4

// disarmed
28 0:380 1:595 0:173 1:605 0:164 1:173 0:174 1:379 0:164 1:173 0:174 1:163 0:380 F7AD4 F7AD4
24 0:379 1:602 0:173 1:599 0:170 1:386 0:386 1:608 0:164 1:173 0:174 1EF67A 1EF67A
16 0:380 1:595 0:10368 0:14 0:599 1:173 0:1 1 1
24 0:373 1:602 0:180 1:598 0:380 1:166 0:171 1:166 0:383 1:173 0:377 7BCA4 7BCA4
20 0:380 1:605 0:173 1:596 0:605 1:173 0:379 1:380 0:595 3DE130 3DE130

// power off
32 0:383 1:173 0:164 1:170 0:382 1:174 0:173 1:164 0:379 1:173 0:174 1:160 0:177 1:170 0:608 52950 52950
32 0:382 1:170 0:167 1:170 0:380 1:170 0:177 1:163 0:380 1:173 0:170 1:167 0:173 1:174 0:595 52950 52950
32 0:383 1:170 0:164 1:170 0:383 1:176 0:161 1:173 0:379 1:174 0:177 1:170 0:173 1:177 0:592 52950 52950
32 0:379 1:173 0:174 1:173 0:370 1:173 0:174 1:173 0:369 1:174 0:173 1:164 0:170 1:173 0:605 52950 52950
32 0:382 1:170 0:164 1:170 0:383 1:176 0:161 1:173 0:380 1:173 0:177 1:170 0:173 1:177 0:592 52950 52950
32 0:383 1:170 0:167 1:170 0:380 1:170 0:177 1:163 0:380 1:173 0:170 1:167 0:173 1:173 0:596 52950 52950
32 0:384 1:175 0:162 1:172 0:381 1:172 0:178 1:169 0:384 1:172 0:175 1:162 0:171 1:172 0:597 52950 52950
32 0:387 1:162 0:172 1:169 0:390 1:159 0:175 1:169 0:384 1:175 0:172 1:171 0:179 1:158 0:604 52950 52950
32 0:381 1:168 0:172 1:172 0:378 1:171 0:172 1:169 0:390 1:169 0:175 1:175 0:162 1:172 0:600 52950 52950
110 changes: 110 additions & 0 deletions lib/EscDriver/src/EscDriver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,8 +39,14 @@ class EscDriverBase
int write(size_t channel, int pulse) { return 1; }
void apply() {}
int pin(size_t channel) const { return -1; }
uint32_t telemetry(size_t channel) const { return 0; }
#endif

static inline uint16_t dshotConvert(uint16_t pulse)
{
return pulse > 1000 ? PWM_TO_DSHOT(pulse) : 0;
}

static inline uint16_t dshotEncode(uint16_t value, bool inverted = false)
{
value <<= 1;
Expand All @@ -62,6 +68,110 @@ class EscDriverBase
return (value << 4) | csum;
}

static inline uint32_t durationToBitLen(uint32_t duration, uint32_t len)
{
return (duration + (len >> 1)) / len;
}

static inline uint32_t 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
*/
static inline uint32_t 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);
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);
value = pushBits(value, level1, len1);
bitCount += len1;
}

// fill missing bits with 1
if(bitCount < 21)
{
value = pushBits(value, 1, 21 - bitCount);
}
value = value ^ (value >> 1); // extract gcr

return value;
}

static const uint32_t INVALID_TELEMETRY_VALUE = 0xffff;

static inline uint32_t convertToErpm(uint32_t value)
{
if (!value || value == INVALID_TELEMETRY_VALUE) {
return INVALID_TELEMETRY_VALUE;
}

// Convert period to erpm * 100
return (1000000 * 60 / 100 + value / 2) / value;
}

static inline uint32_t 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);
}

static uint32_t gcrToRawValue(uint32_t value)
{
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) {
value = INVALID_TELEMETRY_VALUE;
} else {
value = decodedValue >> 4;
}

return value;
}

static const size_t DSHOT_BIT_COUNT = 16;
};

Expand Down
69 changes: 44 additions & 25 deletions lib/EscDriver/src/EscDriverEsp32.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,19 +50,6 @@ IRAM_ATTR static esp_err_t _rmt_tx_start(rmt_channel_t channel, bool tx_idx_rst)
#define _rmt_fill_tx_items rmt_fill_tx_items
#endif

static int IRAM_ATTR _rmt_rx_get_mem_len_in_isr(rmt_channel_t channel)
{
volatile rmt_item32_t *data = (rmt_item32_t *)RMTMEM.chan[RMT_ENCODE_RX_CHANNEL(channel)].data32;
for (size_t idx = 0; idx < RMT_MEM_ITEM_NUM; idx++) {
if (data[idx].duration0 == 0) {
return idx;
} else if (data[idx].duration1 == 0) {
return idx + 1;
}
}
return RMT_MEM_ITEM_NUM;
}

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;
Expand Down Expand Up @@ -95,12 +82,13 @@ void EscDriverEsp32::end()

int EscDriverEsp32::begin(const EscConfig& conf)
{
_protocol = conf.protocol;
_digital = isDigital(conf.protocol);
_dshot_tlm = conf.dshotTelemetry;
_async = conf.async;
_rate = conf.rate;
_protocol = ESC_PROTOCOL_SANITIZE(conf.protocol);
_async = _protocol == ESC_PROTOCOL_BRUSHED ? true : conf.async; // force async for brushed
_rate = constrain(conf.rate, 50, 8000);
_interval = TO_INTERVAL_US(_rate);
_digital = isDigital(_protocol);
_dshot_tlm = conf.dshotTelemetry && (_protocol == ESC_PROTOCOL_DSHOT300 || _protocol == ESC_PROTOCOL_DSHOT600);
_timer.setInterval(1000000);

return 1;
}
Expand Down Expand Up @@ -132,6 +120,12 @@ int EscDriverEsp32::pin(size_t channel) const
return _channel[channel].pin;
}

uint32_t EscDriverEsp32::telemetry(size_t channel) const
{
if (channel < 0 || channel >= ESC_CHANNEL_COUNT) return 0;
return _channel[channel].telemetryValue;
}

void EscDriverEsp32::initChannel(int i, gpio_num_t pin, int pulse)
{
if (pin == -1) return;
Expand Down Expand Up @@ -322,21 +316,46 @@ void EscDriverEsp32::transmitAll()
void EscDriverEsp32::readTelemetry()
{
PIN_DEBUG(HIGH);
uint32_t bit_len = getAnalogPulse(2000);
for (size_t i = 0; i < ESC_CHANNEL_COUNT; i++)
{
if (!_digital || !_dshot_tlm) continue;
if (!_channel[i].attached()) continue;
//_channel[i].telemetryValue = _rmt_rx_get_mem_len_in_isr((rmt_channel_t)i);
if(!_digital || !_dshot_tlm) continue;
if(!_channel[i].attached()) continue;

HardwareSerial* s = (i == 0 && _timer.check()) ? &Serial : nullptr;

RingbufHandle_t rb = NULL;
rmt_get_ringbuf_handle((rmt_channel_t)i, &rb);
if(ESP_OK != rmt_get_ringbuf_handle((rmt_channel_t)i, &rb)) continue;

size_t rmt_len = 0;
rmt_item32_t * data = (rmt_item32_t *)xRingbufferReceive(rb, &rmt_len, 0/*portMAX_DELAY*/);
if (data) {
// process data here
rmt_item32_t* data = (rmt_item32_t*)xRingbufferReceive(rb, &rmt_len, 0);
if(s) { s->print((uint8_t)rmt_len); }
if (data)
{
for(size_t j = 0; j < rmt_len >> 2; j++)
{
if(!data[j].duration0) break;
if(s) s->print(' ');
if(s) s->print(data[j].level0);
if(s) s->print(':');
if(s) s->print(data[j].duration0);

if(!data[j].duration1) break;
if(s) s->print(' ');
if(s) s->print(data[j].level1);
if(s) s->print(':');
if(s) s->print(data[j].duration1);
}
uint32_t value = extractTelemetryGcr((uint32_t*)data, rmt_len >> 2, bit_len);

if(s) s->print(' ');
if(s) s->print(value, HEX);

vRingbufferReturnItem(rb, (void *)data);

_channel[i].telemetryValue = value;
}
if(s) s->print('\n');
}
PIN_DEBUG(LOW);
}
Expand Down
5 changes: 4 additions & 1 deletion lib/EscDriver/src/EscDriverEsp32.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@

#include "EscDriver.h"
#include <driver/rmt.h>
#include "Timer.h"

class EscDriverEsp32: public EscDriverBase
{
Expand Down Expand Up @@ -73,7 +74,7 @@ class EscDriverEsp32: public EscDriverBase
uint16_t dshot_t0l;
uint16_t dshot_t1h;
uint16_t dshot_t1l;
int telemetryValue;
uint32_t telemetryValue;
};

EscDriverEsp32();
Expand All @@ -83,6 +84,7 @@ class EscDriverEsp32: public EscDriverBase
int write(size_t channel, int pulse);
void apply();
int pin(size_t channel) const;
uint32_t telemetry(size_t channel) const;

private:
void initChannel(int channel, gpio_num_t pin, int pulse);
Expand Down Expand Up @@ -118,6 +120,7 @@ class EscDriverEsp32: public EscDriverBase

static bool _tx_end_installed;
static EscDriverEsp32* instances[];
Espfc::Timer _timer;
};

#endif
Expand Down
1 change: 1 addition & 0 deletions lib/Espfc/src/Model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -172,6 +172,7 @@ class Model

bool armingDisabled() const /* IRAM_ATTR */
{
return false;
return state.armingDisabledFlags != 0;
}

Expand Down
11 changes: 11 additions & 0 deletions lib/Espfc/src/ModelState.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -219,6 +219,17 @@ struct ModelState
int16_t outputDisarmed[OUTPUT_CHANNELS];
bool outputSaturated;

int16_t outputTelemetryErrors[OUTPUT_CHANNELS];
float outputTelemetryRpm[OUTPUT_CHANNELS];
float outputTelemetryFreq[OUTPUT_CHANNELS];
int8_t outputTelemetryTemperature[OUTPUT_CHANNELS];
int8_t outputTelemetryVoltage[OUTPUT_CHANNELS];
int8_t outputTelemetryCurrent[OUTPUT_CHANNELS];
int8_t outputTelemetryDebug1[OUTPUT_CHANNELS];
int8_t outputTelemetryDebug2[OUTPUT_CHANNELS];
int8_t outputTelemetryDebug3[OUTPUT_CHANNELS];
int8_t outputTelemetryEvents[OUTPUT_CHANNELS];

// other state
Kalman kalman[AXES];
VectorFloat accelPrev;
Expand Down
29 changes: 29 additions & 0 deletions lib/Espfc/src/Msp/MspProcessor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1268,6 +1268,35 @@ class MspProcessor
}
break;

case MSP_MOTOR_TELEMETRY:
r.writeU8(OUTPUT_CHANNELS);
for (size_t i = 0; i < OUTPUT_CHANNELS; i++)
{
int rpm = 0;
uint16_t invalidPct = 0;
uint8_t escTemperature = 0; // degrees celcius
uint16_t escVoltage = 0; // 0.01V per unit
uint16_t escCurrent = 0; // 0.01A per unit
uint16_t escConsumption = 0; // mAh

if (_model.config.pin[i + PIN_OUTPUT_0] != -1)
{
rpm = lrintf(_model.state.outputTelemetryRpm[i]);
invalidPct = _model.state.outputTelemetryErrors[i];
escTemperature = _model.state.outputTelemetryTemperature[i];
escVoltage = _model.state.outputTelemetryVoltage[i];
escCurrent = _model.state.outputTelemetryCurrent[i];
}

r.writeU32(rpm);
r.writeU16(invalidPct);
r.writeU8(escTemperature);
r.writeU16(escVoltage);
r.writeU16(escCurrent);
r.writeU16(escConsumption);
}
break;

case MSP_SET_MOTOR:
for(size_t i = 0; i < OUTPUT_CHANNELS; i++)
{
Expand Down
Loading

0 comments on commit cf7c223

Please sign in to comment.