--- a/drivers/timers_xeno/timers_xeno.c	Fri Apr 20 15:00:04 2018 +0200
+++ b/drivers/timers_xeno/timers_xeno.c	Mon Apr 23 12:32:03 2018 +0200
@@ -64,13 +64,6 @@
 	rt_cond_signal(&timer_set);
 }
 
-void cleanup_all(void)
-{
-	if (rt_task_join(&timerloop_task) != 0){
-		printf("Failed to join with Timerloop task\n");
-	}
-	rt_task_delete(&timerloop_task);
-}
 
 /**
  * Clean all Semaphores, mutex, condition variable and main task
@@ -81,6 +74,7 @@
 	rt_mutex_delete(&condition_mutex);
 	rt_cond_delete(&timer_set);
 	rt_sem_delete(&control_task);
+	rt_task_unblock(&timerloop_task);
 	if (rt_task_join(&timerloop_task) != 0){
 		printf("Failed to join with Timerloop task\n");
 	}
@@ -187,7 +181,7 @@
 	return;
 	
 error:
-	cleanup_all();
+	rt_task_delete(&timerloop_task);
 }
 
 /**
@@ -225,6 +219,7 @@
  */
 void WaitReceiveTaskEnd(TASK_HANDLE *ReceiveLoop_task)
 {
+	rt_task_unblock(ReceiveLoop_task);
 	if (rt_task_join(ReceiveLoop_task) != 0){
 		printf("Failed to join with Receive task\n");
 	}