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: