diff --git a/include/sta/rtos/queue.hpp b/include/sta/rtos/queue.hpp
index f731c91..3fb127e 100644
--- a/include/sta/rtos/queue.hpp
+++ b/include/sta/rtos/queue.hpp
@@ -34,6 +34,11 @@ namespace sta
 		 */
 		RtosQueue(osMessageQueueId_t handle);
 
+		/**
+		 * @param length The maximum number of elements to be stored in the queue.
+		 */
+		RtosQueue(uint32_t length);
+
 		/**
 		 * @brief Place message in queue.
 		 *
diff --git a/include/sta/rtos/queue.tpp b/include/sta/rtos/queue.tpp
index 0ca5676..a92767c 100644
--- a/include/sta/rtos/queue.tpp
+++ b/include/sta/rtos/queue.tpp
@@ -21,6 +21,13 @@ namespace sta
 	{
 		STA_ASSERT(handle != nullptr);
 	}
+	
+	template <typename T>
+	RtosQueue<T>::RtosQueue(uint32_t length)
+		: handle_{osMessageQueueNew(length, sizeof(Message), NULL)}
+	{
+		STA_ASSERT(handle_ != NULL);
+	}
 
 	template <typename T>
 	bool RtosQueue<T>::put(const Message & msg, uint32_t timeout /* = osWaitForever */)
diff --git a/include/sta/rtos/thread.hpp b/include/sta/rtos/thread.hpp
index 8378c87..1e4d2b7 100644
--- a/include/sta/rtos/thread.hpp
+++ b/include/sta/rtos/thread.hpp
@@ -35,9 +35,9 @@
 #define STA_RTOS_THREAD_FLAG_ERROR_CODE             ( 1UL << 31 )
 
 /**
- * @brief Request thread termination.
+ * @brief Request thread start.
  */
-#define STA_RTOS_THREAD_FLAG_TERMINATE              ( 1UL << 30 )
+#define STA_RTOS_THREAD_FLAG_START					( 1UL << 30 )
 
 
 #define STA_RTOS_THREAD_FLAGS_ERROR_CODE_BITS       UINT32_C( 0x0000000F )
@@ -81,10 +81,37 @@ namespace sta
 		 */
 		void notify(uint32_t flags);
 
+		/**
+		 * @return Returns the flags that were set for this thread.
+		 */
+		uint32_t getFlags();
+
+		/**
+		 * @brief Wait for certain thread flags.
+		 *
+		 * @return Returns the flags triggering the activation.
+		 */
+		uint32_t wait(uint32_t flags);
+
+		/**
+		 * @brief Clears the thread flags set for this thread.
+		 *
+		 * @return Returns the thread flags that set before clearing.
+		 */
+		uint32_t clear(uint32_t flags);
+
 		/**
 		 * @brief Send termination request to thread.
 		 */
 		void requestTermination();
+
+		/**
+		 * @brief Resets the terminate bool to false.
+		 *
+		 * @return Returns the previous value of this variable.
+		 */
+		bool resetTerminationRequest();
+
 		/**
 		 * @brief Forcibly terminate thread.
 		 */
@@ -99,6 +126,7 @@ namespace sta
 
 	private:
 		Handle handle_;
+		bool terminate_;
 	};
 } // namespace sta
 
diff --git a/include/sta/rtos/timer.hpp b/include/sta/rtos/timer.hpp
index d54cfeb..ac6714f 100644
--- a/include/sta/rtos/timer.hpp
+++ b/include/sta/rtos/timer.hpp
@@ -28,7 +28,7 @@ namespace sta
 		 * @param callback 	The callback function to call upon timer timeout.
 		 * @param arg 		The argument to pass to the callback function.
 		 */
-		RtosTimer(void (*callback)(void *arg), void *arg);
+		RtosTimer(std::function<void(void*)>, void *arg);
 
 		/**
 		 * @brief Initializes the timer with a callback that has no effect.
@@ -43,7 +43,7 @@ namespace sta
          * @param callback 	The callback function to call upon timer timeout.
 		 * @param arg 		The argument to pass to the callback function.
          */
-        void setCallback(void (*callback)(void *arg), void *arg);
+        void setCallback(std::function<void(void*)> callback, void *arg);
 
         /**
          * @brief Run the timer for a given number of milliseconds.
diff --git a/src/system/startup.cpp b/src/system/startup.cpp
index 9636bd7..f68decc 100644
--- a/src/system/startup.cpp
+++ b/src/system/startup.cpp
@@ -44,15 +44,15 @@ void startALPAKA(void * arg)
 {
 	STA_ASSERT_MSG(osKernelGetState() != osKernelInactive, "Cannot call startALPAKA() before osKernelInitialize()");
 
-	// Call further initialization code
-	sta::rtos::startupExtras(arg);
-
 	// Initialize HAL
 	sta::initHAL();
 
 	// Initialize RTOS system resources
 	sta::rtos::initSystem();
 
+	// Call further initialization code
+	sta::rtos::startupExtras(arg);
+
 	// Wake threads
 	sta::rtos::signalStartupEvent();
 
diff --git a/src/thread.cpp b/src/thread.cpp
index 781f0d1..7e88251 100644
--- a/src/thread.cpp
+++ b/src/thread.cpp
@@ -29,13 +29,46 @@ namespace sta
 		STA_ASSERT(handle_.get() != nullptr);
 		STA_ASSERT_MSG(IS_THREAD_SYS_FLAGS(flags), "Only system flags allowed");
 
-		osThreadFlagsSet(handle_.get(), flags);
+		uint32_t rslt = osThreadFlagsSet(handle_.get(), flags);
+
+		STA_ASSERT_MSG(rslt != osFlagsErrorUnknown, "Unknown thread flag error.");
+		STA_ASSERT_MSG(rslt != osFlagsErrorParameter, "Parameter thread_id is not a valid thread or flags has highest bit set.");
+		STA_ASSERT_MSG(rslt != osFlagsErrorResource, "The thread is in invalid state.");
 	}
 
+	uint32_t RtosThread::wait(uint32_t flags)
+	{
+		return osThreadFlagsWait(flags, osFlagsWaitAny, osWaitForever);
+	}
+
+	uint32_t RtosThread::clear(uint32_t flags)
+	{
+		uint32_t setFlags = osThreadFlagsClear(flags);
+
+		STA_ASSERT(setFlags != (uint32_t)osError);
+
+		return setFlags;
+	}
+
+	uint32_t RtosThread::getFlags()
+	{
+		STA_ASSERT(handle_.get() != nullptr);
+
+		return osThreadFlagsGet();
+	}
 
 	void RtosThread::requestTermination()
 	{
-		sysNotify(STA_RTOS_THREAD_FLAG_TERMINATE);
+		// sysNotify(STA_RTOS_THREAD_FLAG_TERMINATE);
+		terminate_ = true;
+	}
+
+	bool RtosThread::resetTerminationRequest()
+	{
+		bool temp = terminate_;
+		terminate_ = false;
+
+		return temp;
 	}
 
 	void RtosThread::terminate()
diff --git a/src/timer.cpp b/src/timer.cpp
index 4f227b0..7a7d6d3 100644
--- a/src/timer.cpp
+++ b/src/timer.cpp
@@ -4,7 +4,7 @@
 
 
 namespace sta {
-    RtosTimer::RtosTimer(void (*callback)(void *arg), void *arg)
+    RtosTimer::RtosTimer(std::function<void(void*)> callback, void *arg)
     	: timer_id_{NULL},
 		  timer_attr_{.name="Timer", .attr_bits=osTimerOnce},
 		  callback_{callback},
@@ -28,7 +28,7 @@ namespace sta {
         osTimerDelete(timer_id_);
     }
 
-    void RtosTimer::setCallback(void (*callback)(void *arg), void *arg)
+    void RtosTimer::setCallback(std::function<void(void*)> callback, void *arg)
     {
     	callback_ = callback;
     	callbackArg_ = arg;