From 2b5f9fdd6b8aea8dc875fd23f3491b12a913c973 Mon Sep 17 00:00:00 2001
From: Lucas De Marchi <lucas.demarchi@intel.com>
Date: Wed, 26 Oct 2016 19:58:57 -0200
Subject: [PATCH] AP_HAL_Linux: teardown scheduler threads

---
 libraries/AP_HAL_Linux/HAL_Linux_Class.cpp |  1 +
 libraries/AP_HAL_Linux/Scheduler.cpp       | 15 +++++++++++++++
 libraries/AP_HAL_Linux/Scheduler.h         |  2 ++
 3 files changed, 18 insertions(+)

diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
index 0744c0baf8..f0417f842a 100644
--- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
+++ b/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
diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp
index 0ff2d8fe57..a231c6ba05 100644
--- a/libraries/AP_HAL_Linux/Scheduler.cpp
+++ b/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();
+}
diff --git a/libraries/AP_HAL_Linux/Scheduler.h b/libraries/AP_HAL_Linux/Scheduler.h
index eaa2205dfb..bb991a8aa8 100644
--- a/libraries/AP_HAL_Linux/Scheduler.h
+++ b/libraries/AP_HAL_Linux/Scheduler.h
@@ -50,6 +50,8 @@ public:
 
     void microsleep(uint32_t usec);
 
+    void teardown();
+
 private:
     class SchedulerThread : public PeriodicThread {
     public: