From 057340cb1d964c9b1c81ed9bb99b6a354cc5ef7f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 2 Apr 2022 19:05:30 +1100 Subject: [PATCH] HAL_ChibiOS: prevent long timeouts in DShot this prevents bad calculated timeouts in DShot. The timeout would sometimes come out as 0xFFFFFFFF, which led to an assert and could block the thread This fix is meant to be minimilistic to allow it to be merged easily into 4.2. A better fix would fix all the uint32_t wrap handling in DShot --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 6a3466e855..eb9d45e30d 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -237,14 +237,23 @@ void RCOutput::dshot_collect_dma_locks(uint32_t time_out_us) // if we have time left wait for the event eventmask_t mask = 0; const uint32_t pulse_elapsed_us = now - group.last_dmar_send_us; + uint32_t wait_us = 0; 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))); + wait_us = 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)); + wait_us = group.dshot_pulse_send_time_us - pulse_elapsed_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 1200us) + // to prevent bugs in handling timer wrap + const uint32_t max_delay_us = 1200; + const uint32_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, chTimeUS2I(wait_us)); + // no time left cancel and restart if (!mask) { dma_cancel(group);