diff --git a/include/sta/tacos/can_bus.hpp b/include/sta/tacos/can_bus.hpp
index 4762098..e265109 100644
--- a/include/sta/tacos/can_bus.hpp
+++ b/include/sta/tacos/can_bus.hpp
@@ -75,6 +75,7 @@ namespace sta
 			 * @return              True if message was retrieved successfully
 			 */
 			bool getCanBusMsg(CanDataMsg * msg, uint32_t timeout);
+
 			/**
 			 * @brief Retrieve system message from CAN driver TX queue.
 			 *
@@ -84,6 +85,13 @@ namespace sta
 			 */
 			bool getCanBusMsg(CanSysMsg * msg, uint32_t timeout);
 
+			/**
+			 *	@brief Buffers incoming message and sets event to notify the system.
+			 *
+			 *	@param fifo		FIFO number of the received message.
+			 */
+			void canCallback(uint32_t fifo);
+
 			void init() override;
 			void func() override;
 
@@ -105,8 +113,9 @@ namespace sta
 
 			sta::STM32CanController * canBusController_;
 
-            CanDataMsg canBusDataQueueBuffer_[STA_RTOS_CAN_BUS_QUEUE_LENGTH];
-            CanSysMsg canBusSysQueueBuffer_[STA_RTOS_CAN_BUS_QUEUE_LENGTH];
+            //CanDataMsg canBusDataQueueBuffer_[STA_RTOS_CAN_BUS_QUEUE_LENGTH];
+            CanSysMsg* canBusSysQueueBuffer_[STA_RTOS_CAN_BUS_QUEUE_LENGTH];
+            uint8_t bufferIndex;
 
             RtosQueue<CanSysMsg> canBusSysQueue_;
             RtosQueue<CanDataMsg> canBusDataQueue_;
diff --git a/src/can_bus.cpp b/src/can_bus.cpp
index e6213e3..e26a8b2 100644
--- a/src/can_bus.cpp
+++ b/src/can_bus.cpp
@@ -18,17 +18,27 @@ namespace sta
 			  canBusDataQueue_(STA_RTOS_CAN_BUS_QUEUE_LENGTH),
               canBus_{canBusController_, HAL_GetTick, sta::tacos::handleSysMessage, sta::tacos::handleDataMessage}
 		{
+			bufferIndex = 0;
+			for(int i = 0; i < STA_RTOS_CAN_BUS_QUEUE_LENGTH; i++){
+				canBusSysQueueBuffer_[i] = nullptr;
+			}
 		}
 
         void CanBus::init()
 	    {
 		    canBusController_->start();
+
+		    if (HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK ||
+		    		HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO1_MSG_PENDING))
+		    {
+		      	 Error_Handler();
+		    }
 	    }
 
         void CanBus::func()
         {
         	messageEvent.clear(STA_RTOS_CAN_ANY);
-            uint32_t flags = messageEvent.wait(STA_RTOS_CAN_ANY, 500);
+            uint32_t flags = messageEvent.wait(STA_RTOS_CAN_ANY, osWaitForever);
 
             if (flags != static_cast<uint32_t>(osErrorTimeout))
             {
@@ -46,8 +56,22 @@ namespace sta
 
                 if (flags & STA_RTOS_CAN_FLAG_MSG_AVAIL)
                 {
-                    STA_DEBUG_PRINTLN("[event] CAN INT");
+                    CanSysMsg sysMsg;
+                    // Iterate through buffer and set back to nullptr after use
+                    for(int i = 0; i < STA_RTOS_CAN_BUS_QUEUE_LENGTH; i++){
+                    	if(canBusSysQueueBuffer_[i] != nullptr){
+                    		sysMsg = *canBusSysQueueBuffer_[i];
+                    		canBusSysQueueBuffer_[i] = nullptr;
 
+                    		// Append to the correct thread's queue
+							for (std::shared_ptr<TacosThread> thread : Manager::instance()->getActiveThreads()){
+								if (thread->getCanID() == sysMsg.header.sid){
+									thread->CAN_queue_.put(sysMsg);
+									break;
+								}
+							}
+						}
+                    }
                     //canBus_.processRx();
                 }
 
@@ -57,43 +81,6 @@ namespace sta
                 }
             }
 
-			CanRxHeader rxHeader;					//CAN Bus Receive Header
-			uint8_t canRX[8] = {0,0,0,0,0,0,0,0};  	//CAN Bus Receive Buffer
-
-			bool received_ = canBusController_->receiveFrame(CAN_RX_FIFO0, &rxHeader, canRX);
-			if (received_){
-				// Create a CANSysMsg from the received data
-				CanSysMsg sysMsg;
-				sysMsg.header.sid = rxHeader.id.sid;
-				sysMsg.header.payloadLength = rxHeader.payloadLength;
-				std::memcpy(sysMsg.payload, canRX, rxHeader.payloadLength);
-
-				// Append to the correct thread's queue
-				for (std::shared_ptr<TacosThread> thread : Manager::instance()->getActiveThreads()){
-					if (thread->getCanID() == rxHeader.id.sid){
-						thread->CAN_queue_.put(sysMsg);
-						break;
-					}
-				}
-			}
-			received_ = canBusController_->receiveFrame(CAN_RX_FIFO1, &rxHeader, canRX);
-			if (received_){
-				// Create a CANSysMsg from the received data
-				CanSysMsg sysMsg;
-				sysMsg.header.sid = rxHeader.id.sid;
-				sysMsg.header.payloadLength = rxHeader.payloadLength;
-				sysMsg.header.format = 0;
-				std::memcpy(sysMsg.payload, canRX, rxHeader.payloadLength);
-
-				// Append to the correct thread's queue
-				for (std::shared_ptr<TacosThread> thread : Manager::instance()->getActiveThreads()){
-					if (thread->getCanID() == rxHeader.id.sid){
-						thread->CAN_queue_.put(sysMsg);
-						break;
-					}
-				}
-			}
-
         }
 
         bool CanBus::queueCanBusMsg(const CanDataMsg& msg, uint32_t timeout)
@@ -116,7 +103,7 @@ namespace sta
 
 		bool CanBus::queueCanBusMsg(const CanSysMsg& msg, uint32_t timeout)
 		{
-			// This technicall should check if we are using a system message, but we just pretending that everything is one of those rn
+			// This technically should check if we are using a system message, but we just pretending that everything is one of those rn
 			//STA_ASSERT((msg.header.sid & ~STA_CAN_SID_SYS_BITS) == 0);
 
 			if (canBusSysQueue_.put(msg, timeout))
@@ -132,6 +119,29 @@ namespace sta
 			}
 		}
 
+		void CanBus::canCallback(uint32_t fifo){
+			if(messageEvent.get() != STA_RTOS_CAN_FLAG_MSG_AVAIL){
+				// get here does not work since FreeRTOS is a buggy mess
+				messageEvent.set(STA_RTOS_CAN_FLAG_MSG_AVAIL);
+
+				CanRxHeader rxHeader;					//CAN Bus Receive Header
+				uint8_t canRX[8] = {0,0,0,0,0,0,0,0};  	//CAN Bus Receive Buffer
+
+				bool received_ = canBusController_->receiveFrame(fifo, &rxHeader, canRX);
+
+				if(received_){
+					CanSysMsg sysMsg;
+					sysMsg.header.sid = rxHeader.id.sid;
+					sysMsg.header.payloadLength = rxHeader.payloadLength;
+					std::memcpy(sysMsg.payload, canRX, rxHeader.payloadLength);
+
+					canBusSysQueueBuffer_[bufferIndex] = &sysMsg;
+
+					bufferIndex++;
+					if (bufferIndex >= STA_RTOS_CAN_BUS_QUEUE_LENGTH) bufferIndex = 0;
+				}
+			}
+		}
 
 		bool CanBus::getCanBusMsg(CanDataMsg * msg, uint32_t timeout)
 		{
@@ -150,7 +160,13 @@ namespace sta
     } /* namespace tacos */
 } /* namespace sta */
 
+
 namespace sta {
+
+	void CanBus_RxPendingCallback(uint32_t fifo){
+			sta::tacos::CanBus::instance()->canCallback(fifo);
+	}
+
 	namespace tacos
 	{
 		void handleSysMessage(const sta::CanRxHeader & header, const uint8_t * payload)