From a16bb2f188712780b942f63781ce8ae034d37c47 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 1 Jan 2020 20:36:19 +0000 Subject: [PATCH] AP_Scheduler: allow registration of tasks at loop rate --- libraries/AP_Scheduler/AP_Scheduler.cpp | 3 ++- libraries/AP_Scheduler/AP_Scheduler.h | 1 + 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Scheduler/AP_Scheduler.cpp b/libraries/AP_Scheduler/AP_Scheduler.cpp index 0ef9810866..46793c08a6 100644 --- a/libraries/AP_Scheduler/AP_Scheduler.cpp +++ b/libraries/AP_Scheduler/AP_Scheduler.cpp @@ -160,7 +160,8 @@ void AP_Scheduler::run(uint32_t time_available) const AP_Scheduler::Task& task = (i < _num_unshared_tasks) ? _tasks[i] : _common_tasks[i - _num_unshared_tasks]; uint32_t dt = _tick_counter - _last_run[i]; - uint32_t interval_ticks = _loop_rate_hz / task.rate_hz; + // we allow 0 to mean loop rate + uint32_t interval_ticks = (is_zero(task.rate_hz) ? 1 : _loop_rate_hz / task.rate_hz); if (interval_ticks < 1) { interval_ticks = 1; } diff --git a/libraries/AP_Scheduler/AP_Scheduler.h b/libraries/AP_Scheduler/AP_Scheduler.h index c2d6c4301a..2ffe125c91 100644 --- a/libraries/AP_Scheduler/AP_Scheduler.h +++ b/libraries/AP_Scheduler/AP_Scheduler.h @@ -26,6 +26,7 @@ #include "PerfInfo.h" // loop perf monitoring #define AP_SCHEDULER_NAME_INITIALIZER(_name) .name = #_name, +#define LOOP_RATE 0 /* useful macro for creating scheduler task table