AP_Scheduler: switch to Hz based scheduler config

libraries/AP_Scheduler/AP_Scheduler.cpp

@@ -24,6 +24,15 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Param/AP_Param.h>
 #include <AP_Progmem/AP_Progmem.h>
+#include <AP_Vehicle/AP_Vehicle.h>
+
+#if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
+#define SCHEDULER_DEFAULT_LOOP_RATE 400
+#define SCHEDULER_EXPOSE_LOOP_RATE_PARAMETER 0
+#else
+#define SCHEDULER_DEFAULT_LOOP_RATE  50
+#define SCHEDULER_EXPOSE_LOOP_RATE_PARAMETER 1
+#endif
 
 extern const AP_HAL::HAL& hal;
 
@@ -36,9 +45,39 @@ const AP_Param::GroupInfo AP_Scheduler::var_info[] = {
     // @Values: 0:Disabled,2:ShowSlips,3:ShowOverruns
     // @User: Advanced
     AP_GROUPINFO("DEBUG",    0, AP_Scheduler, _debug, 0),
+
+#if SCHEDULER_EXPOSE_LOOP_RATE_PARAMETER
+    // @Param: LOOP_RATE
+    // @DisplayName: Scheduling main loop rate
+    // @Description: This controls the rate of the main control loop in Hz. This should only be changed by developers. This only takes effect on restart
+    // @Values: 50:50Hz,100:100Hz,200:200Hz,400:400Hz
+    // @RebootRequired: True
+    // @User: Advanced
+    AP_GROUPINFO("LOOP_RATE",  1, AP_Scheduler, _loop_rate_hz, SCHEDULER_DEFAULT_LOOP_RATE),
+#endif
+
     AP_GROUPEND
 };
 
+// constructor
+AP_Scheduler::AP_Scheduler(void)
+{
+    _loop_rate_hz.set_default(SCHEDULER_DEFAULT_LOOP_RATE);
+    AP_Param::setup_object_defaults(this, var_info);
+
+    // only allow specific values of main loop rate. This code is not
+    // setup for arbitrary rates yet
+    if (_loop_rate_hz <= 50) {
+        _loop_rate_hz.set(50);
+    } else if (_loop_rate_hz <= 100) {
+        _loop_rate_hz.set(100);
+    } else if (_loop_rate_hz <= 200) {
+        _loop_rate_hz.set(200);
+    } else {
+        _loop_rate_hz.set(400);
+    }
+}
+
 // initialise the scheduler
 void AP_Scheduler::init(const AP_Scheduler::Task *tasks, uint8_t num_tasks) 
 {
@@ -66,7 +105,10 @@ void AP_Scheduler::run(uint16_t time_available)
 
     for (uint8_t i=0; i<_num_tasks; i++) {
         uint16_t dt = _tick_counter - _last_run[i];
-        uint16_t interval_ticks = pgm_read_word(&_tasks[i].interval_ticks);
+        uint16_t interval_ticks = _loop_rate_hz / _tasks[i].rate_hz;
+        if (interval_ticks < 1) {
+            interval_ticks = 1;
+        }
         if (dt >= interval_ticks) {
             // this task is due to run. Do we have enough time to run it?
             _task_time_allowed = pgm_read_word(&_tasks[i].max_time_micros);

libraries/AP_Scheduler/AP_Scheduler.h

@@ -27,6 +27,16 @@
 
 #define AP_SCHEDULER_NAME_INITIALIZER(_name) .name = #_name,
 
+/*
+  useful macro for creating scheduler task table
+ */
+#define SCHED_TASK_CLASS(classname, classptr, func, _rate_hz, _max_time_micros) { \
+    .function = FUNCTOR_BIND(classptr, &classname::func, void),\
+    AP_SCHEDULER_NAME_INITIALIZER(func)\
+    .rate_hz = _rate_hz,\
+    .max_time_micros = _max_time_micros\
+}
+
 /*
   A task scheduler for APM main loops
 
@@ -43,12 +53,15 @@
 class AP_Scheduler
 {
 public:
+    // constructor
+    AP_Scheduler(void);
+    
     FUNCTOR_TYPEDEF(task_fn_t, void);
 
     struct Task {
         task_fn_t function;
         const char *name;
-        uint16_t interval_ticks;
+        float rate_hz;
         uint16_t max_time_micros;
     };
 
@@ -74,6 +87,11 @@ public:
     // end of a run()
     float load_average(uint32_t tick_time_usec) const;
 
+    // get the configured main loop rate
+    uint16_t get_loop_rate_hz(void) const {
+        return _loop_rate_hz;
+    }
+    
     static const struct AP_Param::GroupInfo var_info[];
 
     // current running task, or -1 if none. Used to debug stuck tasks
@@ -83,6 +101,9 @@ private:
     // used to enable scheduler debugging
     AP_Int8 _debug;
 
+    // overall scheduling rate in Hz
+    AP_Int16 _loop_rate_hz;
+    
     // progmem list of tasks to run
     const struct Task *_tasks;
 

libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp

@@ -30,12 +30,7 @@ private:
 
 static SchedTest schedtest;
 
-#define SCHED_TASK(func, _interval_ticks, _max_time_micros) {\
-    .function = FUNCTOR_BIND(&schedtest, &SchedTest::func, void),\
-    AP_SCHEDULER_NAME_INITIALIZER(func)\
-    .interval_ticks = _interval_ticks,\
-    .max_time_micros = _max_time_micros,\
-}
+#define SCHED_TASK(func, _interval_ticks, _max_time_micros) SCHED_TASK_CLASS(SchedTest, &schedtest, func, _interval_ticks, _max_time_micros)
 
 /*
   scheduler table - all regular tasks are listed here, along with how
@@ -43,16 +38,15 @@ static SchedTest schedtest;
   they are expected to take (in microseconds)
  */
 const AP_Scheduler::Task SchedTest::scheduler_tasks[] = {
-    SCHED_TASK(ins_update,              1,   1000),
-    SCHED_TASK(one_hz_print,           50,   1000),
-    SCHED_TASK(five_second_call,      250,   1800),
+    SCHED_TASK(ins_update,             50,   1000),
+    SCHED_TASK(one_hz_print,            1,   1000),
+    SCHED_TASK(five_second_call,      0.2,   1800),
 };
 
 
 void SchedTest::setup(void)
 {
-    // we 
-    ins.init(AP_InertialSensor::RATE_50HZ);
+    ins.init(scheduler.get_loop_rate_hz());
 
     // initialise the scheduler
     scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));