Skip to content

Commit

Permalink
AP_HAL_ChibiOS: provide start time and timeout to all dshot APIs that…
Browse files Browse the repository at this point in the history 
… require it

correct timeout checking for dshot across timer wrap boundaries
fix trigger_groups timeout checks
use rcout_timer_t instead of uint32_t or uint64_t
  • Loading branch information
@andyp1per
andyp1per committed May 29, 2023
1 parent 60c6e6c commit 03cae25
Show file tree
Hide file tree
Showing 3 changed files with 62 additions and 62 deletions.
88 changes: 40 additions & 48 deletions libraries/AP_HAL_ChibiOS/RCOutput.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -195,7 +195,7 @@ void RCOutput::led_thread()
// actually sending out data - thus we need to work out how much time we have left to collect the locks

// process any pending LED output requests
led_timer_tick(LED_OUTPUT_PERIOD_US + AP_HAL::micros64());
led_timer_tick(rcout_micros(), LED_OUTPUT_PERIOD_US);
}
}

Expand All @@ -204,8 +204,8 @@ void RCOutput::led_thread()
*/
void RCOutput::rcout_thread()
{
uint64_t last_thread_run_us = 0; // last time we did a 1kHz run of rcout
uint64_t last_cycle_run_us = 0;
rcout_timer_t last_thread_run_us = 0; // last time we did a 1kHz run of rcout
rcout_timer_t last_cycle_run_us = 0;

rcout_thread_ctx = chThdGetSelfX();

Expand All @@ -220,11 +220,11 @@ void RCOutput::rcout_thread()
const auto mask = chEvtWaitOne(EVT_PWM_SEND | EVT_PWM_SYNTHETIC_SEND);
const bool have_pwm_event = (mask & (EVT_PWM_SEND | EVT_PWM_SYNTHETIC_SEND)) != 0;
// start the clock
last_thread_run_us = AP_HAL::micros64();
last_thread_run_us = rcout_micros();

// this is when the cycle is supposed to start
if (_dshot_cycle == 0 && have_pwm_event) {
last_cycle_run_us = AP_HAL::micros64();
last_cycle_run_us = rcout_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);
Expand All @@ -233,33 +233,32 @@ void RCOutput::rcout_thread()

// 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
uint64_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;
}
const rcout_timer_t timeout_period_us = _dshot_rate ? (_dshot_cycle + 1) * _dshot_period_us : _dshot_period_us;
// timeout is measured from the beginning of the push() that initiated it to preserve periodicity
const rcout_timer_t cycle_start_us = _dshot_rate ? last_cycle_run_us : last_thread_run_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 && have_pwm_event) {
dshot_send_groups(time_out_us);
dshot_send_groups(cycle_start_us, timeout_period_us);

// now unlock everything
dshot_collect_dma_locks(time_out_us);
dshot_collect_dma_locks(cycle_start_us, timeout_period_us);

if (_dshot_rate > 0) {
_dshot_cycle = (_dshot_cycle + 1) % _dshot_rate;
}
}

// process any pending RC output requests
timer_tick(time_out_us);
timer_tick(cycle_start_us, timeout_period_us);
#if RCOU_DSHOT_TIMING_DEBUG
static bool output_masks = true;
if (AP_HAL::millis() > 5000 && output_masks) {
output_masks = false;
hal.console->printf("bdmask 0x%x, en_mask 0x%lx, 3dmask 0x%x:\n", _bdshot.mask, en_mask, _reversible_mask);
hal.console->printf("bdmask 0x%lx, en_mask 0x%lx, 3dmask 0x%lx:\n", _bdshot.mask, en_mask, _reversible_mask);
for (auto &group : pwm_group_list) {
hal.console->printf(" timer %u: ch_mask 0x%x, en_mask 0x%x\n", group.timer_id, group.ch_mask, group.en_mask);
hal.console->printf(" timer %u: ch_mask 0x%lx, en_mask 0x%lx\n", group.timer_id, group.ch_mask, group.en_mask);
}
}
#endif
Expand All @@ -280,7 +279,7 @@ __RAMFUNC__ void RCOutput::dshot_update_tick(virtual_timer_t* vt, void* p)

#if AP_HAL_SHARED_DMA_ENABLED
// release locks on the groups that are pending in reverse order
void RCOutput::dshot_collect_dma_locks(uint64_t time_out_us, bool led_thread)
void RCOutput::dshot_collect_dma_locks(rcout_timer_t cycle_start_us, rcout_timer_t timeout_period_us, bool led_thread)
{
if (NUM_GROUPS == 0) {
return;
Expand All @@ -294,25 +293,22 @@ void RCOutput::dshot_collect_dma_locks(uint64_t time_out_us, bool led_thread)

if (group.dma_handle != nullptr && group.dma_handle->is_locked()) {
// calculate how long we have left
uint64_t now = AP_HAL::micros64();
rcout_timer_t now = rcout_micros();
// if we have time left wait for the event
eventmask_t mask = 0;
const uint64_t pulse_elapsed_us = now - group.last_dmar_send_us;
uint32_t wait_us = 0;
if (now < time_out_us) {
wait_us = time_out_us - now;
}
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
wait_us = MAX(wait_us, group.dshot_pulse_send_time_us - pulse_elapsed_us);
}
const rcout_timer_t pulse_remaining_us
= AP_HAL::timeout_remaining(group.last_dmar_send_us, now, group.dshot_pulse_send_time_us);
const rcout_timer_t timeout_remaining_us
= AP_HAL::timeout_remaining(cycle_start_us, now, timeout_period_us);
// better to let the burst write in progress complete rather than cancelling mid way through
rcout_timer_t wait_us = MAX(pulse_remaining_us, timeout_remaining_us);

// waiting for a very short period of time can cause a
// timer wrap with ChibiOS timers. Use CH_CFG_ST_TIMEDELTA
// as minimum. Don't allow for a very long delay (over _dshot_period_us)
// to prevent bugs in handling timer wrap
const uint32_t max_delay_us = led_thread ? LED_OUTPUT_PERIOD_US : _dshot_period_us;
const uint32_t min_delay_us = 10; // matches our CH_CFG_ST_TIMEDELTA
const rcout_timer_t max_delay_us = led_thread ? LED_OUTPUT_PERIOD_US : _dshot_period_us;
const rcout_timer_t min_delay_us = 10; // matches our CH_CFG_ST_TIMEDELTA
wait_us = constrain_uint32(wait_us, min_delay_us, max_delay_us);
mask = chEvtWaitOneTimeout(group.dshot_event_mask, MIN(TIME_MAX_INTERVAL, chTimeUS2I(wait_us)));

Expand Down Expand Up @@ -769,7 +765,7 @@ void RCOutput::push_local(void)
if (widest_pulse > 2300) {
widest_pulse = 2300;
}
trigger_widest_pulse = widest_pulse;
trigger_widest_pulse = widest_pulse + 50;

trigger_groupmask = need_trigger;

Expand Down Expand Up @@ -880,7 +876,7 @@ void RCOutput::print_group_setup_error(pwm_group &group, const char* error_strin
This is used for both DShot and serial output
*/
bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_width, bool active_high, const uint16_t buffer_length,
uint32_t pulse_time_us, bool is_dshot)
rcout_timer_t pulse_time_us, bool is_dshot)
{
#ifndef DISABLE_DSHOT
// for dshot we setup for DMAR based output
Expand Down Expand Up @@ -1238,15 +1234,15 @@ bool RCOutput::enable_px4io_sbus_out(uint16_t rate_hz)
/*
trigger output groups for oneshot or dshot modes
*/
void RCOutput::trigger_groups(void)
void RCOutput::trigger_groups()
{
if (!chMtxTryLock(&trigger_mutex)) {
return;
}
uint64_t now = AP_HAL::micros64();
if (now < min_pulse_trigger_us) {

if (!AP_HAL::timeout_expired_us(last_pulse_trigger_us, trigger_widest_pulse)) {
// guarantee minimum pulse separation
hal.scheduler->delay_microseconds(min_pulse_trigger_us - now);
hal.scheduler->delay_microseconds(AP_HAL::timeout_remaining_us(last_pulse_trigger_us, trigger_widest_pulse));
}

osalSysLock();
Expand Down Expand Up @@ -1275,7 +1271,7 @@ void RCOutput::trigger_groups(void)
calculate time that we are allowed to trigger next pulse
to guarantee at least a 50us gap between pulses
*/
min_pulse_trigger_us = AP_HAL::micros64() + trigger_widest_pulse + 50;
last_pulse_trigger_us = rcout_micros();

chMtxUnlock(&trigger_mutex);
}
Expand All @@ -1284,20 +1280,17 @@ 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(uint64_t time_out_us)
void RCOutput::timer_tick(rcout_timer_t cycle_start_us, rcout_timer_t timeout_period_us)
{
if (serial_group) {
return;
}

if (min_pulse_trigger_us == 0) {
if (last_pulse_trigger_us == 0) {
return;
}

uint64_t now = AP_HAL::micros64();

if (now > min_pulse_trigger_us &&
now - min_pulse_trigger_us > 4000) {
if (AP_HAL::timeout_expired_us(last_pulse_trigger_us, trigger_widest_pulse + 4000)) {
// trigger at a minimum of 250Hz
trigger_groups();
}
Expand All @@ -1306,7 +1299,7 @@ void RCOutput::timer_tick(uint64_t time_out_us)
/*
periodic timer called from led thread. This is used for LED output
*/
void RCOutput::led_timer_tick(uint64_t time_out_us)
void RCOutput::led_timer_tick(rcout_timer_t cycle_start_us, rcout_timer_t timeout_period_us)
{
if (serial_group) {
return;
Expand All @@ -1320,12 +1313,12 @@ void RCOutput::led_timer_tick(uint64_t time_out_us)
}

// release locks on the groups that are pending in reverse order
dshot_collect_dma_locks(time_out_us, true);
dshot_collect_dma_locks(cycle_start_us, timeout_period_us, true);
}
}

// send dshot for all groups that support it
void RCOutput::dshot_send_groups(uint64_t time_out_us)
void RCOutput::dshot_send_groups(rcout_timer_t cycle_start_us, rcout_timer_t timeout_period_us)
{
#ifndef DISABLE_DSHOT
if (serial_group) {
Expand All @@ -1346,7 +1339,7 @@ void RCOutput::dshot_send_groups(uint64_t time_out_us)
command_sent = dshot_send_command(group, _dshot_current_command.command, _dshot_current_command.chan);
// actually do a dshot send
} else if (group.can_send_dshot_pulse()) {
dshot_send(group, time_out_us);
dshot_send(group, cycle_start_us, timeout_period_us);
}
}

Expand Down Expand Up @@ -1454,7 +1447,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, uint64_t time_out_us)
void RCOutput::dshot_send(pwm_group &group, rcout_timer_t cycle_start_us, rcout_timer_t timeout_period_us)
{
#ifndef DISABLE_DSHOT
if (irq.waiter || (group.dshot_state != DshotState::IDLE && group.dshot_state != DshotState::RECV_COMPLETE)) {
Expand All @@ -1468,8 +1461,7 @@ void RCOutput::dshot_send(pwm_group &group, uint64_t time_out_us)

// 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::micros64() + group.dshot_pulse_time_us > time_out_us) {
if (AP_HAL::timeout_remaining_us(cycle_start_us, timeout_period_us) < group.dshot_pulse_time_us) {
group.dma_handle->unlock();
return;
}
Expand Down Expand Up @@ -1684,7 +1676,7 @@ void RCOutput::send_pulses_DMAR(pwm_group &group, uint32_t buffer_length)

dmaStreamEnable(group.dma);
// record when the transaction was started
group.last_dmar_send_us = AP_HAL::micros64();
group.last_dmar_send_us = rcout_micros();
#endif //#ifndef DISABLE_DSHOT
}

Expand Down
34 changes: 21 additions & 13 deletions libraries/AP_HAL_ChibiOS/RCOutput.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,14 @@
#define DISABLE_DSHOT
#endif

#ifdef AP_RCOUT_USE_64BIT_TIME
typedef uint64_t rcout_timer_t;
#define rcout_micros() AP_HAL::micros64()
#else
typedef uint32_t rcout_timer_t;
#define rcout_micros() AP_HAL::micros()
#endif

#define RCOU_DSHOT_TIMING_DEBUG 0

class ChibiOS::RCOutput : public AP_HAL::RCOutput
Expand Down Expand Up @@ -102,12 +110,12 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
/*
timer push (for oneshot min rate)
*/
void timer_tick(uint64_t last_run_us);
void timer_tick(rcout_timer_t cycle_start_us, rcout_timer_t timeout_period_us);

/*
LED push
*/
void led_timer_tick(uint64_t last_run_us);
void led_timer_tick(rcout_timer_t cycle_start_us, rcout_timer_t timeout_period_us);

/*
setup for serial output to a set of ESCs, using the given
Expand Down Expand Up @@ -325,9 +333,9 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
uint32_t bit_width_mul;
uint32_t rc_frequency;
bool in_serial_dma;
uint64_t last_dmar_send_us;
uint64_t dshot_pulse_time_us;
uint64_t dshot_pulse_send_time_us;
rcout_timer_t last_dmar_send_us;
rcout_timer_t dshot_pulse_time_us;
rcout_timer_t dshot_pulse_send_time_us;
virtual_timer_t dma_timeout;

// serial LED support
Expand Down Expand Up @@ -374,7 +382,7 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
#if RCOU_DSHOT_TIMING_DEBUG
uint16_t telem_rate[4];
uint16_t telem_err_rate[4];
uint64_t last_print; // debug
rcout_timer_t last_print; // debug
#endif
#endif
} bdshot;
Expand Down Expand Up @@ -508,7 +516,7 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
} _bdshot;

// dshot period
uint32_t _dshot_period_us = 400;
rcout_timer_t _dshot_period_us = 400;
// dshot rate as a multiple of loop rate or 0 for 1Khz
uint8_t _dshot_rate;
// dshot periods since the last push()
Expand Down Expand Up @@ -549,8 +557,8 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
// mask of active ESCs
uint32_t _active_escs_mask;

// min time to trigger next pulse to prevent overlap
uint64_t min_pulse_trigger_us;
// last time pulse was triggererd used to prevent overlap
rcout_timer_t last_pulse_trigger_us;

// mutex for oneshot triggering
mutex_t trigger_mutex;
Expand Down Expand Up @@ -619,19 +627,19 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput
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(uint64_t time_out_us);
void dshot_send(pwm_group &group, uint64_t time_out_us);
void dshot_send_groups(rcout_timer_t cycle_start_us, rcout_timer_t timeout_us);
void dshot_send(pwm_group &group, rcout_timer_t cycle_start_us, rcout_timer_t timeout_us);
bool dshot_send_command(pwm_group &group, uint8_t command, uint8_t chan);
static void dshot_update_tick(virtual_timer_t*, void* p);
static void dshot_send_next_group(void* p);
// release locks on the groups that are pending in reverse order
void dshot_collect_dma_locks(uint64_t last_run_us, bool led_thread = false);
void dshot_collect_dma_locks(rcout_timer_t cycle_start_us, rcout_timer_t timeout_period_us, bool led_thread = false);
static void dma_up_irq_callback(void *p, uint32_t flags);
static void dma_unlock(virtual_timer_t*, void *p);
void dma_cancel(pwm_group& group);
bool mode_requires_dma(enum output_mode mode) const;
bool setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_width, bool active_high,
const uint16_t buffer_length, uint32_t pulse_time_us,
const uint16_t buffer_length, rcout_timer_t pulse_time_us,
bool is_dshot);
void send_pulses_DMAR(pwm_group &group, uint32_t buffer_length);
void set_group_mode(pwm_group &group);
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -441,7 +441,7 @@ bool RCOutput::bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan)
#endif
}

uint64_t now = AP_HAL::micros64();
rcout_timer_t now = rcout_micros();
if (chan == DEBUG_CHANNEL && (now - group.bdshot.last_print) > 1000000) {
hal.console->printf("TELEM: %d <%d Hz, %.1f%% err>", group.bdshot.erpm[chan], group.bdshot.telem_rate[chan],
100.0f * float(group.bdshot.telem_err_rate[chan]) / (group.bdshot.telem_err_rate[chan] + group.bdshot.telem_rate[chan]));
Expand Down

0 comments on commit 03cae25

Please sign in to comment.