diff --git a/libraries/AP_Scheduler/AP_Scheduler.cpp b/libraries/AP_Scheduler/AP_Scheduler.cpp index 46793c08a6..4ba015190a 100644 --- a/libraries/AP_Scheduler/AP_Scheduler.cpp +++ b/libraries/AP_Scheduler/AP_Scheduler.cpp @@ -57,6 +57,13 @@ const AP_Param::GroupInfo AP_Scheduler::var_info[] = { // @User: Advanced AP_GROUPINFO("LOOP_RATE", 1, AP_Scheduler, _loop_rate_hz, SCHEDULER_DEFAULT_LOOP_RATE), + // @Param: OPTIONS + // @DisplayName: Scheduling options + // @Description: This controls optional aspects of the scheduler. + // @Bitmask: 0:Enable per-task perf info + // @User: Advanced + AP_GROUPINFO("OPTIONS", 2, AP_Scheduler, _options, 0), + AP_GROUPEND }; @@ -114,6 +121,10 @@ void AP_Scheduler::init(const AP_Scheduler::Task *tasks, uint8_t num_tasks, uint perf_info.set_loop_rate(get_loop_rate_hz()); perf_info.reset(); + if (_options & uint8_t(Options::RECORD_TASK_INFO)) { + perf_info.allocate_task_info(_num_tasks); + } + _log_performance_bit = log_performance_bit; } @@ -173,13 +184,7 @@ void AP_Scheduler::run(uint32_t time_available) _task_time_allowed = task.max_time_micros; if (dt >= interval_ticks*2) { - // we've slipped a whole run of this task! - debug(2, "Scheduler slip task[%u-%s] (%u/%u/%u)\n", - (unsigned)i, - task.name, - (unsigned)dt, - (unsigned)interval_ticks, - (unsigned)_task_time_allowed); + perf_info.task_slipped(i); } if (dt >= interval_ticks*max_task_slowdown) { @@ -216,8 +221,9 @@ void AP_Scheduler::run(uint32_t time_available) // work out how long the event actually took now = AP_HAL::micros(); uint32_t time_taken = now - _task_time_started; - + bool overrun = false; if (time_taken > _task_time_allowed) { + overrun = true; // the event overran! debug(3, "Scheduler overrun task[%u-%s] (%u/%u)\n", (unsigned)i, @@ -225,6 +231,9 @@ void AP_Scheduler::run(uint32_t time_available) (unsigned)time_taken, (unsigned)_task_time_allowed); } + + perf_info.update_task_info(i, time_taken, overrun); + if (time_taken >= time_available) { time_available = 0; break; @@ -365,6 +374,12 @@ void AP_Scheduler::update_logging() } perf_info.set_loop_rate(get_loop_rate_hz()); perf_info.reset(); + // dynamically update the per-task perf counter + if (!(_options & uint8_t(Options::RECORD_TASK_INFO)) && perf_info.has_task_info()) { + perf_info.free_task_info(); + } else if ((_options & uint8_t(Options::RECORD_TASK_INFO)) && !perf_info.has_task_info()) { + perf_info.allocate_task_info(_num_tasks); + } } // Write a performance monitoring packet @@ -389,6 +404,61 @@ void AP_Scheduler::Log_Write_Performance() AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt)); } +// display task statistics as text buffer for @SYS/tasks.txt +size_t AP_Scheduler::task_info(char *buf, size_t bufsize) +{ + size_t total = 0; + + // a header to allow for machine parsers to determine format + int n = hal.util->snprintf(buf, bufsize, "TasksV1\n"); + + if (n <= 0) { + return 0; + } + + // dynamically enable statistics collection + if (!(_options & uint8_t(Options::RECORD_TASK_INFO))) { + _options |= uint8_t(Options::RECORD_TASK_INFO); + return n; + } + + if (perf_info.get_task_info(0) == nullptr) { + return n; + } + + buf += n; + bufsize -= n; + total += n; + + 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]; + const AP::PerfInfo::TaskInfo* ti = perf_info.get_task_info(i); + + uint16_t avg = 0; + if (ti->tick_count > 0) { + avg = MIN(uint16_t(ti->elapsed_time_us / ti->tick_count), 999); + } + +#if HAL_MINIMIZE_FEATURES + const char* fmt = "%-16.16s MIN=%3u MAX=%3u AVG=%3u OVR=%3u SLP=%3u\n"; +#else + const char* fmt = "%-32.32s MIN=%3u MAX=%3u AVG=%3u OVR=%3u SLP=%3u\n"; +#endif + n = hal.util->snprintf(buf, bufsize, fmt, task.name, + unsigned(MIN(ti->min_time_us, 999)), unsigned(MIN(ti->max_time_us, 999)), unsigned(avg), + unsigned(MIN(ti->overrun_count, 999)), unsigned(MIN(ti->slip_count, 999))); + + if (n <= 0) { + break; + } + buf += n; + bufsize -= n; + total += n; + } + + return total; +} + namespace AP { AP_Scheduler &scheduler() diff --git a/libraries/AP_Scheduler/AP_Scheduler.h b/libraries/AP_Scheduler/AP_Scheduler.h index 2ffe125c91..f86c868267 100644 --- a/libraries/AP_Scheduler/AP_Scheduler.h +++ b/libraries/AP_Scheduler/AP_Scheduler.h @@ -22,10 +22,15 @@ #include #include +#include #include #include "PerfInfo.h" // loop perf monitoring -#define AP_SCHEDULER_NAME_INITIALIZER(_name) .name = #_name, +#if HAL_MINIMIZE_FEATURES +#define AP_SCHEDULER_NAME_INITIALIZER(_clazz,_name) .name = #_name, +#else +#define AP_SCHEDULER_NAME_INITIALIZER(_clazz,_name) .name = #_clazz "::" #_name, +#endif #define LOOP_RATE 0 /* @@ -33,7 +38,7 @@ */ #define SCHED_TASK_CLASS(classname, classptr, func, _rate_hz, _max_time_micros) { \ .function = FUNCTOR_BIND(classptr, &classname::func, void),\ - AP_SCHEDULER_NAME_INITIALIZER(func)\ + AP_SCHEDULER_NAME_INITIALIZER(classname, func)\ .rate_hz = _rate_hz,\ .max_time_micros = _max_time_micros\ } @@ -74,6 +79,10 @@ public: uint16_t max_time_micros; }; + enum class Options : uint8_t { + RECORD_TASK_INFO = 1 << 0 + }; + // initialise scheduler void init(const Task *tasks, uint8_t num_tasks, uint32_t log_performance_bit); @@ -147,6 +156,8 @@ public: HAL_Semaphore &get_semaphore(void) { return _rsem; } + size_t task_info(char *buf, size_t bufsize); + static const struct AP_Param::GroupInfo var_info[]; // loop performance monitoring: @@ -164,6 +175,9 @@ private: // loop rate in Hz as set at startup AP_Int16 _active_loop_rate_hz; + + // scheduler options + AP_Int8 _options; // calculated loop period in usec uint16_t _loop_period_us; diff --git a/libraries/AP_Scheduler/PerfInfo.cpp b/libraries/AP_Scheduler/PerfInfo.cpp index 94b5de98d0..a265d2f4c4 100644 --- a/libraries/AP_Scheduler/PerfInfo.cpp +++ b/libraries/AP_Scheduler/PerfInfo.cpp @@ -2,6 +2,7 @@ #include #include +#include #include "AP_Scheduler.h" extern const AP_HAL::HAL& hal; @@ -29,6 +30,50 @@ void AP::PerfInfo::ignore_this_loop() ignore_loop = true; } +// 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]; + if (_task_info == nullptr) { + hal.console->printf("Unable to allocate scheduler TaskInfo\n"); + _num_tasks = 0; + return; + } + _num_tasks = num_tasks; +} + +void AP::PerfInfo::free_task_info() +{ + delete[] _task_info; + _task_info = nullptr; + _num_tasks = 0; +} + +// called after each run of a task to update its statistics based on measurements taken by the scheduler +void AP::PerfInfo::update_task_info(uint8_t task_index, uint16_t task_time_us, bool overrun) +{ + if (_task_info == nullptr) { + return; + } + + 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; + } else { + ti.min_time_us = MIN(ti.min_time_us, task_time_us); + } + ti.elapsed_time_us += task_time_us; + ti.tick_count++; + if (overrun) { + ti.overrun_count++; + } +} + // check_loop_time - check latest loop time vs min, max and overtime threshold void AP::PerfInfo::check_loop_time(uint32_t time_in_micros) { diff --git a/libraries/AP_Scheduler/PerfInfo.h b/libraries/AP_Scheduler/PerfInfo.h index d88214da55..f766bac84a 100644 --- a/libraries/AP_Scheduler/PerfInfo.h +++ b/libraries/AP_Scheduler/PerfInfo.h @@ -8,6 +8,16 @@ class PerfInfo { public: PerfInfo() {} + // per-task timing information + struct TaskInfo { + uint16_t min_time_us; + uint16_t max_time_us; + uint32_t elapsed_time_us; + uint32_t tick_count; + uint16_t slip_count; + uint16_t overrun_count; + }; + /* Do not allow copies */ PerfInfo(const PerfInfo &other) = delete; PerfInfo &operator=(const PerfInfo&) = delete; @@ -26,6 +36,24 @@ public: void update_logging(); + // allocate the array of task statistics for use by @SYS/tasks.txt + void allocate_task_info(uint8_t num_tasks); + void free_task_info(); + // whether or not we have task info allocated + 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; + } + // 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) { + _task_info[task_index].overrun_count++; + } + } + private: uint16_t loop_rate_hz; uint16_t overtime_threshold_micros; @@ -38,7 +66,9 @@ private: uint32_t last_check_us; float filtered_loop_time; bool ignore_loop; - + // performance monitoring + uint8_t _num_tasks; + TaskInfo* _task_info; }; };