AP_Scheduler: add per-task performance information accesible from @SYS/tasks.txt

print fully qualified name on most boards
remove old task slip message
Don't die on allocation failure.
dynamically allocation/free task info based on SCHED_OPTIONS
dynamically enable task info on ftp get

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!
+            perf_info.task_slipped(i);
-            debug(2, "Scheduler slip task[%u-%s] (%u/%u/%u)\n",
-                  (unsigned)i,
-                  task.name,
-                  (unsigned)dt,
-                  (unsigned)interval_ticks,
-                  (unsigned)_task_time_allowed);
         }
 
         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()

libraries/AP_Scheduler/AP_Scheduler.h

@@ -22,10 +22,15 @@
 
 #include <AP_Param/AP_Param.h>
 #include <AP_HAL/Util.h>
+#include <AP_HAL/AP_HAL.h>
 #include <AP_Math/AP_Math.h>
 #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;

libraries/AP_Scheduler/PerfInfo.cpp

@@ -2,6 +2,7 @@
 
 #include <AP_Logger/AP_Logger.h>
 #include <GCS_MAVLink/GCS.h>
+#include <AP_InternalError/AP_InternalError.h>
 #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)
 {

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;
 };
 
 };