AP_Scheduler: implement loop()

libraries/AP_Scheduler/AP_Scheduler.cpp

@@ -23,6 +23,8 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Param/AP_Param.h>
 #include <AP_Vehicle/AP_Vehicle.h>
+#include <DataFlash/DataFlash.h>
+#include <AP_InertialSensor/AP_InertialSensor.h>
 #include <stdio.h>
 
 #if APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_ArduSub)
@@ -57,7 +59,8 @@ const AP_Param::GroupInfo AP_Scheduler::var_info[] = {
 };
 
 // constructor
-AP_Scheduler::AP_Scheduler(void)
+AP_Scheduler::AP_Scheduler(scheduler_fastloop_fn_t fastloop_fn) :
+    _fastloop_fn(fastloop_fn)
 {
     AP_Param::setup_object_defaults(this, var_info);
 
@@ -77,6 +80,11 @@ void AP_Scheduler::init(const AP_Scheduler::Task *tasks, uint8_t num_tasks)
     _last_run = new uint16_t[_num_tasks];
     memset(_last_run, 0, sizeof(_last_run[0]) * _num_tasks);
     _tick_counter = 0;
+
+    // setup initial performance counters
+    perf_info.set_loop_rate(get_loop_rate_hz());
+    perf_info.reset();
+    loop_start = AP_HAL::micros();
 }
 
 // one tick has passed
@@ -195,3 +203,36 @@ float AP_Scheduler::load_average()
     const uint32_t used_time = loop_us - (_spare_micros/_spare_ticks);
     return used_time / (float)loop_us;
 }
+
+void AP_Scheduler::loop()
+{
+    // wait for an INS sample
+    AP::ins().wait_for_sample();
+
+    const uint32_t timer = AP_HAL::micros();
+
+    // check loop time
+    perf_info.check_loop_time(timer - loop_start);
+
+    // used by PI Loops
+    last_loop_time          = (float)(timer - loop_start) / 1000000.0f;
+    loop_start              = timer;
+
+    // Execute the fast loop
+    // ---------------------
+    if (_fastloop_fn) {
+        _fastloop_fn();
+    }
+
+    // tell the scheduler one tick has passed
+    tick();
+
+    // run all the tasks that are due to run. Note that we only
+    // have to call this once per loop, as the tasks are scheduled
+    // in multiples of the main loop tick. So if they don't run on
+    // the first call to the scheduler they won't run on a later
+    // call until scheduler.tick() is called again
+    const uint32_t loop_us = get_loop_period_us();
+    const uint32_t time_available = (timer + loop_us) - AP_HAL::micros();
+    run(time_available > loop_us ? 0u : time_available);
+}

libraries/AP_Scheduler/AP_Scheduler.h

@@ -23,6 +23,7 @@
 #include <AP_Param/AP_Param.h>
 #include <AP_HAL/Util.h>
 #include <AP_Math/AP_Math.h>
+#include "PerfInfo.h"       // loop perf monitoring
 
 #define AP_SCHEDULER_NAME_INITIALIZER(_name) .name = #_name,
 
@@ -52,7 +53,10 @@
 class AP_Scheduler
 {
 public:
-    AP_Scheduler();
+
+    FUNCTOR_TYPEDEF(scheduler_fastloop_fn_t, void);
+
+    AP_Scheduler(scheduler_fastloop_fn_t fastloop_fn = nullptr);
 
     /* Do not allow copies */
     AP_Scheduler(const AP_Scheduler &other) = delete;
@@ -70,6 +74,10 @@ public:
     // initialise scheduler
     void init(const Task *tasks, uint8_t num_tasks);
 
+    // called by vehicle's main loop - which should be the only thing
+    // that function does
+    void loop();
+
     // call when one tick has passed
     void tick(void);
 
@@ -119,7 +127,16 @@ public:
     // current running task, or -1 if none. Used to debug stuck tasks
     static int8_t current_task;
 
+    // loop performance monitoring:
+    AP::PerfInfo perf_info;
+
+    // time taken for previous loop (in seconds):
+    float last_loop_time;
+
 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;
 
@@ -160,6 +177,9 @@ private:
     // number of ticks that _spare_micros is counted over
     uint8_t _spare_ticks;
 
+    // timestamp of loop start:
+    uint32_t loop_start;
+
     // performance counters
     AP_HAL::Util::perf_counter_t *_perf_counters;
 };

libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp

@@ -6,9 +6,13 @@
 #include <AP_InertialSensor/AP_InertialSensor.h>
 #include <AP_Scheduler/AP_Scheduler.h>
 #include <AP_BoardConfig/AP_BoardConfig.h>
+#include <DataFlash/DataFlash.h>
 
 const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 
+AP_Int32 log_bitmask;
+DataFlash_Class DataFlash{"Bob", log_bitmask};
+
 class SchedTest {
 public:
     void setup();
@@ -17,7 +21,7 @@ public:
 private:
 
     AP_InertialSensor ins;
-    AP_Scheduler scheduler;
+    AP_Scheduler scheduler{nullptr};
 
     uint32_t ins_counter;
     static const AP_Scheduler::Task scheduler_tasks[];
@@ -52,19 +56,13 @@ void SchedTest::setup(void)
     ins.init(scheduler.get_loop_rate_hz());
 
     // initialise the scheduler
-    scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));
+    scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks), (uint32_t)-1);
 }
 
 void SchedTest::loop(void)
 {
-    // wait for an INS sample
+    // run all tasks
-    ins.wait_for_sample();
+    scheduler.loop();
-
-    // tell the scheduler one tick has passed
-    scheduler.tick();
-
-    // run all tasks that fit in 20ms
-    scheduler.run(20000);
 }
 
 /*