From ebc9616e00637f9194b2b77b4065d827a4550d2f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 5 Dec 2022 21:25:32 +1100 Subject: [PATCH] AP_Scheduler: guarantee that FAST_TASK tasks do run on every loop the breakup of the fast loop resulted in us sometimes (under heavy CPU load) not running a fast task on every loop --- libraries/AP_Scheduler/AP_Scheduler.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Scheduler/AP_Scheduler.cpp b/libraries/AP_Scheduler/AP_Scheduler.cpp index 7e2e708fa3..7cfc1dc203 100644 --- a/libraries/AP_Scheduler/AP_Scheduler.cpp +++ b/libraries/AP_Scheduler/AP_Scheduler.cpp @@ -275,10 +275,19 @@ void AP_Scheduler::run(uint32_t time_available) perf_info.update_task_info(i, time_taken, overrun); if (time_taken >= time_available) { + /* + we are out of time, but we need to keep walking the task + table in case there is another fast loop task after this + task, plus we need to update the accouting so we can + work out if we need to allocate extra time for the loop + (lower the loop rate) + Just set time_available to zero, which means we will + only run fast tasks after this one + */ time_available = 0; - break; + } else { + time_available -= time_taken; } - time_available -= time_taken; } // update number of spare microseconds