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

AP_Scheduler: refactor task info into TaskInfo

Browse Source 
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
This commit is contained in:
Andy Piper 2022-04-01 20:57:03 +01:00 committed by Randy Mackay
parent 0b754aeba8
commit 2209576c90
5 changed files with 117 additions and 109 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

145
libraries/AP_Scheduler/AP_Scheduler.cpp
View File

@ -71,8 +71,7 @@ const AP_Param::GroupInfo AP_Scheduler::var_info[] = {
};
// constructor
AP_Scheduler::AP_Scheduler(scheduler_fastloop_fn_t fastloop_fn) :
_fastloop_fn(fastloop_fn)
AP_Scheduler::AP_Scheduler()
{
if (_singleton) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@ -213,33 +212,37 @@ void AP_Scheduler::run(uint32_t time_available)
common_tasks_offset++;
}
const uint16_t dt = _tick_counter - _last_run[i];
// we allow 0 to mean loop rate
uint32_t interval_ticks = (is_zero(task.rate_hz) ? 1 : _loop_rate_hz / task.rate_hz);
if (interval_ticks < 1) {
interval_ticks = 1;
}
if (dt < interval_ticks) {
// 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;
if (task.priority > MAX_FAST_TASK_PRIORITIES) {
const uint16_t dt = _tick_counter - _last_run[i];
// we allow 0 to mean loop rate
uint32_t interval_ticks = (is_zero(task.rate_hz) ? 1 : _loop_rate_hz / task.rate_hz);
if (interval_ticks < 1) {
interval_ticks = 1;
}
if (dt < interval_ticks) {
// 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;
if (dt >= interval_ticks*2) {
perf_info.task_slipped(i);
}
if (dt >= interval_ticks*2) {
perf_info.task_slipped(i);
}
if (dt >= interval_ticks*max_task_slowdown) {
// we are going beyond the maximum slowdown factor for a
// task. This will trigger increasing the time budget
task_not_achieved++;
}
if (dt >= interval_ticks*max_task_slowdown) {
// we are going beyond the maximum slowdown factor for a
// task. This will trigger increasing the time budget
task_not_achieved++;
}
if (_task_time_allowed > time_available) {
// not enough time to run this task. Continue loop -
// maybe another task will fit into time remaining
continue;
if (_task_time_allowed > time_available) {
// not enough time to run this task. Continue loop -
// maybe another task will fit into time remaining
continue;
}
} else {
_task_time_allowed = get_loop_period_us();
}
// run it
@ -331,14 +334,6 @@ void AP_Scheduler::loop()
_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
{
/*
@ -369,8 +364,6 @@ void AP_Scheduler::loop()
// add in extra loop time determined by not achieving scheduler tasks
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(time_available);
@ -477,67 +470,41 @@ 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 AP::PerfInfo::TaskInfo* ti;
for (uint8_t i = 0; i < _num_tasks; i++) {
const AP::PerfInfo::TaskInfo* ti = perf_info.get_task_info(i);
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
// 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 (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
} 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;
}
uint16_t avg = 0;
float pct = 0.0f;
if (ti != nullptr && ti->tick_count > 0) {
pct = ti->elapsed_time_us * 100.0f / total_time;
avg = MIN(uint16_t(ti->elapsed_time_us / ti->tick_count), 9999);
if (run_vehicle_task) {
task_name = _vehicle_tasks[vehicle_tasks_offset++].name;
} else {
task_name = _common_tasks[common_tasks_offset++].name;
}
#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(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);
ti->print(task_name, total_time, str);
}
}

28
libraries/AP_Scheduler/AP_Scheduler.h
View File

@ -34,8 +34,10 @@
#if HAL_MINIMIZE_FEATURES
#define AP_SCHEDULER_NAME_INITIALIZER(_clazz,_name) .name = #_name,
#define AP_FAST_NAME_INITIALIZER(_clazz,_name) .name = #_name "*",
#else
#define AP_SCHEDULER_NAME_INITIALIZER(_clazz,_name) .name = #_clazz "::" #_name,
#define AP_FAST_NAME_INITIALIZER(_clazz,_name) .name = #_clazz "::" #_name "*",
#endif
#define LOOP_RATE 0
@ -50,6 +52,17 @@
.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
@ -63,10 +76,7 @@
class AP_Scheduler
{
public:
FUNCTOR_TYPEDEF(scheduler_fastloop_fn_t, void);
AP_Scheduler(scheduler_fastloop_fn_t fastloop_fn = nullptr);
AP_Scheduler();
/* Do not allow copies */
AP_Scheduler(const AP_Scheduler &other) = delete;
@ -89,6 +99,13 @@ public:
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
void init(const Task *tasks, uint8_t num_tasks, uint32_t log_performance_bit);
@ -170,9 +187,6 @@ public:
AP::PerfInfo perf_info;
private:
// function that is called before anything in the scheduler table:
scheduler_fastloop_fn_t _fastloop_fn;
// used to enable scheduler debugging
AP_Int8 _debug;

43
libraries/AP_Scheduler/PerfInfo.cpp
View File

@ -23,7 +23,7 @@ void AP::PerfInfo::reset()
sigma_time = 0;
sigmasquared_time = 0;
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
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) {
hal.console->printf("Unable to allocate scheduler TaskInfo\n");
_num_tasks = 0;
@ -59,24 +59,47 @@ void AP::PerfInfo::update_task_info(uint8_t task_index, uint16_t task_time_us, b
return;
}
if (task_index > _num_tasks) {
if (task_index >= _num_tasks) {
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return;
}
TaskInfo& ti = _task_info[task_index];
ti.max_time_us = MAX(ti.max_time_us, task_time_us);
if (ti.min_time_us == 0) {
ti.min_time_us = task_time_us;
ti.update(task_time_us, overrun);
}
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 {
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;
ti.tick_count++;
elapsed_time_us += task_time_us;
tick_count++;
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
void AP::PerfInfo::check_loop_time(uint32_t time_in_micros)
{

8
libraries/AP_Scheduler/PerfInfo.h
View File

@ -1,6 +1,7 @@
#pragma once
#include <stdint.h>
#include <AP_Common/ExpandingString.h>
namespace AP {
@ -16,6 +17,9 @@ public:
uint32_t tick_count;
uint16_t slip_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 */
@ -43,13 +47,13 @@ public:
bool has_task_info() { return _task_info != nullptr; }
// return a task info
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
void update_task_info(uint8_t task_index, uint16_t task_time_us, bool overrun);
// record that a task slipped
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++;
}
}

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

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