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

libraries/AP_Scheduler/AP_Scheduler.cpp

@@ -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
-                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;
+        // 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;
             }
-            // 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);
     }
 }
 

libraries/AP_Scheduler/AP_Scheduler.h

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

libraries/AP_Scheduler/PerfInfo.cpp

@@ -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,22 +59,45 @@ 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

libraries/AP_Scheduler/PerfInfo.h

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

libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp

@@ -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[];