diff --git a/sim/queue.cpp b/sim/queue.cpp
index 2d1a564..eafa31b 100644
--- a/sim/queue.cpp
+++ b/sim/queue.cpp
@@ -47,3 +47,17 @@ BaseType_t xQueueReceive(QueueHandle_t xQueue, void * const pvBuffer, TickType_t
   pxQueue->queue.erase(pxQueue->queue.begin());
   return true;
 }
+
+UBaseType_t uxQueueMessagesWaiting(const QueueHandle_t xQueue)
+{
+  UBaseType_t uxReturn;
+  SDL_assert(xQueue);
+  Queue_t* pxQueue = ( Queue_t * ) xQueue;
+  // taskENTER_CRITICAL();
+  {
+    std::lock_guard<std::mutex> guard(pxQueue->mutex);
+    uxReturn = static_cast<UBaseType_t>(pxQueue->queue.size());
+  }
+  // taskEXIT_CRITICAL();
+  return uxReturn;
+}
diff --git a/sim/queue.h b/sim/queue.h
index 7f38c9e..81a57eb 100644
--- a/sim/queue.h
+++ b/sim/queue.h
@@ -27,3 +27,4 @@ QueueHandle_t xQueueCreate(const UBaseType_t uxQueueLength, const UBaseType_t ux
 BaseType_t xQueueSend(QueueHandle_t xQueue, const void * const pvItemToQueue, TickType_t xTicksToWait);
 BaseType_t xQueueSendFromISR(QueueHandle_t xQueue, const void * const pvItemToQueue, BaseType_t *xHigherPriorityTaskWoken);
 BaseType_t xQueueReceive(QueueHandle_t xQueue, void * const pvBuffer, TickType_t xTicksToWait );
+UBaseType_t uxQueueMessagesWaiting(const QueueHandle_t xQueue);