diff --git a/include/sta/tacos/can_bus.hpp b/include/sta/tacos/can_bus.hpp
index 21e684e..a9ffdc9 100644
--- a/include/sta/tacos/can_bus.hpp
+++ b/include/sta/tacos/can_bus.hpp
@@ -79,6 +79,11 @@ namespace sta
 			 */
 			void canCallback(uint32_t fifo);
 
+			/**
+			 *	@brief Notify the system that a message was transmitted. Used to avoid overwriting the message buffer.
+			 */
+			void txCallback();
+
 			void init() override;
 			void func() override;
 
diff --git a/src/can_bus.cpp b/src/can_bus.cpp
index 78686d3..a08a25b 100644
--- a/src/can_bus.cpp
+++ b/src/can_bus.cpp
@@ -35,7 +35,7 @@ namespace sta
 		void CanBus::func()
 		{
 			messageEvent.clear(STA_RTOS_CAN_ANY);
-			uint32_t flags = messageEvent.wait(STA_RTOS_CAN_ANY, osWaitForever);
+			uint32_t flags = messageEvent.wait(STA_RTOS_CAN_FLAG_SYS_QUEUED | STA_RTOS_CAN_FLAG_MSG_AVAIL, osWaitForever);
 
 			if (flags != static_cast<uint32_t>(osErrorTimeout))
 			{
@@ -47,6 +47,8 @@ namespace sta
 					CanSysMsg msg;
 					while (CanBus::_instance->getCanBusMsg(&msg, 0))
 					{
+						// wait for the mailbox to be free, but only for a very short time
+						messageEvent.wait(STA_RTOS_TX_MAILBOX_COMPLETE, 1);
 						canBus_.send(msg);
 					}
 				}
@@ -132,6 +134,11 @@ namespace sta
 			return (canBusSysQueue_.get(msg, timeout));
 		}
 
+		void CanBus::txCallback()
+		{
+			messageEvent.set(STA_RTOS_TX_MAILBOX_COMPLETE);
+		}
+
 		CanBus *CanBus::_instance = nullptr;
 
 		RtosEvent CanBus::messageEvent;
@@ -147,6 +154,11 @@ namespace sta
 		sta::tacos::CanBus::instance()->canCallback(fifo);
 	}
 
+	void CanBus_TxMailboxCompleteCallback()
+	{
+		sta::tacos::CanBus::instance()->txCallback();
+	}
+
 	namespace tacos
 	{
 		STA_WEAK