From 86b6f3a2c69088f8371fc903e9ea8269d38fbb79 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 17 Apr 2023 14:12:25 +1000 Subject: [PATCH] AP_HAL_ChibiOS: change to 16 bit timeout in event interface prevent issues on 16 bit timers where we can end up with TIME_INFINITE --- libraries/AP_HAL_ChibiOS/EventSource.cpp | 7 ++++--- libraries/AP_HAL_ChibiOS/EventSource.h | 2 +- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 2 +- libraries/AP_HAL_ChibiOS/Scheduler.cpp | 1 + 4 files changed, 7 insertions(+), 5 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/EventSource.cpp b/libraries/AP_HAL_ChibiOS/EventSource.cpp index 4977a46be1..f1821bca00 100644 --- a/libraries/AP_HAL_ChibiOS/EventSource.cpp +++ b/libraries/AP_HAL_ChibiOS/EventSource.cpp @@ -5,16 +5,17 @@ using namespace ChibiOS; #if CH_CFG_USE_EVENTS == TRUE -bool EventSource::wait(uint64_t duration, AP_HAL::EventHandle *evt_handle) +bool EventSource::wait(uint16_t duration_us, AP_HAL::EventHandle *evt_handle) { chibios_rt::EventListener evt_listener; eventmask_t evt_mask = evt_handle->get_evt_mask(); msg_t ret = msg_t(); ch_evt_src_.registerMask(&evt_listener, evt_mask); - if (duration == 0) { + if (duration_us == 0) { ret = chEvtWaitAnyTimeout(evt_mask, TIME_IMMEDIATE); } else { - ret = chEvtWaitAnyTimeout(evt_mask, MAX(CH_CFG_ST_TIMEDELTA, chTimeUS2I(duration))); + const sysinterval_t wait_us = MIN(TIME_MAX_INTERVAL, MAX(CH_CFG_ST_TIMEDELTA, chTimeUS2I(duration_us))); + ret = chEvtWaitAnyTimeout(evt_mask, wait_us); } ch_evt_src_.unregister(&evt_listener); return ret == MSG_OK; diff --git a/libraries/AP_HAL_ChibiOS/EventSource.h b/libraries/AP_HAL_ChibiOS/EventSource.h index 5990228441..a1dda78b2e 100644 --- a/libraries/AP_HAL_ChibiOS/EventSource.h +++ b/libraries/AP_HAL_ChibiOS/EventSource.h @@ -21,6 +21,6 @@ public: void signalI(uint32_t evt_mask) override; // Wait on an Event handle, method for internal use by EventHandle - bool wait(uint64_t duration, AP_HAL::EventHandle* evt_handle) override; + bool wait(uint16_t duration_us, AP_HAL::EventHandle* evt_handle) override; }; #endif //#if CH_CFG_USE_EVENTS == TRUE diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 1acb4a4354..eed80928f3 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -255,7 +255,7 @@ void RCOutput::dshot_collect_dma_locks(uint64_t time_out_us) const uint32_t max_delay_us = _dshot_period_us; 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)); + mask = chEvtWaitOneTimeout(group.dshot_event_mask, MIN(TIME_MAX_INTERVAL, chTimeUS2I(wait_us))); // no time left cancel and restart if (!mask) { diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp index 7d4bf11c8f..cabc756c03 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp +++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp @@ -153,6 +153,7 @@ void Scheduler::delay_microseconds(uint16_t usec) // calling with ticks == 0 causes a hard fault on ChibiOS ticks = 1; } + ticks = MIN(TIME_MAX_INTERVAL, ticks); chThdSleep(MAX(ticks,CH_CFG_ST_TIMEDELTA)); //Suspends Thread for desired microseconds }