From 1b1be7e4a2ae87e3c4bb11ced05bcba2469a49db Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 1 Dec 2015 06:59:07 +1100 Subject: [PATCH] AP_Scheduler: switch to Hz based scheduler config --- libraries/AP_Scheduler/AP_Scheduler.cpp | 44 ++++++++++++++++++- libraries/AP_Scheduler/AP_Scheduler.h | 23 +++++++++- .../Scheduler_test/Scheduler_test.cpp | 16 +++---- 3 files changed, 70 insertions(+), 13 deletions(-) diff --git a/libraries/AP_Scheduler/AP_Scheduler.cpp b/libraries/AP_Scheduler/AP_Scheduler.cpp index 0e17af59e3..a42c6e45b2 100644 --- a/libraries/AP_Scheduler/AP_Scheduler.cpp +++ b/libraries/AP_Scheduler/AP_Scheduler.cpp @@ -24,6 +24,15 @@ #include #include #include +#include + +#if APM_BUILD_TYPE(APM_BUILD_ArduCopter) +#define SCHEDULER_DEFAULT_LOOP_RATE 400 +#define SCHEDULER_EXPOSE_LOOP_RATE_PARAMETER 0 +#else +#define SCHEDULER_DEFAULT_LOOP_RATE 50 +#define SCHEDULER_EXPOSE_LOOP_RATE_PARAMETER 1 +#endif extern const AP_HAL::HAL& hal; @@ -36,9 +45,39 @@ const AP_Param::GroupInfo AP_Scheduler::var_info[] = { // @Values: 0:Disabled,2:ShowSlips,3:ShowOverruns // @User: Advanced AP_GROUPINFO("DEBUG", 0, AP_Scheduler, _debug, 0), + +#if SCHEDULER_EXPOSE_LOOP_RATE_PARAMETER + // @Param: LOOP_RATE + // @DisplayName: Scheduling main loop rate + // @Description: This controls the rate of the main control loop in Hz. This should only be changed by developers. This only takes effect on restart + // @Values: 50:50Hz,100:100Hz,200:200Hz,400:400Hz + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("LOOP_RATE", 1, AP_Scheduler, _loop_rate_hz, SCHEDULER_DEFAULT_LOOP_RATE), +#endif + AP_GROUPEND }; +// constructor +AP_Scheduler::AP_Scheduler(void) +{ + _loop_rate_hz.set_default(SCHEDULER_DEFAULT_LOOP_RATE); + AP_Param::setup_object_defaults(this, var_info); + + // only allow specific values of main loop rate. This code is not + // setup for arbitrary rates yet + if (_loop_rate_hz <= 50) { + _loop_rate_hz.set(50); + } else if (_loop_rate_hz <= 100) { + _loop_rate_hz.set(100); + } else if (_loop_rate_hz <= 200) { + _loop_rate_hz.set(200); + } else { + _loop_rate_hz.set(400); + } +} + // initialise the scheduler void AP_Scheduler::init(const AP_Scheduler::Task *tasks, uint8_t num_tasks) { @@ -66,7 +105,10 @@ 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]; - uint16_t interval_ticks = pgm_read_word(&_tasks[i].interval_ticks); + uint16_t interval_ticks = _loop_rate_hz / _tasks[i].rate_hz; + if (interval_ticks < 1) { + interval_ticks = 1; + } if (dt >= 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); diff --git a/libraries/AP_Scheduler/AP_Scheduler.h b/libraries/AP_Scheduler/AP_Scheduler.h index 912398ab88..8383d2c7a9 100644 --- a/libraries/AP_Scheduler/AP_Scheduler.h +++ b/libraries/AP_Scheduler/AP_Scheduler.h @@ -27,6 +27,16 @@ #define AP_SCHEDULER_NAME_INITIALIZER(_name) .name = #_name, +/* + useful macro for creating scheduler task table + */ +#define SCHED_TASK_CLASS(classname, classptr, func, _rate_hz, _max_time_micros) { \ + .function = FUNCTOR_BIND(classptr, &classname::func, void),\ + AP_SCHEDULER_NAME_INITIALIZER(func)\ + .rate_hz = _rate_hz,\ + .max_time_micros = _max_time_micros\ +} + /* A task scheduler for APM main loops @@ -43,12 +53,15 @@ class AP_Scheduler { public: + // constructor + AP_Scheduler(void); + FUNCTOR_TYPEDEF(task_fn_t, void); struct Task { task_fn_t function; const char *name; - uint16_t interval_ticks; + float rate_hz; uint16_t max_time_micros; }; @@ -74,6 +87,11 @@ public: // end of a run() float load_average(uint32_t tick_time_usec) const; + // get the configured main loop rate + uint16_t get_loop_rate_hz(void) const { + return _loop_rate_hz; + } + static const struct AP_Param::GroupInfo var_info[]; // current running task, or -1 if none. Used to debug stuck tasks @@ -83,6 +101,9 @@ private: // used to enable scheduler debugging AP_Int8 _debug; + // overall scheduling rate in Hz + AP_Int16 _loop_rate_hz; + // progmem list of tasks to run const struct Task *_tasks; diff --git a/libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp b/libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp index aea128d524..bc1399a25f 100644 --- a/libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp +++ b/libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp @@ -30,12 +30,7 @@ private: static SchedTest schedtest; -#define SCHED_TASK(func, _interval_ticks, _max_time_micros) {\ - .function = FUNCTOR_BIND(&schedtest, &SchedTest::func, void),\ - AP_SCHEDULER_NAME_INITIALIZER(func)\ - .interval_ticks = _interval_ticks,\ - .max_time_micros = _max_time_micros,\ -} +#define SCHED_TASK(func, _interval_ticks, _max_time_micros) SCHED_TASK_CLASS(SchedTest, &schedtest, func, _interval_ticks, _max_time_micros) /* scheduler table - all regular tasks are listed here, along with how @@ -43,16 +38,15 @@ static SchedTest schedtest; they are expected to take (in microseconds) */ const AP_Scheduler::Task SchedTest::scheduler_tasks[] = { - SCHED_TASK(ins_update, 1, 1000), - SCHED_TASK(one_hz_print, 50, 1000), - SCHED_TASK(five_second_call, 250, 1800), + SCHED_TASK(ins_update, 50, 1000), + SCHED_TASK(one_hz_print, 1, 1000), + SCHED_TASK(five_second_call, 0.2, 1800), }; void SchedTest::setup(void) { - // we - ins.init(AP_InertialSensor::RATE_50HZ); + ins.init(scheduler.get_loop_rate_hz()); // initialise the scheduler scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));