diff --git a/include/sta/rtos/system/can_bus.hpp b/include/sta/rtos/system/can_bus.hpp
index 17e25eb..7b909f0 100644
--- a/include/sta/rtos/system/can_bus.hpp
+++ b/include/sta/rtos/system/can_bus.hpp
@@ -192,14 +192,23 @@ namespace sta
 
 } // namespace sta
 
-namespace dummy
+namespace debug
 {
-	void handleSysMessage(const sta::CanRxHeader & header, const uint8_t * payload);
-
-	void handleDataMessage(const sta::IsotpMessage & msg);
-
-} // namespace dummy
+	/**
+	 * @brief Output CAN frame ID to UART.
+	 *
+	 * @param id    Frame ID
+	 */
+	void printFrameID(const sta::CanFrameId & id);
 
+	/**
+	 * @brief Output CAN frame payload to UART.
+	 *
+	 * @param payload   Payload buffer
+	 * @param size      Payload size
+	 */
+	void printPayloadHex(const uint8_t * payload, uint8_t size);
+} // namespace debug
 
 #endif // STA_RTOS_CAN_BUS_ENABLE
 
diff --git a/src/system/can_bus.cpp b/src/system/can_bus.cpp
index 9fc36be..c9dd465 100644
--- a/src/system/can_bus.cpp
+++ b/src/system/can_bus.cpp
@@ -16,32 +16,13 @@
 #include <sta/rtos/system/events.hpp>
 #include <sta/devices/stm32/hal.hpp>
 #include <sta/rtos/system/events.hpp>
-
 #include <sta/rtos/system/can_bus.hpp>
-#include <sta/tacos/can_bus.hpp>
-
-#include <sta/config.hpp>
-#include <sta/tacos/statemachine.hpp>
 
 #include <cmsis_os2.h>
 #include <FreeRTOS.h>
 
 #include <cstring>
 
-namespace sta
-{
-	namespace rtos
-	{
-
-		void initCanBus()
-		{
-			sta::tacos::CanBus::instance(getCanController())->start();
-		}
-
-	} // namespace rtos
-} // namespace sta
-
-
 namespace debug
 {
 	/**
@@ -79,82 +60,6 @@ namespace debug
 	}
 } // namespace debug
 
-
-namespace dummy
-{
-	void handleSysMessage(const sta::CanRxHeader & header, const uint8_t * payload)
-	{
-		// Write frame payload to DebugSerial
-		STA_DEBUG_PRINTLN("[event] RX sys frame");
-
-		debug::printFrameID(header.id);
-		debug::printPayloadHex(payload, header.payloadLength);
-
-		// Sysmessage is mainly only state change from GRSM
-		// TODO add other cases
-
-		// 0 is from state, 1 is to state, 2 is lockout, 3 is failsafe (-1 if inactive)
-		if(payload[1] > 0 && payload[1] < STA_TACOS_NUM_STATES){
-
-			if(payload[3] == -1){
-				sta::tacos::Statemachine::instance()->requestStateTransition(payload[0], payload[1], payload[2]);
-			}
-			else{
-				sta::tacos::Statemachine::instance()->requestTimedStateTransition(payload[0], payload[1], payload[2], payload[3]);
-			}
-		}
-	}
-
-	void handleDataMessage(const sta::IsotpMessage & msg)
-	{
-		STA_ASSERT(msg.buffer != nullptr);
-		STA_ASSERT(msg.size != 0);
-
-		STA_DEBUG_PRINTLN("[event] RX data message");
-
-		debug::printFrameID(msg.frameID);
-
-		// TODO Forward message to other threads
-
-//		if (buffer[0] == DEMO_BMP_PACKET_ID)
-//		{
-//			BmpPacket packet;
-//			if (unpack(buffer + 1, size - 1, &packet))
-//			{
-//				STA_DEBUG_PRINTLN();
-//				STA_DEBUG_PRINTLN("# ############");
-//				STA_DEBUG_PRINTLN("# ## BMP380 ##");
-//				STA_DEBUG_PRINTLN("# ############");
-//				STA_DEBUG_PRINTLN("#");
-//
-//				STA_DEBUG_PRINT("# temperature: ");
-//				STA_DEBUG_PRINT(packet.temperature);
-//				STA_DEBUG_PRINTLN(" *C");
-//				STA_DEBUG_PRINT("# pressure: ");
-//				STA_DEBUG_PRINT(packet.pressure);
-//				STA_DEBUG_PRINTLN(" Pa");
-//				STA_DEBUG_PRINT("# altitude: ");
-//				STA_DEBUG_PRINT(packet.altitude);
-//				STA_DEBUG_PRINTLN(" m");
-//				STA_DEBUG_PRINTLN();
-//			}
-//			else
-//			{
-//				STA_DEBUG_PRINTLN("[error] BMP unpack failed");
-//			}
-//		}
-//		else
-		{
-			STA_DEBUG_PRINT("ID: ");
-			STA_DEBUG_PRINTLN(msg.buffer[0], sta::IntegerBase::HEX);
-			STA_DEBUG_PRINT("size: ");
-			STA_DEBUG_PRINTLN(msg.size);
-		}
-	}
-} // namespace dummy
-
-
-
 namespace sta
 {
 	AlpakaCanBus::AlpakaCanBus(CanController * controller, TimeMsFn timeMs, SysMsgHandler sysMsgHandler, DataMsgHandler dataMsgHandler)