AP_HAL_ChibiOS: allow forcing of trigger_groups()

This commit is contained in:
Andy Piper 2024-06-07 16:36:08 +01:00 committed by Andrew Tridgell
parent 76897e9674
commit 048a3af785
2 changed files with 9 additions and 1 deletions

View File

@ -1398,7 +1398,8 @@ void RCOutput::trigger_groups()
osalSysUnlock(); osalSysUnlock();
#if !defined(HAL_NO_RCOUT_THREAD) || HAL_DSHOT_ENABLED #if !defined(HAL_NO_RCOUT_THREAD) || HAL_DSHOT_ENABLED
// trigger a PWM send // trigger a PWM send
if (!in_soft_serial() && hal.scheduler->in_main_thread() && rcout_thread_ctx) { if (!in_soft_serial() &&
(hal.scheduler->in_main_thread() || force_trigger) && rcout_thread_ctx) {
chEvtSignal(rcout_thread_ctx, EVT_PWM_SEND); chEvtSignal(rcout_thread_ctx, EVT_PWM_SEND);
} }
#endif #endif

View File

@ -285,6 +285,11 @@ public:
*/ */
void rcout_thread(); void rcout_thread();
/*
Force group trigger from all callers rather than just from the main thread
*/
void force_trigger_groups(bool onoff) override { force_trigger = onoff; }
/* /*
timer information timer information
*/ */
@ -579,6 +584,8 @@ private:
uint8_t _dshot_cycle; uint8_t _dshot_cycle;
// virtual timer for post-push() pulses // virtual timer for post-push() pulses
virtual_timer_t _dshot_rate_timer; virtual_timer_t _dshot_rate_timer;
// force triggering of groups
bool force_trigger;
#if HAL_DSHOT_ENABLED #if HAL_DSHOT_ENABLED
// dshot commands // dshot commands