HAL_Linux: better realtime setup

lock all memory and pre-fault the stack in all threads

libraries/AP_HAL_Linux/Scheduler.cpp

@@ -11,6 +11,8 @@
 #include <unistd.h>
 #include <stdlib.h>
 #include <stdio.h>
+#include <errno.h>
+#include <sys/mman.h>
 
 using namespace Linux;
 
@@ -26,10 +28,23 @@ LinuxScheduler::LinuxScheduler()
 
 typedef void *(*pthread_startroutine_t)(void *);
 
+/*
+  setup for realtime. Lock all of memory in the thread and pre-fault
+  the given stack size, so stack faults don't cause timing jitter
+ */
+void LinuxScheduler::_setup_realtime(uint32_t size) 
+{
+        uint8_t dummy[size];
+        mlockall(MCL_CURRENT|MCL_FUTURE);
+        memset(dummy, 0, sizeof(dummy));
+}
+
 void LinuxScheduler::init(void* machtnichts)
 {
     clock_gettime(CLOCK_MONOTONIC, &_sketch_start_time);
 
+    _setup_realtime(32768);
+
     pthread_attr_t thread_attr;
     struct sched_param param;
 
@@ -62,13 +77,21 @@ void LinuxScheduler::init(void* machtnichts)
     pthread_create(&_io_thread_ctx, &thread_attr, (pthread_startroutine_t)&Linux::LinuxScheduler::_io_thread, this);
 }
 
+void LinuxScheduler::_microsleep(uint32_t usec)
+{
+    struct timespec ts;
+    ts.tv_sec = 0;
+    ts.tv_nsec = usec*1000UL;
+    while (nanosleep(&ts, &ts) == -1 && errno == EINTR) ;
+}
+
 void LinuxScheduler::delay(uint16_t ms)
 {
     uint32_t start = millis();
     
     while ((millis() - start) < ms) {
         // this yields the CPU to other apps
-        poll(NULL, 0, 1);
+        _microsleep(1000);
         if (_min_delay_cb_ms <= ms) {
             if (_delay_cb) {
                 _delay_cb();
@@ -97,7 +120,7 @@ uint32_t LinuxScheduler::micros()
 
 void LinuxScheduler::delay_microseconds(uint16_t us)
 {
-    usleep(us);
+    _microsleep(us);
 }
 
 void LinuxScheduler::register_delay_callback(AP_HAL::Proc proc,
@@ -189,8 +212,9 @@ void LinuxScheduler::_run_timers(bool called_from_timer_thread)
 
 void *LinuxScheduler::_timer_thread(void)
 {
+    _setup_realtime(32768);
     while (true) {
-        poll(NULL, 0, 1);
+        _microsleep(1000);
 
         // run registered timers
         _run_timers(true);
@@ -220,8 +244,9 @@ void LinuxScheduler::_run_io(void)
 
 void *LinuxScheduler::_uart_thread(void)
 {
+    _setup_realtime(32768);
     while (true) {
-        poll(NULL, 0, 1);
+        _microsleep(1000);
 
         // process any pending serial bytes
         ((LinuxUARTDriver *)hal.uartA)->_timer_tick();
@@ -233,8 +258,9 @@ void *LinuxScheduler::_uart_thread(void)
 
 void *LinuxScheduler::_io_thread(void)
 {
+    _setup_realtime(32768);
     while (true) {
-        poll(NULL, 0, 1);
+        _microsleep(1000);
 
         // process any pending storage writes
         ((LinuxStorage *)hal.storage)->_timer_tick();

libraries/AP_HAL_Linux/Scheduler.h

@@ -42,6 +42,7 @@ public:
 private:
     struct timespec _sketch_start_time;    
     void _timer_handler(int signum);
+    void _microsleep(uint32_t usec);
 
     AP_HAL::Proc _delay_cb;
     uint16_t _min_delay_cb_ms;
@@ -73,6 +74,7 @@ private:
 
     void _run_timers(bool called_from_timer_thread);
     void _run_io(void);
+    void _setup_realtime(uint32_t size);
 };
 
 #endif // CONFIG_HAL_BOARD