AP_HAL_Linux: support for timesliced timers

In order for lower frequency timers not to be called at the same iteration
of the main timer loop. This improves scheduling.

libraries/AP_HAL_Linux/Scheduler.cpp

@@ -9,6 +9,7 @@
 #include "Util.h"
 #include "SPIUARTDriver.h"
 #include "RPIOUARTDriver.h"
+#include <algorithm>
 #include <poll.h>
 #include <unistd.h>
 #include <stdlib.h>
@@ -195,6 +196,69 @@ void Scheduler::register_timer_process(AP_HAL::MemberProc proc)
     }
 }
 
+bool Scheduler::register_timer_process(AP_HAL::MemberProc proc,
+                                       uint8_t freq_div)
+{
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
+    if (freq_div > 1) {
+        return _register_timesliced_proc(proc, freq_div);
+    }
+    /* fallback to normal timer process */
+#endif
+    register_timer_process(proc);
+    return false;
+}
+
+bool Scheduler::_register_timesliced_proc(AP_HAL::MemberProc proc,
+                                          uint8_t freq_div)
+{
+    unsigned int i, j;
+    uint8_t distance, min_distance, best_distance;
+    uint8_t best_timeslot;
+
+    if (_num_timesliced_procs > LINUX_SCHEDULER_MAX_TIMESLICED_PROCS) {
+        hal.console->printf("Out of timesliced processes\n");
+        return false;
+    }
+
+    /* if max_freq_div increases, update the timeslots accordingly */
+    if (freq_div > _max_freq_div) {
+        for (i = 0; i < _num_timesliced_procs; i++) {
+            _timesliced_proc[i].timeslot =  _timesliced_proc[i].timeslot
+                                            / _max_freq_div * freq_div;
+        }
+        _max_freq_div = freq_div;
+    }
+
+    best_distance = 0;
+    best_timeslot = 0;
+
+    /* Look for the timeslot that maximizes the min distance with other timeslots */
+    for (i = 0; i < _max_freq_div; i++) {
+        min_distance = _max_freq_div;
+        for (j = 0; j < _num_timesliced_procs; j++) {
+            distance = std::min(i - _timesliced_proc[j].timeslot,
+                            _max_freq_div + _timesliced_proc[j].timeslot - i);
+            if (distance < min_distance) {
+                min_distance = distance;
+                if (min_distance == 0) {
+                    break;
+                }
+            }
+        }
+        if (min_distance > best_distance) {
+            best_distance = min_distance;
+            best_timeslot = i;
+        }
+    }
+
+    _timesliced_proc[_num_timesliced_procs].proc = proc;
+    _timesliced_proc[_num_timesliced_procs].timeslot = best_timeslot;
+    _timesliced_proc[_num_timesliced_procs].freq_div = freq_div;
+    _num_timesliced_procs++;
+    return true;
+}
+
 void Scheduler::register_io_process(AP_HAL::MemberProc proc)
 {
     for (uint8_t i = 0; i < _num_io_procs; i++) {
@@ -230,6 +294,8 @@ void Scheduler::resume_timer_procs()
 
 void Scheduler::_run_timers(bool called_from_timer_thread)
 {
+    int i;
+
     if (_in_timer_proc) {
         return;
     }
@@ -239,7 +305,7 @@ void Scheduler::_run_timers(bool called_from_timer_thread)
         printf("Failed to take timer semaphore in _run_timers\n");
     }
     // now call the timer based drivers
-    for (int i = 0; i < _num_timer_procs; i++) {
+    for (i = 0; i < _num_timer_procs; i++) {
         if (_timer_proc[i]) {
             _timer_proc[i]();
         }
@@ -253,6 +319,20 @@ void Scheduler::_run_timers(bool called_from_timer_thread)
     }
 #endif
 
+    for (i = 0; i < _num_timesliced_procs; i++) {
+        if ((_timeslices_count + _timesliced_proc[i].timeslot)
+            % _timesliced_proc[i].freq_div == 0) {
+            _timesliced_proc[i].proc();
+        }
+    }
+
+    if (_max_freq_div != 0) {
+        _timeslices_count++;
+        if (_timeslices_count == _max_freq_div) {
+            _timeslices_count = 0;
+        }
+    }
+
     _timer_semaphore.give();
 
     // and the failsafe, if one is setup

libraries/AP_HAL_Linux/Scheduler.h

@@ -10,6 +10,7 @@
 #include <pthread.h>
 
 #define LINUX_SCHEDULER_MAX_TIMER_PROCS 10
+#define LINUX_SCHEDULER_MAX_TIMESLICED_PROCS 10
 #define LINUX_SCHEDULER_MAX_IO_PROCS 10
 
 class Linux::Scheduler : public AP_HAL::Scheduler {
@@ -30,6 +31,7 @@ public:
                 uint16_t min_time_ms);
 
     void     register_timer_process(AP_HAL::MemberProc);
+    bool     register_timer_process(AP_HAL::MemberProc, uint8_t);
     void     register_io_process(AP_HAL::MemberProc);
     void     suspend_timer_procs();
     void     resume_timer_procs();
@@ -65,6 +67,16 @@ private:
     AP_HAL::MemberProc _timer_proc[LINUX_SCHEDULER_MAX_TIMER_PROCS];
     uint8_t _num_timer_procs;
     volatile bool _in_timer_proc;
+    uint8_t _timeslices_count;
+
+    struct timesliced_proc {
+        AP_HAL::MemberProc proc;
+        uint8_t timeslot;
+        uint8_t freq_div;
+    };
+    timesliced_proc _timesliced_proc[LINUX_SCHEDULER_MAX_TIMESLICED_PROCS];
+    uint8_t _num_timesliced_procs;
+    uint8_t _max_freq_div;
 
     AP_HAL::MemberProc _io_proc[LINUX_SCHEDULER_MAX_IO_PROCS];
     uint8_t _num_io_procs;
@@ -89,6 +101,7 @@ private:
     void _run_io(void);
     void _create_realtime_thread(pthread_t *ctx, int rtprio, const char *name,
                                  pthread_startroutine_t start_routine);
+    bool _register_timesliced_proc(AP_HAL::MemberProc, uint8_t);
 
     uint64_t _stopped_clock_usec;