From 20650e14b70cfba24b5a1172d3da9d91204d7986 Mon Sep 17 00:00:00 2001
From: Lucas De Marchi <lucas.demarchi@intel.com>
Date: Wed, 26 Oct 2016 11:14:09 -0200
Subject: [PATCH] AP_HAL_Linux: allow PollerThread to stop

---
 libraries/AP_HAL_Linux/PollerThread.cpp | 17 ++++++++++++++++-
 libraries/AP_HAL_Linux/PollerThread.h   |  2 ++
 libraries/AP_HAL_Linux/Thread.h         |  3 +++
 3 files changed, 21 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_Linux/PollerThread.cpp b/libraries/AP_HAL_Linux/PollerThread.cpp
index d754966f6c..8b8302b626 100644
--- a/libraries/AP_HAL_Linux/PollerThread.cpp
+++ b/libraries/AP_HAL_Linux/PollerThread.cpp
@@ -140,10 +140,25 @@ void PollerThread::mainloop()
         return;
     }
 
-    while (true) {
+    while (!_should_exit) {
         _poller.poll();
         _cleanup_timers();
     }
+
+    _started = false;
+    _should_exit = false;
+}
+
+bool PollerThread::stop()
+{
+    if (!is_started()) {
+        return false;
+    }
+
+    _should_exit = true;
+    _poller.wakeup();
+
+    return true;
 }
 
 }
diff --git a/libraries/AP_HAL_Linux/PollerThread.h b/libraries/AP_HAL_Linux/PollerThread.h
index c73ea5fe76..85a3398454 100644
--- a/libraries/AP_HAL_Linux/PollerThread.h
+++ b/libraries/AP_HAL_Linux/PollerThread.h
@@ -72,6 +72,8 @@ public:
 
     void mainloop();
 
+    bool stop() override;
+
 protected:
     void _cleanup_timers();
 
diff --git a/libraries/AP_HAL_Linux/Thread.h b/libraries/AP_HAL_Linux/Thread.h
index ccf2ebfb73..d13534f86b 100644
--- a/libraries/AP_HAL_Linux/Thread.h
+++ b/libraries/AP_HAL_Linux/Thread.h
@@ -45,6 +45,8 @@ public:
 
     bool set_stack_size(size_t stack_size);
 
+    virtual bool stop() { return false; }
+
 protected:
     static void *_run_trampoline(void *arg);
 
@@ -59,6 +61,7 @@ protected:
 
     task_t _task;
     bool _started = false;
+    bool _should_exit = false;
     pthread_t _ctx = 0;
 
     struct stack_debug {