diff --git a/include/sta/tacos/configs/default.hpp b/include/sta/tacos/configs/default.hpp
index 42b0b18..c8a698f 100644
--- a/include/sta/tacos/configs/default.hpp
+++ b/include/sta/tacos/configs/default.hpp
@@ -15,7 +15,7 @@
 #define STA_TACOS_INITIAL_STATE             0
 
 // Can Bus settings
-#define STA_RTOS_CAN_BUS_QUEUE_LENGTH 5
+#define STA_RTOS_CAN_BUS_QUEUE_LENGTH 25
 #define STA_RTOS_CAN_BUS_MAX_FILTER 14
 #define STA_RTOS_CAN_BUS_MAX_PAYLOAD_SIZE 8
 
diff --git a/src/can_bus.cpp b/src/can_bus.cpp
index 529581b..87e147f 100644
--- a/src/can_bus.cpp
+++ b/src/can_bus.cpp
@@ -62,7 +62,9 @@ namespace sta
                     	if(canBusSysQueueBuffer_[i] != nullptr){
                     		sysMsg = *canBusSysQueueBuffer_[i];
                     		canBusSysQueueBuffer_[i] = nullptr;
-
+#ifndef STA_CAN_BUS_FWD_ENABLE
+							handleSysMessage(sysMsg.header, sysMsg.payload);
+#else 
 							if (!handleSysMessage(sysMsg.header, sysMsg.payload)){
 
 								// Append to the correct thread's queue
@@ -73,6 +75,7 @@ namespace sta
 									}
 								}
 							}
+#endif // STA_CAN_BUS_FWD_ENABLE
 						}
                     }
                 }
diff --git a/src/thread.cpp b/src/thread.cpp
index e13863d..4b65bb1 100644
--- a/src/thread.cpp
+++ b/src/thread.cpp
@@ -27,10 +27,10 @@ namespace sta
 #ifdef STA_TACOS_WATCHDOG_ENABLED
 				  status_{ThreadStatus::STOPPED},
 #endif // STA_TACOS_WATCHDOG_ENABLED
-#ifdef STA_CAN_BUS_ENABLE
+#ifdef STA_CAN_BUS_FWD_ENABLE
 				  CAN_queue_{STA_RTOS_CAN_BUS_QUEUE_LENGTH},
 				  canID_{0},
-#endif // STA_CAN_BUS_ENABLE
+#endif // STA_CAN_BUS_FWD_ENABLE
 				  terminate_{false},
 				  previous_tick_{0}
 		{
@@ -189,7 +189,7 @@ namespace sta
 
 		TacosThread::~TacosThread(){}
 
-#ifdef STA_CAN_BUS_ENABLE
+#ifdef STA_CAN_BUS_FWD_ENABLE
 		void TacosThread::setCanID(uint32_t canID){
 			canID_ = canID;
 		}
@@ -197,7 +197,7 @@ namespace sta
 		uint32_t TacosThread::getCanID(){
 			return canID_;
 		}
-#endif // STA_CAN_BUS_ENABLE
+#endif // STA_CAN_BUS_FWD_ENABLE
 
 	}  // namespace tacos
 }  // namespace sta