Loading files
This file is pinned to the selected release. Download the original to open it in your design tool.
f4aa2738eThis file is pinned to the selected release. Download the original to open it in your design tool.
#if defined(ESP32C3)
#include "EscDriverEsp32c3.h"
#include <Arduino.h>
#include <driver/timer.h>
#include <algorithm>
#define ESC_TIMER_DIVIDER 2
#define ESC_TIMER_IDX TIMER_0
#define DELTA_TICKS_MAX ((APB_CLK_FREQ / ESC_TIMER_DIVIDER / 1000000L) * 50000L)
#define DELTA_TICKS_MIN 5
#define TIMER_WAIT_SHORT_COMP 10UL
#define TIMER_WAIT_EDGE 240UL
#define TIMER_WAIT_COMP 90UL
void EscDriverEsp32c3::_isr_init(EscDriverTimer timer, void * driver)
{
timer_group_t group = (timer_group_t)timer;
timer_config_t config = {
.alarm_en = TIMER_ALARM_EN,
.counter_en = TIMER_PAUSE,
.intr_type = TIMER_INTR_LEVEL,
.counter_dir = TIMER_COUNT_UP,
.auto_reload = TIMER_AUTORELOAD_DIS,
.divider = ESC_TIMER_DIVIDER,
};
timer_init(group, ESC_TIMER_IDX, &config);
timer_set_counter_value(group, ESC_TIMER_IDX, 0);
timer_isr_callback_add(group, ESC_TIMER_IDX, EscDriverEsp32c3::handle, driver, ESP_INTR_FLAG_IRAM);
timer_enable_intr(group, ESC_TIMER_IDX);
timer_start(group, ESC_TIMER_IDX);
}
bool IRAM_ATTR EscDriverEsp32c3::_isr_wait(EscDriverTimer timer, const uint32_t ticks)
{
if(ticks >= TIMER_WAIT_EDGE) { // yield
uint64_t value = timer_group_get_counter_value_in_isr((timer_group_t)timer, ESC_TIMER_IDX);
value += (ticks - TIMER_WAIT_COMP);
timer_group_set_alarm_value_in_isr((timer_group_t)timer, ESC_TIMER_IDX, value);
timer_group_enable_alarm_in_isr((timer_group_t)timer, ESC_TIMER_IDX);
return true;
}
if(ticks > 20) { // busy delay
const uint32_t end = ESP.getCycleCount() + (2 * ticks * ESC_TIMER_DIVIDER) - TIMER_WAIT_SHORT_COMP;
while(ESP.getCycleCount() < end) {
__asm__ __volatile__ ("nop");
};
}
return false;
}
// run as soon as possible
void IRAM_ATTR EscDriverEsp32c3::_isr_start(EscDriverTimer timer)
{
_isr_wait(timer, TIMER_WAIT_EDGE + 10);
//timer_start((timer_group_t)timer, ESC_TIMER_IDX);
}
void EscDriverEsp32c3::_isr_end(EscDriverTimer timer, void* p)
{
timer_pause((timer_group_t)timer, ESC_TIMER_IDX);
timer_disable_intr((timer_group_t)timer, ESC_TIMER_IDX);
}
int IRAM_ATTR EscDriverEsp32c3::attach(size_t channel, int pin, int pulse)
{
if(channel < 0 || channel >= ESC_CHANNEL_COUNT) return 0;
_slots[channel].pin = pin;
_slots[channel].pulse = usToTicks(pulse);
pinMode(pin, OUTPUT);
digitalWrite(pin, LOW);
return 1;
}
int IRAM_ATTR EscDriverEsp32c3::write(size_t channel, int pulse)
{
if(channel < 0 || channel >= ESC_CHANNEL_COUNT) return 0;
_slots[channel].pulse = usToTicks(pulse);
return 1;
}
int EscDriverEsp32c3::pin(size_t channel) const
{
if(channel < 0 || channel >= ESC_CHANNEL_COUNT) return -1;
return _slots[channel].pin;
}
uint32_t EscDriverEsp32c3::telemetry(size_t channel) const
{
return 0;
}
void IRAM_ATTR EscDriverEsp32c3::apply()
{
if(_protocol == ESC_PROTOCOL_DISABLED) return;
if(_protocol >= ESC_PROTOCOL_DSHOT150)
{
dshotWrite();
return;
}
if(_async || _busy) return;
_isr_start(_timer);
}
bool IRAM_ATTR EscDriverEsp32c3::handle(void * p)
{
// Time critical section
EscDriver * instance = (EscDriver *)p;
//instance->_it = NULL;
if(!instance->_it)
{
instance->_busy = true;
instance->commit();
instance->_it = instance->_items;
}
while(instance->_it && instance->_it != instance->_end)
{
const uint32_t ticks = instance->_it->ticks;
if(instance->_it->clr_mask)
{
GPIO.out_w1tc.out_w1tc = instance->_it->clr_mask;
}
if(instance->_it->set_mask)
{
GPIO.out_w1ts.out_w1ts = instance->_it->set_mask;
}
if(instance->_it->last)
{
instance->_it = NULL;
instance->_busy = false;
if(ticks) _isr_wait(instance->_timer, ticks);
break;
}
instance->_it++;
if(_isr_wait(instance->_timer, ticks)) break;
}
// return whether we need to yield at the end of ISR
return false;
}
void IRAM_ATTR EscDriverEsp32c3::commit()
{
Slot sorted[ESC_CHANNEL_COUNT];
std::copy(_slots, _slots + ESC_CHANNEL_COUNT, sorted);
std::sort(sorted, sorted + ESC_CHANNEL_COUNT);
Slot * end = sorted + ESC_CHANNEL_COUNT;
for(Item * it = _items; it != _items + ESC_CHANNEL_COUNT * 2; ++it) *it = Item();
Item * item = _items;
for(Slot * it = sorted; it != end; ++it)
{
if(!it->active()) break;
if(!(_protocol == ESC_PROTOCOL_BRUSHED && it->pulse <= _intervalMin))
{
item->set_mask |= (1 << it->pin) & 0xffff;
}
if(it == sorted) item->ticks = it->pulse;
}
item++;
Slot * last = NULL;
for(Slot * it = sorted; it != end; ++it)
{
if(!it->active()) break;
if(last)
{
if(!(_protocol == ESC_PROTOCOL_BRUSHED && last->pulse >= _intervalMax))
{
item->clr_mask |= (1 << last->pin) & 0xffff;
}
uint32_t delta = it->pulse - last->pulse;
if(delta > DELTA_TICKS_MIN && delta < DELTA_TICKS_MAX)
{
item->ticks = delta;
item++;
}
}
last = it;
}
// terminator
item->last = true;
if(last)
{
if(!(_protocol == ESC_PROTOCOL_BRUSHED && last->pulse >= _intervalMax))
{
item->clr_mask |= (1 << last->pin);
}
}
if(_async)
{
item->ticks = _interval;
if(last)
{
const int32_t mt = minTicks(_timer);
if(item->ticks > last->pulse + mt) item->ticks -= last->pulse;
else item->ticks = mt;
}
}
}
uint32_t IRAM_ATTR EscDriverEsp32c3::usToTicksReal(EscDriverTimer timer, uint32_t us)
{
return (APB_CLK_FREQ / ESC_TIMER_DIVIDER / 1000000L) * us;
}
int32_t IRAM_ATTR EscDriverEsp32c3::minTicks(EscDriverTimer timer)
{
return 150L;
}
uint32_t IRAM_ATTR EscDriverEsp32c3::usToTicks(uint32_t us)
{
uint32_t ticks = 0;
switch(_protocol)
{
case ESC_PROTOCOL_ONESHOT125:
ticks = map(us, 1000, 2000, usToTicksReal(_timer, 125), usToTicksReal(_timer, 250));
break;
case ESC_PROTOCOL_ONESHOT42:
ticks = map(us, 1000, 2000, usToTicksReal(_timer, 42), usToTicksReal(_timer, 84));
break;
case ESC_PROTOCOL_MULTISHOT:
ticks = map(us, 1000, 2000, usToTicksReal(_timer, 5), usToTicksReal(_timer, 25));
break;
case ESC_PROTOCOL_BRUSHED:
ticks = map(std::clamp<uint32_t>(us, 1000, 2000), 1000, 2000, 0, _interval); // strange behaviour at bonduaries
break;
case ESC_PROTOCOL_DSHOT150:
case ESC_PROTOCOL_DSHOT300:
case ESC_PROTOCOL_DSHOT600:
ticks = us;
break;
default:
ticks = usToTicksReal(_timer, us); // PWM
break;
}
return ticks;
}
EscDriverEsp32c3::EscDriverEsp32c3(): _busy(false), _async(true), _protocol(ESC_PROTOCOL_PWM), _rate(50), _timer(ESC_DRIVER_TIMER1), _interval(usToTicksReal(_timer, 1000000L / _rate)), _it(NULL), _end(_items + ESC_CHANNEL_COUNT * 2)
{
for(size_t i = 0; i < ESC_CHANNEL_COUNT; ++i) _slots[i] = Slot();
for(size_t i = 0; i < ESC_CHANNEL_COUNT * 2; ++i) _items[i] = Item();
}
int EscDriverEsp32c3::begin(const EscConfig& conf)
{
_protocol = ESC_PROTOCOL_SANITIZE(conf.protocol);
_async = _protocol == ESC_PROTOCOL_BRUSHED ? true : conf.async; // force async for brushed
_rate = std::clamp(conf.rate, 50, 8000);
_timer = (EscDriverTimer)conf.timer;
_interval = usToTicksReal(_timer, 1000000L / _rate)/* - 400*/; // small compensation to keep frequency
_intervalMin = _interval / 500; // do not generate brushed pulses if duty < ~0.2% (1002)
_intervalMax = _interval - _intervalMin; // set brushed output hi if duty > ~99.8% (1998)
switch(_protocol)
{
// pulse delays experimentally selected
case ESC_PROTOCOL_DSHOT150:
{
_dh = 69;
_dl = 57;
}
break;
case ESC_PROTOCOL_DSHOT300:
{
_dh = 32;
_dl = 26;
}
break;
case ESC_PROTOCOL_DSHOT600:
case ESC_PROTOCOL_PROSHOT:
{
_dh = 14;
_dl = 10;
}
break;
case ESC_PROTOCOL_DISABLED:
break;
default: // analog
_isr_init(_timer, this);
if(_async) _isr_start(_timer);
}
return 1;
}
void EscDriverEsp32c3::end()
{
if(_protocol < ESC_PROTOCOL_DSHOT150) // analog
{
_isr_end(_timer, this);
}
for(size_t i = 0; i < ESC_CHANNEL_COUNT; ++i)
{
if(_slots[i].pin == -1) continue;
digitalWrite(_slots[i].pin, LOW);
}
_protocol = ESC_PROTOCOL_DISABLED;
}
static inline void dshotDelay(int delay)
{
while(delay--) __asm__ __volatile__ ("nop");
}
void IRAM_ATTR EscDriverEsp32c3::dshotWrite()
{
// zero mask arrays
mask_t * sm = dshotSetMask;
mask_t * cm = dshotClrMask;
for(size_t i = 0; i < DSHOT_BIT_COUNT; i++)
{
*sm = 0; sm++;
*cm = 0; cm++;
*cm = 0; cm++;
}
// compute bits
for(size_t c = 0; c < ESC_CHANNEL_COUNT; c++)
{
if(_slots[c].pin > 16 || _slots[c].pin < 0) continue;
mask_t mask = (1U << _slots[c].pin);
int pulse = std::clamp(_slots[c].pulse, 0, 2000);
// scale to dshot commands (0 or 48-2047)
int value = dshotConvert(pulse);
uint16_t frame = dshotEncode(value);
for(size_t i = 0; i < DSHOT_BIT_COUNT; i++)
{
int val = (frame >> (DSHOT_BIT_COUNT - 1 - i)) & 0x01;
dshotSetMask[i] |= mask;
dshotClrMask[(i << 1) + val] |= mask;
}
}
// write output
sm = dshotSetMask;
cm = dshotClrMask;
for(size_t i = 0; i < DSHOT_BIT_COUNT; i++)
{
GPIO.out_w1ts.out_w1ts = *sm; sm++; dshotDelay(_dh);
GPIO.out_w1tc.out_w1tc = *cm; cm++; dshotDelay(_dh);
GPIO.out_w1tc.out_w1tc = *cm; cm++; dshotDelay(_dl);
}
}
#endif // ESP32c3