From 2b0e214168e787fe49fcb98e00223cc42a3d47da Mon Sep 17 00:00:00 2001
From: Lucas De Marchi <lucas.demarchi@intel.com>
Date: Tue, 2 Feb 2016 21:07:23 -0200
Subject: [PATCH] AP_HAL_Linux: Scheduler: move synchronization to common
 method

---
 libraries/AP_HAL_Linux/Scheduler.cpp | 26 +++++++++-----------------
 libraries/AP_HAL_Linux/Scheduler.h   | 23 ++++++++++++++++++-----
 2 files changed, 27 insertions(+), 22 deletions(-)

diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp
index a3d4e7783b..07c00a0083 100644
--- a/libraries/AP_HAL_Linux/Scheduler.cpp
+++ b/libraries/AP_HAL_Linux/Scheduler.cpp
@@ -52,7 +52,6 @@ extern const AP_HAL::HAL& hal;
 #define APM_LINUX_IO_PERIOD             20000
 #endif // CONFIG_HAL_BOARD_SUBTYPE
 
-
 Scheduler::Scheduler()
 { }
 
@@ -280,10 +279,6 @@ void Scheduler::_run_timers(bool called_from_timer_thread)
 
 void Scheduler::_timer_task()
 {
-    while (system_initializing()) {
-        poll(NULL, 0, 1);
-    }
-
 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
     printf("Initialising rpcmem\n");
     rpcmem_init();
@@ -336,9 +331,6 @@ void Scheduler::_run_io(void)
 
 void Scheduler::_rcin_task()
 {
-    while (system_initializing()) {
-        poll(NULL, 0, 1);
-    }
     while (true) {
         microsleep(APM_LINUX_RCIN_PERIOD);
 #if !HAL_LINUX_UARTS_ON_TIMER_THREAD
@@ -368,9 +360,6 @@ void Scheduler::_run_uarts()
 
 void Scheduler::_uart_task()
 {
-    while (system_initializing()) {
-        poll(NULL, 0, 1);
-    }
     while (true) {
         microsleep(APM_LINUX_UART_PERIOD);
 #if !HAL_LINUX_UARTS_ON_TIMER_THREAD
@@ -381,9 +370,6 @@ void Scheduler::_uart_task()
 
 void Scheduler::_tonealarm_task()
 {
-    while (system_initializing()) {
-        poll(NULL, 0, 1);
-    }
     while (true) {
         microsleep(APM_LINUX_TONEALARM_PERIOD);
 
@@ -394,9 +380,6 @@ void Scheduler::_tonealarm_task()
 
 void Scheduler::_io_task()
 {
-    while (system_initializing()) {
-        poll(NULL, 0, 1);
-    }
     while (true) {
         microsleep(APM_LINUX_IO_PERIOD);
 
@@ -443,3 +426,12 @@ void Scheduler::stop_clock(uint64_t time_usec)
         _run_io();
     }
 }
+
+bool Scheduler::SchedulerThread::_run()
+{
+    while (_sched.system_initializing()) {
+        poll(NULL, 0, 1);
+    }
+
+    return Thread::_run();
+}
diff --git a/libraries/AP_HAL_Linux/Scheduler.h b/libraries/AP_HAL_Linux/Scheduler.h
index b47d597e50..cdc592959f 100644
--- a/libraries/AP_HAL_Linux/Scheduler.h
+++ b/libraries/AP_HAL_Linux/Scheduler.h
@@ -47,6 +47,19 @@ public:
     void microsleep(uint32_t usec);
 
 private:
+    class SchedulerThread : public Thread {
+    public:
+        SchedulerThread(Thread::task_t t, Scheduler &sched)
+            : Thread(t)
+            , _sched(sched)
+        { }
+
+    protected:
+        bool _run() override;
+
+        Scheduler &_sched;
+    };
+
     void _timer_handler(int signum);
 
     AP_HAL::Proc _delay_cb;
@@ -77,11 +90,11 @@ private:
 
     volatile bool _timer_event_missed;
 
-    Thread _timer_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_timer_task, void)};
-    Thread _io_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_io_task, void)};
-    Thread _rcin_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_rcin_task, void)};
-    Thread _uart_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_uart_task, void)};
-    Thread _tonealarm_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_tonealarm_task, void)};
+    SchedulerThread _timer_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_timer_task, void), *this};
+    SchedulerThread _io_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_io_task, void), *this};
+    SchedulerThread _rcin_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_rcin_task, void), *this};
+    SchedulerThread _uart_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_uart_task, void), *this};
+    SchedulerThread _tonealarm_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_tonealarm_task, void), *this};
 
     void _timer_task();
     void _io_task();