AP_Scheduler: added load_average() function

useful to give a load average in SYS_STATUS mavlink message

libraries/AP_Scheduler/AP_Scheduler.cpp

@@ -86,12 +86,22 @@ void AP_Scheduler::run(uint16_t time_available)
                                               (unsigned)time_taken,
                                               (unsigned)_task_time_allowed);
                     }
-                    return;
+                    goto update_spare_ticks;
                 }
                 time_available -= time_taken;
             }
         }
     }
+
+    // update number of spare microseconds
+    _spare_micros += time_available;
+
+update_spare_ticks:
+    _spare_ticks++;
+    if (_spare_ticks == 32) {
+        _spare_ticks /= 2;
+        _spare_micros /= 2;
+    }
 }
 
 /*
@@ -106,3 +116,11 @@ uint16_t AP_Scheduler::time_available_usec(void)
     return _task_time_allowed - dt;
 }
 
+/*
+  calculate load average as a number from 0 to 1
+ */
+float AP_Scheduler::load_average(uint32_t tick_time_usec) const
+{
+    uint32_t used_time = tick_time_usec - (_spare_micros/_spare_ticks);
+    return used_time / (float)tick_time_usec;
+}

libraries/AP_Scheduler/AP_Scheduler.h

@@ -53,6 +53,11 @@ public:
     // return debug parameter
     uint8_t debug(void) { return _debug; }
 
+    // return load average, as a number between 0 and 1. 1 means
+    // 100% load. Calculated from how much spare time we have at the
+    // end of a run()
+    float load_average(uint32_t tick_time_usec) const;
+
 	static const struct AP_Param::GroupInfo var_info[];
 
 private:
@@ -77,6 +82,12 @@ private:
 
 	// the time in microseconds when the task started
 	uint32_t _task_time_started;
+
+    // number of spare microseconds accumulated
+    uint32_t _spare_micros;
+
+    // number of ticks that _spare_micros is counted over
+    uint8_t _spare_ticks;
 };
 
 #endif // AP_SCHEDULER_H