From 5b9fc2d9fa16a7ac9f6ddfb4f0a29d8ef4b3ebfb Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Sat, 17 Oct 2015 20:29:49 +0300
Subject: [PATCH] Proper termination of UAVACN server thread

---
 src/modules/uavcan/uavcan_servers.cpp | 17 ++++++++++-------
 src/modules/uavcan/uavcan_servers.hpp |  1 +
 2 files changed, 11 insertions(+), 7 deletions(-)

diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp
index 8ae013ce48..ca5bcede66 100644
--- a/src/modules/uavcan/uavcan_servers.cpp
+++ b/src/modules/uavcan/uavcan_servers.cpp
@@ -141,13 +141,14 @@ int UavcanServers::stop()
 		return -1;
 	}
 
-	_instance = nullptr;
-
-	if (server->_subnode_thread != -1) {
-		pthread_cancel(server->_subnode_thread);
-		pthread_join(server->_subnode_thread, NULL);
+	if (server->_subnode_thread) {
+		warnx("stopping fw srv thread...");
+		server->_subnode_thread_should_exit = true;
+		(void)pthread_join(server->_subnode_thread, NULL);
 	}
 
+	_instance = nullptr;
+
 	server->_main_node.getDispatcher().removeRxFrameListener();
 
 	delete server;
@@ -334,7 +335,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
 	memset(_esc_enumeration_ids, 0, sizeof(_esc_enumeration_ids));
 	_esc_enumeration_index = 0;
 
-	while (1) {
+	while (!_subnode_thread_should_exit) {
 
 		if (_check_fw == true) {
 			_check_fw = false;
@@ -554,7 +555,9 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
 		}
 	}
 
-	warnx("exiting.");
+	_subnode_thread_should_exit = false;
+
+	warnx("exiting");
 	return (pthread_addr_t) 0;
 }
 
diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp
index 9bf9705771..3cf4cbb4da 100644
--- a/src/modules/uavcan/uavcan_servers.hpp
+++ b/src/modules/uavcan/uavcan_servers.hpp
@@ -110,6 +110,7 @@ public:
 private:
 	pthread_t         _subnode_thread;
 	pthread_mutex_t   _subnode_mutex;
+	volatile bool     _subnode_thread_should_exit = false;
 
 	int		init();