From 7ddadcf34e50a139a71c634a0c5887eb8e3929da Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 12 Jan 2013 11:59:20 +1100 Subject: [PATCH] AP_Scheduler: added new scheduler library this will be used for main loop control --- libraries/AP_Scheduler/AP_Scheduler.cpp | 95 +++++++++++++++++++++++++ libraries/AP_Scheduler/AP_Scheduler.h | 79 ++++++++++++++++++++ 2 files changed, 174 insertions(+) create mode 100644 libraries/AP_Scheduler/AP_Scheduler.cpp create mode 100644 libraries/AP_Scheduler/AP_Scheduler.h diff --git a/libraries/AP_Scheduler/AP_Scheduler.cpp b/libraries/AP_Scheduler/AP_Scheduler.cpp new file mode 100644 index 0000000000..791fc8369b --- /dev/null +++ b/libraries/AP_Scheduler/AP_Scheduler.cpp @@ -0,0 +1,95 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/* + * main loop scheduler for APM + * Author: Andrew Tridgell, January 2013 + * + * This firmware is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + */ + +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +const AP_Param::GroupInfo AP_Scheduler::var_info[] PROGMEM = { + // @Param: DEBUG + // @DisplayName: Scheduler debug level + // @Description: Set to non-zero to enable scheduler debug messages + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + AP_GROUPINFO("DEBUG", 0, AP_Scheduler, _debug, 0), + AP_GROUPEND +}; + +// initialise the scheduler +void AP_Scheduler::init(const AP_Scheduler::Task *tasks, uint8_t num_tasks) +{ + _tasks = tasks; + _num_tasks = num_tasks; + _last_run = new uint16_t[_num_tasks]; + memset(_last_run, 0, sizeof(_last_run[0]*_num_tasks)); +} + +// one tick has passed +void AP_Scheduler::tick(void) +{ + _tick_counter++; +} + +/* + run one tick + this will run as many scheduler tasks as we can in the specified time + */ +void AP_Scheduler::run(uint16_t time_available) +{ + for (uint8_t i=0; i<_num_tasks; i++) { + uint16_t dt = _tick_counter - _last_run[i]; + if (dt >= pgm_read_word(&_tasks[i].interval_ticks)) { + // this task is due to run. Do we have enough time to run it? + _task_time_allowed = pgm_read_word(&_tasks[i].max_time_micros); + if (_task_time_allowed <= time_available) { + // run it + _task_time_started = hal.scheduler->micros(); + task_fn_t func = (task_fn_t)pgm_read_pointer(&_tasks[i].function); + func(); + + // record the tick counter when we ran. This drives + // when we next run the event + _last_run[i] = _tick_counter; + + // work out how long the event actually took + uint32_t time_taken = hal.scheduler->micros() - _task_time_started; + + if (time_taken > _task_time_allowed) { + // the event overran! + if (_debug != 0) { + hal.console->printf_P(PSTR("Scheduler overrun task[%u] (%u/%u)\n"), + (unsigned)i, + (unsigned)time_taken, + (unsigned)_task_time_allowed); + } + return; + } + time_available -= time_taken; + } + } + } +} + +/* + return number of micros until the current task reaches its deadline + */ +uint16_t AP_Scheduler::time_available_usec(void) +{ + uint32_t dt = hal.scheduler->micros() - _task_time_started; + if (dt > _task_time_allowed) { + return 0; + } + return _task_time_allowed - dt; +} + diff --git a/libraries/AP_Scheduler/AP_Scheduler.h b/libraries/AP_Scheduler/AP_Scheduler.h new file mode 100644 index 0000000000..a5700e67a7 --- /dev/null +++ b/libraries/AP_Scheduler/AP_Scheduler.h @@ -0,0 +1,79 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/* + * main loop scheduler for APM + * Author: Andrew Tridgell, January 2013 + * + * This firmware is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + */ + +#ifndef AP_SCHEDULER_H +#define AP_SCHEDULER_H + +#include + +/* + A task scheduler for APM main loops + + Sketches should call scheduler.init() on startup, then call + scheduler.tick() at regular intervals (typically every 10ms). + + To run tasks use scheduler.run(), passing the amount of time that + the scheduler is allowed to use before it must return + */ + +class AP_Scheduler +{ +public: + typedef void (*task_fn_t)(void); + + struct Task { + task_fn_t function; + uint16_t interval_ticks; + uint16_t max_time_micros; + }; + + // initialise scheduler + void init(const Task *tasks, uint8_t num_tasks); + + // call when one tick has passed + void tick(void); + + // run the tasks. Call this once per 'tick'. + // time_available is the amount of time available to run + // tasks in microseconds + void run(uint16_t time_available); + + // return the number of microseconds available for the current task + uint16_t time_available_usec(void); + + static const struct AP_Param::GroupInfo var_info[]; + +private: + // used to enable scheduler debugging + AP_Int8 _debug; + + // progmem list of tasks to run + const struct Task *_tasks; + + // number of tasks in _tasks list + uint8_t _num_tasks; + + // number of 'ticks' that have passed (number of times that + // tick() has been called + uint16_t _tick_counter; + + // tick counter at the time we last ran each task + uint16_t *_last_run; + + // number of microseconds allowed for the current task + uint16_t _task_time_allowed; + + // the time in microseconds when the task started + uint32_t _task_time_started; +}; + +#endif // AP_SCHEDULER_H