zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Scheduler: refactor task info into TaskInfo 

break fast_loop down into FastTasks to aid profiling
run fast tasks before scheduled tasks
fix scheduler example
introduce fast task priorities
remove fast loop
apm_2208
Andy Piper 3 years ago committed by Randy Mackay
parent
0b754aeba8
commit
2209576c90
5 changed files with 118 additions and 110 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 147
      libraries/AP_Scheduler/AP_Scheduler.cpp
  2. 28
      libraries/AP_Scheduler/AP_Scheduler.h
  3. 43
      libraries/AP_Scheduler/PerfInfo.cpp
  4. 8
      libraries/AP_Scheduler/PerfInfo.h
  5. 2
      libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp

147
libraries/AP_Scheduler/AP_Scheduler.cpp
Unescape View File

@ -71,8 +71,7 @@ const AP_Param::GroupInfo AP_Scheduler::var_info[] = {
}; };
// constructor // constructor
AP_Scheduler::AP_Scheduler(scheduler_fastloop_fn_t fastloop_fn) : AP_Scheduler::AP_Scheduler()
_fastloop_fn(fastloop_fn)
{ {
if (_singleton) { if (_singleton) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@ -213,33 +212,37 @@ void AP_Scheduler::run(uint32_t time_available)
common_tasks_offset++; common_tasks_offset++;
} }
const uint16_t dt = _tick_counter - _last_run[i]; if (task.priority > MAX_FAST_TASK_PRIORITIES) {
// we allow 0 to mean loop rate const uint16_t dt = _tick_counter - _last_run[i];
uint32_t interval_ticks = (is_zero(task.rate_hz) ? 1 : _loop_rate_hz / task.rate_hz); // we allow 0 to mean loop rate
if (interval_ticks < 1) { uint32_t interval_ticks = (is_zero(task.rate_hz) ? 1 : _loop_rate_hz / task.rate_hz);
interval_ticks = 1; if (interval_ticks < 1) {
} interval_ticks = 1;
if (dt < interval_ticks) { }
// this task is not yet scheduled to run again if (dt < interval_ticks) {
continue; // this task is not yet scheduled to run again
} continue;
// this task is due to run. Do we have enough time to run it? }
_task_time_allowed = task.max_time_micros; // this task is due to run. Do we have enough time to run it?
_task_time_allowed = task.max_time_micros;
if (dt >= interval_ticks*2) { if (dt >= interval_ticks*2) {
perf_info.task_slipped(i); perf_info.task_slipped(i);
} }
if (dt >= interval_ticks*max_task_slowdown) { if (dt >= interval_ticks*max_task_slowdown) {
// we are going beyond the maximum slowdown factor for a // we are going beyond the maximum slowdown factor for a
// task. This will trigger increasing the time budget // task. This will trigger increasing the time budget
task_not_achieved++; task_not_achieved++;
} }
if (_task_time_allowed > time_available) { if (_task_time_allowed > time_available) {
// not enough time to run this task. Continue loop - // not enough time to run this task. Continue loop -
// maybe another task will fit into time remaining // maybe another task will fit into time remaining
continue; continue;
}
} else {
_task_time_allowed = get_loop_period_us();
} }
// run it // run it
@ -331,14 +334,6 @@ void AP_Scheduler::loop()
_last_loop_time_s = (sample_time_us - _loop_timer_start_us) * 1.0e-6; _last_loop_time_s = (sample_time_us - _loop_timer_start_us) * 1.0e-6;
} }
// Execute the fast loop
// ---------------------
if (_fastloop_fn) {
hal.util->persistent_data.scheduler_task = -2;
_fastloop_fn();
hal.util->persistent_data.scheduler_task = -1;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
{ {
/* /*
@ -369,8 +364,6 @@ void AP_Scheduler::loop()
// add in extra loop time determined by not achieving scheduler tasks // add in extra loop time determined by not achieving scheduler tasks
time_available += extra_loop_us; time_available += extra_loop_us;
// update the task info for the fast loop
perf_info.update_task_info(_num_tasks, loop_tick_us, loop_tick_us > loop_us);
// run the tasks // run the tasks
run(time_available); run(time_available);
@ -477,67 +470,41 @@ void AP_Scheduler::task_info(ExpandingString &str)
uint8_t vehicle_tasks_offset = 0; uint8_t vehicle_tasks_offset = 0;
uint8_t common_tasks_offset = 0; uint8_t common_tasks_offset = 0;
for (uint8_t i = 0; i < _num_tasks + 1; i++) { for (uint8_t i = 0; i < _num_tasks; i++) {
const AP::PerfInfo::TaskInfo* ti; const AP::PerfInfo::TaskInfo* ti = perf_info.get_task_info(i);
const char *task_name; 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) { // determine which of the common task / vehicle task to run
task_name = _vehicle_tasks[vehicle_tasks_offset++].name; bool run_vehicle_task = false;
} else { if (vehicle_tasks_offset < _num_vehicle_tasks &&
task_name = _common_tasks[common_tasks_offset++].name; 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;
} }
// the loop counter i is adjusted here because we emit the } else if (vehicle_tasks_offset < _num_vehicle_tasks) {
// fast-loop entry first but it appears last in the // out of common tasks to run
// perf_info list 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;
} }
uint16_t avg = 0; if (run_vehicle_task) {
float pct = 0.0f; task_name = _vehicle_tasks[vehicle_tasks_offset++].name;
if (ti != nullptr && ti->tick_count > 0) { } else {
pct = ti->elapsed_time_us * 100.0f / total_time; task_name = _common_tasks[common_tasks_offset++].name;
avg = MIN(uint16_t(ti->elapsed_time_us / ti->tick_count), 9999);
} }
#if HAL_MINIMIZE_FEATURES ti->print(task_name, total_time, str);
const char* fmt = "%-16.16s MIN=%4u MAX=%4u AVG=%4u OVR=%3u SLP=%3u, TOT=%4.1f%%\n";
#else
const char* fmt = "%-32.32s MIN=%4u MAX=%4u AVG=%4u OVR=%3u SLP=%3u, TOT=%4.1f%%\n";
#endif
str.printf(fmt, task_name,
unsigned(MIN(ti->min_time_us, 9999)), unsigned(MIN(ti->max_time_us, 9999)), unsigned(avg),
unsigned(MIN(ti->overrun_count, 999)), unsigned(MIN(ti->slip_count, 999)), pct);
} }
} }

28
libraries/AP_Scheduler/AP_Scheduler.h
Unescape View File

@ -34,8 +34,10 @@
#if HAL_MINIMIZE_FEATURES #if HAL_MINIMIZE_FEATURES
#define AP_SCHEDULER_NAME_INITIALIZER(_clazz,_name) .name = #_name, #define AP_SCHEDULER_NAME_INITIALIZER(_clazz,_name) .name = #_name,
#define AP_FAST_NAME_INITIALIZER(_clazz,_name) .name = #_name "*",
#else #else
#define AP_SCHEDULER_NAME_INITIALIZER(_clazz,_name) .name = #_clazz "::" #_name, #define AP_SCHEDULER_NAME_INITIALIZER(_clazz,_name) .name = #_clazz "::" #_name,
#define AP_FAST_NAME_INITIALIZER(_clazz,_name) .name = #_clazz "::" #_name "*",
#endif #endif
#define LOOP_RATE 0 #define LOOP_RATE 0
@ -50,6 +52,17 @@
.priority = _priority \ .priority = _priority \
} }
/*
useful macro for creating the fastloop task table
*/
#define FAST_TASK_CLASS(classname, classptr, func) { \
.function = FUNCTOR_BIND(classptr, &classname::func, void),\
AP_FAST_NAME_INITIALIZER(classname, func)\
.rate_hz = 0,\
.max_time_micros = 0,\
.priority = AP_Scheduler::FAST_TASK_PRI0 \
}
/* /*
A task scheduler for APM main loops A task scheduler for APM main loops
@ -63,10 +76,7 @@
class AP_Scheduler class AP_Scheduler
{ {
public: public:
AP_Scheduler();
FUNCTOR_TYPEDEF(scheduler_fastloop_fn_t, void);
AP_Scheduler(scheduler_fastloop_fn_t fastloop_fn = nullptr);
/* Do not allow copies */ /* Do not allow copies */
AP_Scheduler(const AP_Scheduler &other) = delete; AP_Scheduler(const AP_Scheduler &other) = delete;
@ -89,6 +99,13 @@ public:
RECORD_TASK_INFO = 1 << 0 RECORD_TASK_INFO = 1 << 0
}; };
enum FastTaskPriorities {
FAST_TASK_PRI0 = 0,
FAST_TASK_PRI1 = 1,
FAST_TASK_PRI2 = 2,
MAX_FAST_TASK_PRIORITIES = 3
};
// initialise scheduler // initialise scheduler
void init(const Task *tasks, uint8_t num_tasks, uint32_t log_performance_bit); void init(const Task *tasks, uint8_t num_tasks, uint32_t log_performance_bit);
@ -170,9 +187,6 @@ public:
AP::PerfInfo perf_info; AP::PerfInfo perf_info;
private: private:
// function that is called before anything in the scheduler table:
scheduler_fastloop_fn_t _fastloop_fn;
// used to enable scheduler debugging // used to enable scheduler debugging
AP_Int8 _debug; AP_Int8 _debug;

43
libraries/AP_Scheduler/PerfInfo.cpp
Unescape View File

@ -23,7 +23,7 @@ void AP::PerfInfo::reset()
sigma_time = 0; sigma_time = 0;
sigmasquared_time = 0; sigmasquared_time = 0;
if (_task_info != nullptr) { if (_task_info != nullptr) {
memset(_task_info, 0, (_num_tasks + 1) * sizeof(TaskInfo)); memset(_task_info, 0, (_num_tasks) * sizeof(TaskInfo));
} }
} }
@ -36,7 +36,7 @@ void AP::PerfInfo::ignore_this_loop()
// allocate the array of task statistics for use by @SYS/tasks.txt // allocate the array of task statistics for use by @SYS/tasks.txt
void AP::PerfInfo::allocate_task_info(uint8_t num_tasks) void AP::PerfInfo::allocate_task_info(uint8_t num_tasks)
{ {
_task_info = new TaskInfo[num_tasks + 1]; // add an extra slot for the fast_loop _task_info = new TaskInfo[num_tasks];
if (_task_info == nullptr) { if (_task_info == nullptr) {
hal.console->printf("Unable to allocate scheduler TaskInfo\n"); hal.console->printf("Unable to allocate scheduler TaskInfo\n");
_num_tasks = 0; _num_tasks = 0;
@ -59,22 +59,45 @@ void AP::PerfInfo::update_task_info(uint8_t task_index, uint16_t task_time_us, b
return; return;
} }
if (task_index > _num_tasks) { if (task_index >= _num_tasks) {
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return; return;
} }
TaskInfo& ti = _task_info[task_index]; TaskInfo& ti = _task_info[task_index];
ti.max_time_us = MAX(ti.max_time_us, task_time_us); ti.update(task_time_us, overrun);
if (ti.min_time_us == 0) { }
ti.min_time_us = task_time_us;
void AP::PerfInfo::TaskInfo::update(uint16_t task_time_us, bool overrun)
{
max_time_us = MAX(max_time_us, task_time_us);
if (min_time_us == 0) {
min_time_us = task_time_us;
} else { } else {
ti.min_time_us = MIN(ti.min_time_us, task_time_us); min_time_us = MIN(min_time_us, task_time_us);
} }
ti.elapsed_time_us += task_time_us; elapsed_time_us += task_time_us;
ti.tick_count++; tick_count++;
if (overrun) { if (overrun) {
ti.overrun_count++; overrun_count++;
}
}
void AP::PerfInfo::TaskInfo::print(const char* task_name, uint32_t total_time, ExpandingString& str) const
{
uint16_t avg = 0;
float pct = 0.0f;
if (tick_count > 0) {
pct = elapsed_time_us * 100.0f / total_time;
avg = MIN(uint16_t(elapsed_time_us / tick_count), 9999);
} }
#if HAL_MINIMIZE_FEATURES
const char* fmt = "%-16.16s MIN=%4u MAX=%4u AVG=%4u OVR=%3u SLP=%3u, TOT=%4.1f%%\n";
#else
const char* fmt = "%-32.32s MIN=%4u MAX=%4u AVG=%4u OVR=%3u SLP=%3u, TOT=%4.1f%%\n";
#endif
str.printf(fmt, task_name,
unsigned(MIN(min_time_us, 9999)), unsigned(MIN(max_time_us, 9999)), unsigned(avg),
unsigned(MIN(overrun_count, 999)), unsigned(MIN(slip_count, 999)), pct);
} }
// check_loop_time - check latest loop time vs min, max and overtime threshold // check_loop_time - check latest loop time vs min, max and overtime threshold

8
libraries/AP_Scheduler/PerfInfo.h
Unescape View File

@ -1,6 +1,7 @@
#pragma once #pragma once
#include <stdint.h> #include <stdint.h>
#include <AP_Common/ExpandingString.h>
namespace AP { namespace AP {
@ -16,6 +17,9 @@ public:
uint32_t tick_count; uint32_t tick_count;
uint16_t slip_count; uint16_t slip_count;
uint16_t overrun_count; uint16_t overrun_count;
void update(uint16_t task_time_us, bool overrun);
void print(const char* task_name, uint32_t total_time, ExpandingString& str) const;
}; };
/* Do not allow copies */ /* Do not allow copies */
@ -43,13 +47,13 @@ public:
bool has_task_info() { return _task_info != nullptr; } bool has_task_info() { return _task_info != nullptr; }
// return a task info // return a task info
const TaskInfo* get_task_info(uint8_t task_index) const { const TaskInfo* get_task_info(uint8_t task_index) const {
return (_task_info && task_index <= _num_tasks) ? &_task_info[task_index] : nullptr; return (_task_info && task_index < _num_tasks) ? &_task_info[task_index] : nullptr;
} }
// called after each run of a task to update its statistics based on measurements taken by the scheduler // called after each run of a task to update its statistics based on measurements taken by the scheduler
void update_task_info(uint8_t task_index, uint16_t task_time_us, bool overrun); void update_task_info(uint8_t task_index, uint16_t task_time_us, bool overrun);
// record that a task slipped // record that a task slipped
void task_slipped(uint8_t task_index) { void task_slipped(uint8_t task_index) {
if (_task_info && task_index <= _num_tasks) { if (_task_info && task_index < _num_tasks) {
_task_info[task_index].overrun_count++; _task_info[task_index].overrun_count++;
} }
} }

2
libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp
Unescape View File

@ -25,7 +25,7 @@ private:
#if HAL_EXTERNAL_AHRS_ENABLED #if HAL_EXTERNAL_AHRS_ENABLED
AP_ExternalAHRS eAHRS; AP_ExternalAHRS eAHRS;
#endif // HAL_EXTERNAL_AHRS_ENABLED #endif // HAL_EXTERNAL_AHRS_ENABLED
AP_Scheduler scheduler{nullptr}; AP_Scheduler scheduler;
uint32_t ins_counter; uint32_t ins_counter;
static const AP_Scheduler::Task scheduler_tasks[]; static const AP_Scheduler::Task scheduler_tasks[];

Write Preview
Loading…
Cancel
Save