diff --git a/App/Inc/sta/config.hpp b/App/Inc/sta/config.hpp
index bd3f7e0..c2e4c29 100644
--- a/App/Inc/sta/config.hpp
+++ b/App/Inc/sta/config.hpp
@@ -29,7 +29,7 @@
 #define STA_RTOS_SYSTEM_EVENTS_ENABLE
 // #define STA_RTOS_SYSTEM_WATCHDOG_ENABLE
 // #define STA_RTOS_WATCHDOG_ENABLE
-//#define STA_CAN_BUS_ENABLE
+#define STA_CAN_BUS_ENABLE
 
 // Uses the default configuration for TACOS.
 #include<sta/tacos/configs/default.hpp>
diff --git a/App/Src/tasks/can_task.cpp b/App/Src/tasks/can_task.cpp
index 4ffd940..73fa665 100644
--- a/App/Src/tasks/can_task.cpp
+++ b/App/Src/tasks/can_task.cpp
@@ -35,17 +35,24 @@ namespace demo
 		for (int i = 0; i < 8; ++i) {
 			payload[i] = i + 1;
 		}
+
+		struct CanMsgHeader header ={
+				txHeader.id.sid, txHeader.id.eid, (uint8_t)txHeader.id.format, txHeader.payloadLength
+		};
+
+		CanDataMsg myCanDataMsg;
+		myCanDataMsg.header = header;
+		for (int i = 0; i < 64; ++i) {
+			myCanDataMsg.payload[i] = payload[i];
+		}
+
+		sta::tacos::CanBus::instance()->queueCanBusMsg(myCanDataMsg, osWaitForever);
 	}
 
 	void CanTask::func()
 	{
-		//struct CanDataMsg myCanDataMsg = {
-        ///*.header = */ txHeader.id.sid, txHeader.id.eid, (uint8_t)txHeader.id.format, txHeader.payloadLength ,
-        ///*.payload = */ *payload  // Initialize payload to zero (or any other values as needed)
-    	//};
-		canController.sendFrame(txHeader, payload);
-		//STA_DEBUG_PRINTLN(sta::tacos::CanBus::instance()->queueCanBusMsg(myCanDataMsg, osWaitForever));
-		STA_DEBUG_PRINTLN("SENT FRAME");
+		
+		STA_DEBUG_PRINTLN("Wasting time in CAN task...");
 		HAL_Delay(1000);
 	}
 } // namespace demo
diff --git a/Libs/TACOS b/Libs/TACOS
index 16b2ed5..411d671 160000
--- a/Libs/TACOS
+++ b/Libs/TACOS
@@ -1 +1 @@
-Subproject commit 16b2ed5a20df1c04b9bbcf5e12101d5e822e45d2
+Subproject commit 411d671ff5a6c495784492244a0e438232fa85d5