From cb3b37d1819215e45e52e67f195c17a4bd65d23c Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 31 Mar 2021 22:57:53 +0100 Subject: [PATCH] AP_HAL_ChibiOS: ensure that non-dshot modes get regular 1Khz updates --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 22 +++++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index d742789ab4..8bc5c8b622 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -189,7 +189,9 @@ void RCOutput::rcout_thread() // now unlock everything dshot_collect_dma_locks(time_out_us); - _dshot_cycle = (_dshot_cycle + 1) % _dshot_rate; + if (_dshot_rate > 0) { + _dshot_cycle = (_dshot_cycle + 1) % _dshot_rate; + } } // process any pending RC output requests @@ -416,7 +418,8 @@ void RCOutput::set_default_rate(uint16_t freq_hz) } /* - Set the dshot rate as a multiple of the loop rate + Set the dshot rate as a multiple of the loop rate. + This is called late after init_ardupilot() so groups will have been setup */ void RCOutput::set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz) { @@ -427,6 +430,17 @@ void RCOutput::set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz) chEvtSignal(rcout_thread_ctx, EVT_PWM_START); return; } + // if there are non-dshot channels then do likewise + for (auto &group : pwm_group_list) { + if (group.current_mode == MODE_PWM_ONESHOT || + group.current_mode == MODE_PWM_ONESHOT125 || + group.current_mode == MODE_PWM_BRUSHED) { + _dshot_period_us = 1000UL; + _dshot_rate = 0; + chEvtSignal(rcout_thread_ctx, EVT_PWM_START); + return; + } + } uint16_t drate = dshot_rate * loop_rate_hz; _dshot_rate = dshot_rate; @@ -1126,8 +1140,7 @@ void RCOutput::timer_tick(uint32_t 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 && @@ -1135,7 +1148,6 @@ void RCOutput::timer_tick(uint32_t time_out_us) // trigger at a minimum of 250Hz trigger_groups(); } -#endif } // send dshot for all groups that support it