mirror of https://github.com/ArduPilot/ardupilot
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
This commit is contained in:
parent
061b0580b3
commit
8055ba9837
|
@ -237,14 +237,23 @@ void RCOutput::dshot_collect_dma_locks(uint32_t time_out_us)
|
||||||
// if we have time left wait for the event
|
// if we have time left wait for the event
|
||||||
eventmask_t mask = 0;
|
eventmask_t mask = 0;
|
||||||
const uint32_t pulse_elapsed_us = now - group.last_dmar_send_us;
|
const uint32_t pulse_elapsed_us = now - group.last_dmar_send_us;
|
||||||
|
uint32_t wait_us = 0;
|
||||||
if (now < time_out_us) {
|
if (now < time_out_us) {
|
||||||
mask = chEvtWaitOneTimeout(group.dshot_event_mask,
|
wait_us = MAX(time_out_us - now, group.dshot_pulse_send_time_us - pulse_elapsed_us);
|
||||||
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) {
|
} 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
|
// better to let the burst write in progress complete rather than cancelling mid way through
|
||||||
mask = chEvtWaitOneTimeout(group.dshot_event_mask,
|
wait_us = group.dshot_pulse_send_time_us - pulse_elapsed_us;
|
||||||
chTimeUS2I(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
|
// no time left cancel and restart
|
||||||
if (!mask) {
|
if (!mask) {
|
||||||
dma_cancel(group);
|
dma_cancel(group);
|
||||||
|
|
Loading…
Reference in New Issue