diff --git a/App/Src/tasks/can_task.cpp b/App/Src/tasks/can_task.cpp
index 2212da7..9bf7983 100644
--- a/App/Src/tasks/can_task.cpp
+++ b/App/Src/tasks/can_task.cpp
@@ -10,6 +10,7 @@
 #include <sta/rtos/debug/heap_stats.hpp>
 #include "can.h"
 #include <sta/tacos.hpp>
+#include <sta/tacos/manager.hpp>
 
 #include <cmsis_os2.h>
 
@@ -27,15 +28,13 @@ namespace tasks
 
 	void CanTask::func()
 	{
-		if (CAN_queue_.available() > 0)
+		CanSysMsg msg;
+		if (CAN_queue_.get(&msg, osWaitForever))
 		{
-			// Receiving message
-			CanSysMsg msg;
-			CAN_queue_.get(&msg);
 
 			// Interpret received
 			uint8_t type_id, sensor_ID, value, include;
-			unpackValues(canRX[0], &type_id, &sensor_ID, &value, &include);
+			unpackValues(msg.payload[0], &type_id, &sensor_ID, &value, &include);
 
 			// Reformat for internal use
 			msg.header.payloadLength = 2;
@@ -60,9 +59,9 @@ namespace tasks
 			}
 
 			// Append to the correct thread's queue
-			for (std::shared_ptr<TacosThread> thread : Manager::instance()->getActiveThreads()){
+			for (std::shared_ptr<TacosThread> thread : sta::tacos::Manager::instance()->getActiveThreads()){
 				if (thread->getCanID() == target_ID){
-					thread->CAN_queue_.put(sysMsg);
+					thread->CAN_queue_.put(msg);
 					break;
 				}
 			}