diff --git a/create_ros_ws.sh b/create_ros_ws.sh
index b0a34c9..c8d2992 100755
--- a/create_ros_ws.sh
+++ b/create_ros_ws.sh
@@ -138,15 +138,13 @@ function main
             catkin config --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCMAKE_BUILD_TYPE=Release
         fi
     else
-        if [ ${dev} -gt 0 ]; then
-            echo 'build:' > ${abspath}/colcon_defaults.yaml
-            echo '    event-handlers: [ "console_direct+" ]' >> ${abspath}/colcon_defaults.yaml
-            echo '    cmake-args: [ "-DCMAKE_BUILD_TYPE=Debug", "-DCMAKE_EXPORT_COMPILE_COMMANDS=ON" ]' >> ${abspath}/colcon_defaults.yaml
-        else
-            echo 'build:' > ${abspath}/colcon_defaults.yaml
-            echo '#    event-handlers: [ "console_direct+" ]' >> ${abspath}/colcon_defaults.yaml
-            echo '    cmake-args: [ "-DCMAKE_BUILD_TYPE=Release", "-DCMAKE_EXPORT_COMPILE_COMMANDS=ON" ]' >> ${abspath}/colcon_defaults.yaml
+        echo 'build:' > ${abspath}/colcon_defaults.yaml
+        if [ ${dev} -eq 0 ]; then
+            echo -n '#' >> ${abspath}/colcon_defaults.yaml
         fi
+        echo '    event-handlers: [ "console_direct+" ]' >> ${abspath}/colcon_defaults.yaml
+        echo '    cmake-args: [ "-DCMAKE_BUILD_TYPE=Release", "-DCMAKE_EXPORT_COMPILE_COMMANDS=ON", "-DFPSDK_BUILD_TESTING=OFF" ]' \
+            >> ${abspath}/colcon_defaults.yaml
     fi
 
     # Happy?
diff --git a/fixposition-sdk b/fixposition-sdk
index 4822a88..e7b00fe 160000
--- a/fixposition-sdk
+++ b/fixposition-sdk
@@ -1 +1 @@
-Subproject commit 4822a88faef16af298794be3c9d3af4f62712fd5
+Subproject commit e7b00fea2d1be2ea7bab9b2ce8215f0738c999b4
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp
index f3142e8..482edf2 100644
--- a/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp
+++ b/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp
@@ -221,7 +221,7 @@ class FixpositionDriver : private boost::noncopyable {
     // Worker thread
     fpsdk::common::parser::Parser parser_;  //!< Protocol parser for incoming messages
     fpsdk::common::thread::Thread worker_;  //!< Worker thread handle
-    void Worker(void* arg);                 //!< Worker thread
+    bool Worker();                          //!< Worker thread
 
     // Observers for received messages
     std::unordered_map<std::string, std::vector<FpaObserver>> fpa_observers_;    //!< FP_A message observers
diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp
index 08bf6ab..269b9fd 100644
--- a/fixposition_driver_lib/src/fixposition_driver.cpp
+++ b/fixposition_driver_lib/src/fixposition_driver.cpp
@@ -35,11 +35,12 @@ namespace fixposition {
 
 using namespace fpsdk::common;
 using namespace fpsdk::common::parser;
+using namespace fpsdk::common::thread;
 
 FixpositionDriver::FixpositionDriver(const DriverParams& params)
     : /* clang-format off */
     params_   { params },
-    worker_   { "driver", std::bind(&FixpositionDriver::Worker, this, std::placeholders::_1) }  // clang-format on
+    worker_   { "driver", std::bind(&FixpositionDriver::Worker, this) }  // clang-format on
 {}
 
 FixpositionDriver::~FixpositionDriver() { StopDriver(); }
@@ -260,11 +261,13 @@ bool FixpositionDriver::Write(const uint8_t* buf, const std::size_t size) {
 bool FixpositionDriver::StartDriver() { return Connect() && worker_.Start(); }
 
 void FixpositionDriver::StopDriver() {
-    worker_.Stop();
+    if (worker_.GetStatus() == worker_.Status::RUNNING) {
+        worker_.Stop();
+    }
     Disconnect();
 }
 
-void FixpositionDriver::Worker(void* /*arg*/) {
+bool FixpositionDriver::Worker() {
     INFO("Driver running...");
 
     while (!worker_.ShouldAbort()) {
@@ -311,12 +314,13 @@ void FixpositionDriver::Worker(void* /*arg*/) {
         // Reconnect after some time...
         else {
             INFO("Reconnecting in %.1f seconds...", params_.reconnect_delay_);
-            if (worker_.Sleep(params_.reconnect_delay_ * 1000)) {
+            if (worker_.Sleep(params_.reconnect_delay_ * 1000) == WaitRes::WOKEN) {
                 break;
             }
             Connect();
         }
     }
+    return true;
 }
 
 // ---------------------------------------------------------------------------------------------------------------------