Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_ChibiOS: allow dshot rate to be set so as to both regularize the output as well as

Browse Source 
allow faster rates synchronized to the loop rate
synchronize 1Khz and use Betaflight definition for prescaler
adjust dshot bitrates
don't allocate IC DMA if already allocated
cancel DMA pulses correctly
try really hard to align pulses with push() by making each dshot pulse event driven
This commit is contained in:
Andy Piper 2021-03-02 19:02:38 +00:00 committed by Andrew Tridgell
parent 9a870e4d75
commit 477ff72214
3 changed files with 146 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

177
libraries/AP_HAL_ChibiOS/RCOutput.cpp
View File

@ -98,6 +98,7 @@ void RCOutput::init()
}
#endif
chMtxObjectInit(&trigger_mutex);
chVTObjectInit(&_dshot_rate_timer);
// setup default output rate of 50Hz
set_freq(0xFFFF ^ ((1U<<chan_offset)-1), 50);
@ -122,6 +123,7 @@ void RCOutput::init()
void RCOutput::rcout_thread()
{
uint32_t last_thread_run_us = 0; // last time we did a 1kHz run of rcout
uint32_t last_cycle_run_us = 0;
rcout_thread_ctx = chThdGetSelfX();
@ -130,33 +132,62 @@ void RCOutput::rcout_thread()
hal.scheduler->delay_microseconds(1000);
}
// dshot is quite sensitive to timing, it's important to output pulses as
// regularly as possible at the correct bitrate
while (true) {
const uint32_t period_us = AP_HAL::micros() - last_thread_run_us;
if (period_us < 1000) {
chEvtWaitOneTimeout(EVT_PWM_SEND, chTimeUS2I(1000 - period_us));
}
chEvtWaitOne(EVT_PWM_SEND);
// start the clock
last_thread_run_us = AP_HAL::micros();
// main thread requested a new dshot send
// this is when the cycle is supposed to start
if (_dshot_cycle == 0) {
last_cycle_run_us = AP_HAL::micros();
// register a timer for the next tick if push() will not be providing it
if (_dshot_rate != 1) {
chVTSet(&_dshot_rate_timer, chTimeUS2I(_dshot_period_us), dshot_update_tick, this);
}
}
// if DMA sharing is in effect there can be quite a delay between the request to begin the cycle and
// actually sending out data - thus we need to work out how much time we have left to collect the locks
uint32_t time_out_us = (_dshot_cycle + 1) * _dshot_period_us + last_cycle_run_us;
if (!_dshot_rate) {
time_out_us = last_thread_run_us + _dshot_period_us;
}
// main thread requested a new dshot send or we timed out - if we are not running
// as a multiple of loop rate then ignore EVT_PWM_SEND events to preserve periodicity
if (!serial_group) {
// do a blocking send now, to guarantee DShot sends at
// above 1000 Hz. This makes the protocol more reliable on
// long cables, and also keeps some ESCs happy that don't
// like low rates
dshot_send_groups();
dshot_send_groups(time_out_us);
// now unlock everything
dshot_collect_dma_locks(last_thread_run_us);
dshot_collect_dma_locks(time_out_us);
_dshot_cycle = (_dshot_cycle + 1) % _dshot_rate;
}
// process any pending RC output requests
timer_tick(last_thread_run_us);
timer_tick(time_out_us);
}
}
void RCOutput::dshot_update_tick(void* p)
{
chSysLockFromISR();
RCOutput* rcout = (RCOutput*)p;
if (rcout->_dshot_cycle < rcout->_dshot_rate - 1) {
chVTSetI(&rcout->_dshot_rate_timer, chTimeUS2I(rcout->_dshot_period_us), dshot_update_tick, p);
}
chEvtSignalI(rcout->rcout_thread_ctx, EVT_PWM_SEND);
chSysUnlockFromISR();
}
#ifndef HAL_NO_SHARED_DMA
// release locks on the groups that are pending in reverse order
void RCOutput::dshot_collect_dma_locks(uint32_t last_run_us)
void RCOutput::dshot_collect_dma_locks(uint32_t time_out_us)
{
if (NUM_GROUPS == 0) {
return;
@ -165,11 +196,17 @@ void RCOutput::dshot_collect_dma_locks(uint32_t last_run_us)
pwm_group &group = pwm_group_list[i];
if (group.dma_handle != nullptr && group.dma_handle->is_locked()) {
// calculate how long we have left
const uint32_t deltat = AP_HAL::micros() - last_run_us;
uint32_t now = AP_HAL::micros();
// if we have time left wait for the event
eventmask_t mask = 0;
if (deltat < 1000) {
mask = chEvtWaitOneTimeout(group.dshot_event_mask, chTimeUS2I(1000 - deltat));
const uint32_t pulse_elapsed_us = now - group.last_dmar_send_us;
if (now < time_out_us) {
mask = chEvtWaitOneTimeout(group.dshot_event_mask,
chTimeUS2I(MAX(time_out_us - now, group.dshot_pulse_send_time_us - pulse_elapsed_us)));
} else if (pulse_elapsed_us < group.dshot_pulse_send_time_us) {
// better to let the burst write in progress complete rather than cancelling mid way through
mask = chEvtWaitOneTimeout(group.dshot_event_mask,
chTimeUS2I(group.dshot_pulse_send_time_us - pulse_elapsed_us));
}
// no time left cancel and restart
if (!mask) {
@ -345,6 +382,34 @@ void RCOutput::set_default_rate(uint16_t freq_hz)
}
}
/*
Set the dshot rate as a multiple of the loop rate
*/
void RCOutput::set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz)
{
// for low loop rates simply output at 1Khz on a timer
if (loop_rate_hz <= 100) {
_dshot_period_us = 1000UL;
_dshot_rate = 0;
return;
}
uint16_t drate = dshot_rate * loop_rate_hz;
_dshot_rate = dshot_rate;
// never allow rates below 500hz
while (drate < 500) {
_dshot_rate++;
drate = _dshot_rate * loop_rate_hz;
}
// prevent stupidly high rates, ideally should also prevent high rates
// with slower dshot variants
if (drate > 4000) {
_dshot_rate = 4000 / loop_rate_hz;
drate = _dshot_rate * loop_rate_hz;
}
_dshot_period_us = 1000000UL / drate;
}
/*
find pwm_group and index in group given a channel number
*/
@ -631,7 +696,7 @@ bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_
group.dma_buffer_len = buffer_length;
}
// reset the pulse time inside the lock
group.dshot_pulse_time_us = pulse_time_us;
group.dshot_pulse_time_us = group.dshot_pulse_send_time_us = pulse_time_us;
#ifdef HAL_WITH_BIDIR_DSHOT
// configure input capture DMA if required
@ -648,42 +713,30 @@ bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_
group.pwm_started = false;
}
// adjust frequency to give an allowed value given the
// clock. There is probably a better way to do this
const uint32_t original_bitrate = bitrate;
uint32_t freq = 0;
uint32_t target_frequency = bitrate * bit_width;
while (true) {
uint32_t clock_hz = group.pwm_drv->clock;
target_frequency = bitrate * bit_width;
uint32_t prescaler = clock_hz / target_frequency;
freq = clock_hz / prescaler;
// hal.console->printf("CLOCK=%u BW=%u FREQ=%u PRE=%u BR=%u\n", clock_hz, bit_width, freq/bit_width, prescaler, bitrate);
if (prescaler > 0x8000) {
group.dma_handle->unlock();
return false;
}
if (!choose_high) {
break;
}
// we want to choose a frequency that gives at least the
// target, erring on the high side not low side
uint32_t actual_bitrate = freq / bit_width;
if (actual_bitrate >= original_bitrate || bitrate < 10000) {
break;
}
bitrate += 10000;
if (bitrate >= 2 * original_bitrate) {
break;
}
// original prescaler calculation from betaflight. bi-dir dshot is incredibly sensitive to the bitrate
const uint32_t target_frequency = bitrate * bit_width;
uint32_t prescaler = uint32_t(lrintf((float) group.pwm_drv->clock / (bitrate * bit_width) + 0.01f) - 1);
uint32_t freq = group.pwm_drv->clock / prescaler;
if (freq > target_frequency && !choose_high) {
prescaler++;
} else if (freq < target_frequency && choose_high) {
prescaler--;
}
if (prescaler > 0x8000) {
group.dma_handle->unlock();
return false;
}
freq = group.pwm_drv->clock / prescaler;
group.pwm_cfg.frequency = freq;
group.pwm_cfg.period = bit_width;
group.pwm_cfg.dier = TIM_DIER_UDE;
group.pwm_cfg.cr2 = 0;
group.bit_width_mul = (freq + (target_frequency/2)) / target_frequency;
//hal.console->printf("CLOCK=%u BW=%u FREQ=%u BR=%u MUL=%u PRE=%u\n", unsigned(group.pwm_drv->clock), unsigned(bit_width), unsigned(group.pwm_cfg.frequency),
// unsigned(bitrate), unsigned(group.bit_width_mul), unsigned(prescaler));
for (uint8_t j=0; j<4; j++) {
pwmmode_t mode = group.pwm_cfg.channels[j].mode;
if (mode != PWM_OUTPUT_DISABLED) {
@ -775,7 +828,7 @@ void RCOutput::set_group_mode(pwm_group &group)
// configure timer driver for DMAR at requested rate
if (!setup_group_DMA(group, rate, bit_period, active_high,
MAX(DSHOT_BUFFER_LENGTH, GCR_TELEMETRY_BUFFER_LEN), false, pulse_send_time_us)) {
MAX(DSHOT_BUFFER_LENGTH, GCR_TELEMETRY_BUFFER_LEN), group.current_mode != MODE_PWM_DSHOT150, pulse_send_time_us)) {
group.current_mode = MODE_PWM_NORMAL;
break;
}
@ -1013,7 +1066,7 @@ void RCOutput::trigger_groups(void)
periodic timer. This is used for oneshot and dshot modes, plus for
safety switch update. Runs every 1000us.
*/
void RCOutput::timer_tick(uint32_t last_run_us)
void RCOutput::timer_tick(uint32_t time_out_us)
{
safety_update();
@ -1022,20 +1075,21 @@ void RCOutput::timer_tick(uint32_t last_run_us)
}
// if we have enough time left send out LED data
if (serial_led_pending && AP_HAL::micros() - last_run_us < 500) {
if (serial_led_pending && (time_out_us > (AP_HAL::micros() + (_dshot_period_us >> 1)))) {
serial_led_pending = false;
for (auto &group : pwm_group_list) {
serial_led_pending |= !serial_led_send(group);
}
// release locks on the groups that are pending in reverse order
dshot_collect_dma_locks(last_run_us);
dshot_collect_dma_locks(time_out_us);
}
if (min_pulse_trigger_us == 0) {
return;
}
#if !APM_BUILD_TYPE(APM_BUILD_ArduCopter)
// this exists simply to cater for non-multirotors whose loop rate might be 50Hz
uint32_t now = AP_HAL::micros();
if (now > min_pulse_trigger_us &&
@ -1043,10 +1097,11 @@ void RCOutput::timer_tick(uint32_t last_run_us)
// trigger at a minimum of 250Hz
trigger_groups();
}
#endif
}
// send dshot for all groups that support it
void RCOutput::dshot_send_groups()
void RCOutput::dshot_send_groups(uint32_t time_out_us)
{
if (serial_group) {
return;
@ -1055,7 +1110,7 @@ void RCOutput::dshot_send_groups()
// actually do a dshot send
for (auto &group : pwm_group_list) {
if (group.can_send_dshot_pulse()) {
dshot_send(group);
dshot_send(group, time_out_us);
// delay sending the next group by the same amount as the IRQ dead time
// to avoid irq collisions
if (group.bdshot.enabled) {
@ -1154,7 +1209,7 @@ void RCOutput::fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t
This call be called in blocking mode from the timer, in which case it waits for the DMA lock.
In normal operation it doesn't wait for the DMA lock.
*/
void RCOutput::dshot_send(pwm_group &group)
void RCOutput::dshot_send(pwm_group &group, uint32_t time_out_us)
{
#ifndef DISABLE_DSHOT
if (irq.waiter || (group.dshot_state != DshotState::IDLE && group.dshot_state != DshotState::RECV_COMPLETE)) {
@ -1165,6 +1220,14 @@ void RCOutput::dshot_send(pwm_group &group)
// first make sure we have the DMA channel before anything else
osalDbgAssert(!group.dma_handle->is_locked(), "DMA handle is already locked");
group.dma_handle->lock();
// if we are sharing UP channels then it might have taken a long time to get here,
// if there's not enough time to actually send a pulse then cancel
if (AP_HAL::micros() + group.dshot_pulse_time_us > time_out_us) {
group.dma_handle->unlock();
return;
}
// only the timer thread releases the locks
group.dshot_waiter = rcout_thread_ctx;
#ifdef HAL_WITH_BIDIR_DSHOT
@ -1406,8 +1469,16 @@ void RCOutput::dma_cancel(pwm_group& group)
dmaStreamDisable(group.bdshot.ic_dma[group.bdshot.curr_telem_chan]);
}
#endif
// normally the CCR registers are reset by the final 0 in the DMA buffer
// since we are cancelling early they need to be reset to avoid infinite pulses
for (uint8_t i = 0; i < 4; i++) {
if (group.chan[i] != CHAN_DISABLED) {
group.pwm_drv->tim->CCR[i] = 0;
}
}
chVTResetI(&group.dma_timeout);
chEvtGetAndClearEventsI(group.dshot_event_mask);
group.dshot_state = DshotState::IDLE;
chSysUnlock();
}

19
libraries/AP_HAL_ChibiOS/RCOutput.h
View File

@ -150,6 +150,11 @@ public:
void set_bidir_dshot_mask(uint16_t mask) override;
#endif
/*
Set the dshot rate as a multiple of the loop rate
*/
void set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz) override;
/*
get safety switch state, used by Util.cpp
*/
@ -414,6 +419,15 @@ private:
#endif
} _bdshot;
// dshot period
uint32_t _dshot_period_us;
// dshot rate as a multiple of loop rate or 0 for 1Khz
uint8_t _dshot_rate;
// dshot periods since the last push()
uint8_t _dshot_cycle;
// virtual timer for post-push() pulses
virtual_timer_t _dshot_rate_timer;
uint16_t safe_pwm[max_channels]; // pwm to use when safety is on
bool corked;
// mask of channels that are running in high speed
@ -482,8 +496,9 @@ private:
uint16_t create_dshot_packet(const uint16_t value, bool telem_request, bool bidir_telem);
void fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t packet, uint16_t clockmul);
void dshot_send_groups();
void dshot_send(pwm_group &group);
void dshot_send_groups(uint32_t time_out_us);
void dshot_send(pwm_group &group, uint32_t time_out_us);
static void dshot_update_tick(void* p);
// release locks on the groups that are pending in reverse order
void dshot_collect_dma_locks(uint32_t last_run_us);
static void dma_up_irq_callback(void *p, uint32_t flags);

5
libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp
View File

@ -63,6 +63,11 @@ void RCOutput::set_bidir_dshot_mask(uint16_t mask)
bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group)
{
// check if already allocated
if (group.has_ic_dma()) {
return true;
}
bool set_curr_chan = false;
for (uint8_t i = 0; i < 4; i++) {