diff --git a/App/Inc/sta/config.hpp b/App/Inc/sta/config.hpp
index c2e4c29..d27f1b7 100644
--- a/App/Inc/sta/config.hpp
+++ b/App/Inc/sta/config.hpp
@@ -35,4 +35,11 @@
 #include<sta/tacos/configs/default.hpp>
 #define STA_TACOS_NUM_STATES 3
 
+// Configure CAN IDs for tasks
+#define SYSTEM_CAN_MSG_HANDLER 	0x123
+#define SYSTEM_CAN_THERMO 		0x124
+#define SYSTEM_CAN_RELAY 		0x125
+#define SYSTEM_CAN_V_IN 		0x126
+
+#define SYSTEM_CAN_ERROR 		0x013
 #endif /* INC_STA_CONFIG_HPP_ */
diff --git a/App/Inc/tasks/can_task.hpp b/App/Inc/tasks/can_task.hpp
index ada096d..ab16a1e 100644
--- a/App/Inc/tasks/can_task.hpp
+++ b/App/Inc/tasks/can_task.hpp
@@ -11,16 +11,18 @@
 #include <sta/tacos/thread.hpp>
 #include <sta/devices/stm32/can.hpp>
 
-namespace demo
+namespace tasks
 {
 	class CanTask : public sta::tacos::TacosThread {
 	public:
-		CanTask(const char* name, uint32_t canID);
+		CanTask(uint32_t canID);
 
 		void init() override;
 
 		void func() override;
+
 	private:
+		void unpackValues(uint8_t packedByte, uint8_t* type_id, uint8_t* sensor_ID, uint8_t* value, uint8_t* include);
 	};
 } // namespace demo
 
diff --git a/App/Src/tasks/can_task.cpp b/App/Src/tasks/can_task.cpp
index 06e642e..2212da7 100644
--- a/App/Src/tasks/can_task.cpp
+++ b/App/Src/tasks/can_task.cpp
@@ -13,10 +13,10 @@
 
 #include <cmsis_os2.h>
 
-namespace demo
+namespace tasks
 {
-	CanTask::CanTask(const char* name, uint32_t canID)
-		: TacosThread(name, osPriorityNormal)
+	CanTask::CanTask(uint32_t canID)
+		: TacosThread("SYS_MSG_HANDLE", osPriorityNormal)
 	{
 		setCanID(canID);
 	}
@@ -27,25 +27,53 @@ namespace demo
 
 	void CanTask::func()
 	{
-		//STA_DEBUG_PRINTLN("Can Task awaiting message");
 		if (CAN_queue_.available() > 0)
 		{
 			// Receiving message
 			CanSysMsg msg;
 			CAN_queue_.get(&msg);
-			STA_DEBUG_PRINTLN("Can Task received message");
 
-			// Sending it back with one changed bit
-			msg.payload[1] = 3;
-			msg.header.sid = getCanID();
-			msg.header.eid = 0;
-			msg.header.format = 0;
+			// Interpret received
+			uint8_t type_id, sensor_ID, value, include;
+			unpackValues(canRX[0], &type_id, &sensor_ID, &value, &include);
 
-			sta::tacos::queueCanBusMsg(msg, 0);
-			STA_DEBUG_PRINTLN("Can Task sent message");
+			// Reformat for internal use
+			msg.header.payloadLength = 2;
+			msg.payload[0] = sensor_ID;			// First Byte determines sensor number
+			msg.payload[1] = value;				// Second Byte determines value
 
-			HAL_Delay(500);
+			uint32_t target_ID;
+
+			switch (type_id){
+				case 0: // Relay
+					target_ID = SYSTEM_CAN_RELAY;
+					break;
+				case 1: // Thermocouple
+					target_ID = SYSTEM_CAN_THERMO;
+					break;
+				case 2: // V_IN
+					target_ID = SYSTEM_CAN_V_IN;
+					break;
+
+				default:
+					target_ID = SYSTEM_CAN_ERROR;
+			}
+
+			// Append to the correct thread's queue
+			for (std::shared_ptr<TacosThread> thread : Manager::instance()->getActiveThreads()){
+				if (thread->getCanID() == target_ID){
+					thread->CAN_queue_.put(sysMsg);
+					break;
+				}
+			}
 		}
 	}
+
+	void CanTask::unpackValues(uint8_t packedByte, uint8_t* type_id, uint8_t* sensor_ID, uint8_t* value, uint8_t* include) {
+		*type_id = (packedByte >> 6) & 0x03;       // Extracting two bits for type_id
+		*sensor_ID = (packedByte >> 3) & 0x07;     // Extracting three bits for sensorID
+		*include = (packedByte >> 2) & 0x01;       // Extracting the flag for included value
+		*value = (packedByte >> 1) & 0x01;         // Extracting one bit for value
+	}
 } // namespace demo