Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Scheduler: add common tasks from AP_Vehicle and manage them

Browse Source
This commit is contained in:
Andy Piper 2019-12-18 22:35:05 +00:00 committed by Andrew Tridgell
parent c16c60a761
commit debc13965a
2 changed files with 25 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_Scheduler/AP_Scheduler.cpp
View File

@ -95,8 +95,14 @@ AP_Scheduler *AP_Scheduler::get_singleton()
// initialise the scheduler // initialise the scheduler
void AP_Scheduler::init(const AP_Scheduler::Task *tasks, uint8_t num_tasks, uint32_t log_performance_bit) void AP_Scheduler::init(const AP_Scheduler::Task *tasks, uint8_t num_tasks, uint32_t log_performance_bit)
{ {
AP_Vehicle* vehicle = AP::vehicle();
if (vehicle != nullptr) {
vehicle->get_common_scheduler_tasks(_common_tasks, _num_tasks);
}
_num_tasks += num_tasks;
_tasks = tasks; _tasks = tasks;
_num_tasks = num_tasks; _num_unshared_tasks = num_tasks;
_last_run = new uint16_t[_num_tasks]; _last_run = new uint16_t[_num_tasks];
memset(_last_run, 0, sizeof(_last_run[0]) * _num_tasks); memset(_last_run, 0, sizeof(_last_run[0]) * _num_tasks);
_tick_counter = 0; _tick_counter = 0;
@ -138,15 +144,20 @@ void AP_Scheduler::run(uint32_t time_available)
if (_debug > 1 && _perf_counters == nullptr) { if (_debug > 1 && _perf_counters == nullptr) {
_perf_counters = new AP_HAL::Util::perf_counter_t[_num_tasks]; _perf_counters = new AP_HAL::Util::perf_counter_t[_num_tasks];
if (_perf_counters != nullptr) { if (_perf_counters != nullptr) {
for (uint8_t i=0; i<_num_tasks; i++) { for (uint8_t i=0; i<_num_unshared_tasks; i++) {
_perf_counters[i] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, _tasks[i].name); _perf_counters[i] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, _tasks[i].name);
} }
for (uint8_t i=_num_unshared_tasks; i<_num_tasks; i++) {
_perf_counters[i] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, _common_tasks[i].name);
}
} }
} }
for (uint8_t i=0; i<_num_tasks; i++) { 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];
uint32_t dt = _tick_counter - _last_run[i]; uint32_t dt = _tick_counter - _last_run[i];
uint32_t interval_ticks = _loop_rate_hz / _tasks[i].rate_hz; uint32_t interval_ticks = _loop_rate_hz / task.rate_hz;
if (interval_ticks < 1) { if (interval_ticks < 1) {
interval_ticks = 1; interval_ticks = 1;
} }
@ -155,13 +166,13 @@ void AP_Scheduler::run(uint32_t time_available)
continue; continue;
} }
// this task is due to run. Do we have enough time to run it? // this task is due to run. Do we have enough time to run it?
_task_time_allowed = _tasks[i].max_time_micros; _task_time_allowed = task.max_time_micros;
if (dt >= interval_ticks*2) { if (dt >= interval_ticks*2) {
// we've slipped a whole run of this task! // we've slipped a whole run of this task!
debug(2, "Scheduler slip task[%u-%s] (%u/%u/%u)\n", debug(2, "Scheduler slip task[%u-%s] (%u/%u/%u)\n",
(unsigned)i, (unsigned)i,
_tasks[i].name, task.name,
(unsigned)dt, (unsigned)dt,
(unsigned)interval_ticks, (unsigned)interval_ticks,
(unsigned)_task_time_allowed); (unsigned)_task_time_allowed);
@ -188,7 +199,7 @@ void AP_Scheduler::run(uint32_t time_available)
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
fill_nanf_stack(); fill_nanf_stack();
#endif #endif
_tasks[i].function(); task.function();
if (_debug > 1 && _perf_counters && _perf_counters[i]) { if (_debug > 1 && _perf_counters && _perf_counters[i]) {
hal.util->perf_end(_perf_counters[i]); hal.util->perf_end(_perf_counters[i]);
} }
@ -206,7 +217,7 @@ void AP_Scheduler::run(uint32_t time_available)
// the event overran! // the event overran!
debug(3, "Scheduler overrun task[%u-%s] (%u/%u)\n", debug(3, "Scheduler overrun task[%u-%s] (%u/%u)\n",
(unsigned)i, (unsigned)i,
_tasks[i].name, task.name,
(unsigned)time_taken, (unsigned)time_taken,
(unsigned)_task_time_allowed); (unsigned)_task_time_allowed);
} }

9
libraries/AP_Scheduler/AP_Scheduler.h
View File

@ -48,7 +48,6 @@
*/ */
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_Vehicle/AP_Vehicle.h>
class AP_Scheduler class AP_Scheduler
{ {
@ -172,9 +171,15 @@ private:
// progmem list of tasks to run // progmem list of tasks to run
const struct Task *_tasks; const struct Task *_tasks;
// number of tasks in _tasks list // progmem list of common tasks to run
const struct Task *_common_tasks;
// total number of tasks in _tasks and _common_tasks list
uint8_t _num_tasks; 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 // number of 'ticks' that have passed (number of times that
// tick() has been called // tick() has been called
uint16_t _tick_counter; uint16_t _tick_counter;