diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 6606401969..427456da4d 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -723,10 +723,16 @@ void RCOutput::trigger_groups(void) void RCOutput::timer_tick(void) { + uint64_t now = AP_HAL::micros64(); for (uint8_t i = 0; i < NUM_GROUPS; i++ ) { pwm_group &group = pwm_group_list[i]; - if (dshot_delayed_trigger_mask & (1U<= MODE_PWM_DSHOT150 && + group.current_mode <= MODE_PWM_DSHOT1200 && + now - group.last_dshot_send_us > 900) { + // 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(group, true); } } @@ -734,7 +740,6 @@ void RCOutput::timer_tick(void) min_pulse_trigger_us == 0) { return; } - uint64_t now = AP_HAL::micros64(); if (now > min_pulse_trigger_us && now - min_pulse_trigger_us > 10000) { // trigger at a minimum of 100Hz @@ -814,13 +819,10 @@ void RCOutput::dshot_send(pwm_group &group, bool blocking) return; } - uint8_t groupidx = &group - pwm_group_list; if (blocking) { group.dma_handle->lock(); - dshot_delayed_trigger_mask &= ~(1U<lock_nonblock()) { - dshot_delayed_trigger_mask |= 1U<