From e1310b20829fd1ef8d4dec6348804cd432163c80 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 14 Nov 2021 14:18:09 +1100 Subject: [PATCH] AP_Scheduler: allow specification of Scheduler table priorities --- libraries/AP_Scheduler/AP_Scheduler.cpp | 115 ++++++++++++++++-- libraries/AP_Scheduler/AP_Scheduler.h | 17 +-- .../Scheduler_test/Scheduler_test.cpp | 33 +++-- 3 files changed, 141 insertions(+), 24 deletions(-) diff --git a/libraries/AP_Scheduler/AP_Scheduler.cpp b/libraries/AP_Scheduler/AP_Scheduler.cpp index 802bb281de..4f40119658 100644 --- a/libraries/AP_Scheduler/AP_Scheduler.cpp +++ b/libraries/AP_Scheduler/AP_Scheduler.cpp @@ -107,16 +107,17 @@ void AP_Scheduler::init(const AP_Scheduler::Task *tasks, uint8_t num_tasks, uint } _last_loop_time_s = 1.0 / _loop_rate_hz; + _vehicle_tasks = tasks; + _num_vehicle_tasks = num_tasks; + AP_Vehicle* vehicle = AP::vehicle(); if (vehicle != nullptr) { - vehicle->get_common_scheduler_tasks(_common_tasks, _num_tasks); + vehicle->get_common_scheduler_tasks(_common_tasks, _num_common_tasks); } - _num_tasks += num_tasks; - _tasks = tasks; - _num_unshared_tasks = num_tasks; - _last_run = new uint16_t[_num_tasks]; - memset(_last_run, 0, sizeof(_last_run[0]) * _num_tasks); + _num_tasks = _num_vehicle_tasks + _num_common_tasks; + + _last_run = new uint16_t[_num_tasks]; _tick_counter = 0; // setup initial performance counters @@ -128,6 +129,25 @@ void AP_Scheduler::init(const AP_Scheduler::Task *tasks, uint8_t num_tasks, uint } _log_performance_bit = log_performance_bit; + + // sanity check the task lists to ensure the priorities are + // never decrease + uint8_t old = 0; + for (uint8_t i=0; i<_num_common_tasks; i++) { + if (_common_tasks[i].priority < old){ + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + break; + } + old = _common_tasks[i].priority; + } + old = 0; + for (uint8_t i=0; i<_num_vehicle_tasks; i++) { + if (_vehicle_tasks[i].priority < old) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + break; + } + old = _vehicle_tasks[i].priority; + } } // one tick has passed @@ -157,8 +177,40 @@ void AP_Scheduler::run(uint32_t time_available) uint32_t run_started_usec = AP_HAL::micros(); uint32_t now = run_started_usec; + uint8_t vehicle_tasks_offset = 0; + uint8_t common_tasks_offset = 0; + for (uint8_t i=0; i<_num_tasks; i++) { - const AP_Scheduler::Task& task = (i < _num_unshared_tasks) ? _tasks[i] : _common_tasks[i - _num_unshared_tasks]; + // determine which of the common task / vehicle task to run + bool run_vehicle_task = false; + if (vehicle_tasks_offset < _num_vehicle_tasks && + common_tasks_offset < _num_common_tasks) { + // still have entries on both lists; compare the + // priorities. In case of a tie the vehicle-specific + // entry wins. + const Task &vehicle_task = _vehicle_tasks[vehicle_tasks_offset]; + const Task &common_task = _common_tasks[common_tasks_offset]; + if (vehicle_task.priority <= common_task.priority) { + run_vehicle_task = true; + } + } else if (vehicle_tasks_offset < _num_vehicle_tasks) { + // out of common tasks to run + run_vehicle_task = true; + } else if (common_tasks_offset < _num_common_tasks) { + // out of vehicle tasks to run + run_vehicle_task = false; + } else { + // this is an error; the outside loop should have terminated + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + break; + } + + const AP_Scheduler::Task &task = run_vehicle_task ? _vehicle_tasks[vehicle_tasks_offset] : _common_tasks[common_tasks_offset]; + if (run_vehicle_task) { + vehicle_tasks_offset++; + } else { + common_tasks_offset++; + } const uint16_t dt = _tick_counter - _last_run[i]; // we allow 0 to mean loop rate @@ -417,9 +469,54 @@ void AP_Scheduler::task_info(ExpandingString &str) } } + uint8_t vehicle_tasks_offset = 0; + uint8_t common_tasks_offset = 0; + for (uint8_t i = 0; i < _num_tasks + 1; i++) { - const char* task_name = (i < _num_unshared_tasks) ? _tasks[i].name : i == _num_tasks ? "fast_loop" : _common_tasks[i - _num_unshared_tasks].name; - const AP::PerfInfo::TaskInfo* ti = perf_info.get_task_info(i); + const AP::PerfInfo::TaskInfo* ti; + const char *task_name; + if (i == 0) { + // put the fast-loop entry at the top of the list - it's the last task in perf_info + ti = perf_info.get_task_info(_num_tasks); + task_name = "fast_loop"; + } else { + ti = perf_info.get_task_info(i - 1); + + // now find the task name: + + // determine which of the common task / vehicle task to run + bool run_vehicle_task = false; + if (vehicle_tasks_offset < _num_vehicle_tasks && + common_tasks_offset < _num_common_tasks) { + // still have entries on both lists; compare the + // priorities. In case of a tie the vehicle-specific + // entry wins. + const Task &vehicle_task = _vehicle_tasks[vehicle_tasks_offset]; + const Task &common_task = _common_tasks[common_tasks_offset]; + if (vehicle_task.priority <= common_task.priority) { + run_vehicle_task = true; + } + } else if (vehicle_tasks_offset < _num_vehicle_tasks) { + // out of common tasks to run + run_vehicle_task = true; + } else if (common_tasks_offset < _num_common_tasks) { + // out of vehicle tasks to run + run_vehicle_task = false; + } else { + // this is an error; the outside loop should have terminated + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return; + } + + if (run_vehicle_task) { + task_name = _vehicle_tasks[vehicle_tasks_offset++].name; + } else { + task_name = _common_tasks[common_tasks_offset++].name; + } + // the loop counter i is adjusted here because we emit the + // fast-loop entry first but it appears last in the + // perf_info list + } uint16_t avg = 0; float pct = 0.0f; diff --git a/libraries/AP_Scheduler/AP_Scheduler.h b/libraries/AP_Scheduler/AP_Scheduler.h index 311dd96a74..f172460bf5 100644 --- a/libraries/AP_Scheduler/AP_Scheduler.h +++ b/libraries/AP_Scheduler/AP_Scheduler.h @@ -40,11 +40,12 @@ /* useful macro for creating scheduler task table */ -#define SCHED_TASK_CLASS(classname, classptr, func, _rate_hz, _max_time_micros) { \ +#define SCHED_TASK_CLASS(classname, classptr, func, _rate_hz, _max_time_micros, _priority) { \ .function = FUNCTOR_BIND(classptr, &classname::func, void),\ AP_SCHEDULER_NAME_INITIALIZER(classname, func)\ .rate_hz = _rate_hz,\ - .max_time_micros = _max_time_micros\ + .max_time_micros = _max_time_micros, \ + .priority = _priority \ } /* @@ -81,6 +82,7 @@ public: const char *name; float rate_hz; uint16_t max_time_micros; + uint8_t priority; // task priority }; enum class Options : uint8_t { @@ -189,18 +191,17 @@ private: // calculated loop period in seconds float _loop_period_s; - // progmem list of tasks to run - const struct Task *_tasks; + // list of tasks to run + const struct Task *_vehicle_tasks; + uint8_t _num_vehicle_tasks; - // progmem list of common tasks to run + // list of common tasks to run const struct Task *_common_tasks; + uint8_t _num_common_tasks; // total number of tasks in _tasks and _common_tasks list uint8_t _num_tasks; - // number of tasks in _tasks list - uint8_t _num_unshared_tasks; - // number of 'ticks' that have passed (number of times that // tick() has been called uint16_t _tick_counter; diff --git a/libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp b/libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp index d883b075af..736ecbfed4 100644 --- a/libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp +++ b/libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp @@ -38,17 +38,36 @@ private: static AP_BoardConfig board_config; static SchedTest schedtest; -#define SCHED_TASK(func, _interval_ticks, _max_time_micros) SCHED_TASK_CLASS(SchedTest, &schedtest, func, _interval_ticks, _max_time_micros) +#define SCHED_TASK(func, _interval_ticks, _max_time_micros, _priority) SCHED_TASK_CLASS(SchedTest, &schedtest, func, _interval_ticks, _max_time_micros, _priority) /* - scheduler table - all regular tasks are listed here, along with how - often they should be called (in 20ms units) and the maximum time - they are expected to take (in microseconds) + scheduler table - all regular tasks should be listed here. + + All entries in this table must be ordered by priority. + + This table is interleaved with the table in AP_Vehicle to determine + the order in which tasks are run. Convenience methods SCHED_TASK + and SCHED_TASK_CLASS are provided to build entries in this structure: + +SCHED_TASK arguments: + - name of static function to call + - rate (in Hertz) at which the function should be called + - expected time (in MicroSeconds) that the function should take to run + - priority (0 through 255, lower number meaning higher priority) + +SCHED_TASK_CLASS arguments: + - class name of method to be called + - instance on which to call the method + - method to call on that instance + - rate (in Hertz) at which the method should be called + - expected time (in MicroSeconds) that the method should take to run + - priority (0 through 255, lower number meaning higher priority) + */ const AP_Scheduler::Task SchedTest::scheduler_tasks[] = { - SCHED_TASK(ins_update, 50, 1000), - SCHED_TASK(one_hz_print, 1, 1000), - SCHED_TASK(five_second_call, 0.2, 1800), + SCHED_TASK(ins_update, 50, 1000, 3), + SCHED_TASK(one_hz_print, 1, 1000, 6), + SCHED_TASK(five_second_call, 0.2, 1800, 9), };