AP_HAL_Linux: Scheduler: move synchronization to common method

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();
+}

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();