AP_HAL_Linux: teardown scheduler threads

libraries/AP_HAL_Linux/HAL_Linux_Class.cpp

@@ -382,6 +382,7 @@ void HAL_Linux::run(int argc, char* const argv[], Callbacks* callbacks) const
     rcin->teardown();
     I2CDeviceManager::from(i2c_mgr)->teardown();
     SPIDeviceManager::from(spi)->teardown();
+    Scheduler::from(scheduler)->teardown();
 }
 
 void HAL_Linux::setup_signal_handlers() const

libraries/AP_HAL_Linux/Scheduler.cpp

@@ -456,3 +456,18 @@ bool Scheduler::SchedulerThread::_run()
 
     return PeriodicThread::_run();
 }
+
+void Scheduler::teardown()
+{
+    _timer_thread.stop();
+    _io_thread.stop();
+    _rcin_thread.stop();
+    _uart_thread.stop();
+    _tonealarm_thread.stop();
+
+    _timer_thread.join();
+    _io_thread.join();
+    _rcin_thread.join();
+    _uart_thread.join();
+    _tonealarm_thread.join();
+}

libraries/AP_HAL_Linux/Scheduler.h

@@ -50,6 +50,8 @@ public:
 
     void microsleep(uint32_t usec);
 
+    void teardown();
+
 private:
     class SchedulerThread : public PeriodicThread {
     public: