diff --git a/sim/queue.cpp b/sim/queue.cpp
index 9e78ca5..accc6b5 100644
--- a/sim/queue.cpp
+++ b/sim/queue.cpp
@@ -1,7 +1,9 @@
 #include "FreeRTOS.h"
 #include "queue.h"
+#include <chrono>
 #include <stdexcept>
 #include <SDL.h>
+#include "task.h" // configTICK_RATE_HZ
 
 QueueHandle_t xQueueCreate(const UBaseType_t uxQueueLength, const UBaseType_t uxItemSize) {
   Queue_t* xQueue = new Queue_t;
@@ -9,38 +11,41 @@ QueueHandle_t xQueueCreate(const UBaseType_t uxQueueLength, const UBaseType_t ux
     throw std::runtime_error("uxItemSize must be 1");
   }
   xQueue->queue.reserve(uxQueueLength);
+  xQueue->max_length = uxQueueLength;
   return xQueue;
 }
 
 BaseType_t xQueueSend(QueueHandle_t xQueue, const void* const pvItemToQueue, TickType_t xTicksToWait) {
   Queue_t* pxQueue = (Queue_t*) xQueue;
-  std::lock_guard<std::mutex> guard(pxQueue->mutex);
+  std::unique_lock<std::mutex> lock(pxQueue->mutex);
+  auto timeout_ms = (static_cast<uint64_t>(xTicksToWait) * 1000) / configTICK_RATE_HZ;
+  if (!pxQueue->cv.wait_for(
+      lock, std::chrono::milliseconds(timeout_ms),
+      [pxQueue]{ return pxQueue->queue.size() < pxQueue->max_length; })) {
+    return false; // timeout
+  }
   pxQueue->queue.push_back(*reinterpret_cast<const uint8_t* const>(pvItemToQueue));
+  pxQueue->cv.notify_one();
   return true;
 }
 
 BaseType_t xQueueSendFromISR(QueueHandle_t xQueue, const void* const pvItemToQueue, BaseType_t* xHigherPriorityTaskWoken) {
   TickType_t xTicksToWait = 0;
   *xHigherPriorityTaskWoken = pdFALSE;
-  return xQueueSend(xQueue, pvItemToQueue, 0.0);
+  return xQueueSend(xQueue, pvItemToQueue, xTicksToWait);
 }
 
 BaseType_t xQueueReceive(QueueHandle_t xQueue, void* const pvBuffer, TickType_t xTicksToWait) {
   Queue_t* pxQueue = (Queue_t*) xQueue;
-  while (pxQueue->queue.empty()) {
-    if (xTicksToWait <= 25) {
-      return false;
-    }
-    SDL_Delay(25);
-    xTicksToWait -= 25;
-  }
-  if (pxQueue->queue.empty()) {
-    return false;
+  std::unique_lock<std::mutex> lock(pxQueue->mutex);
+  auto timeout_ms = (static_cast<uint64_t>(xTicksToWait) * 1000) / configTICK_RATE_HZ;
+  if (!pxQueue->cv.wait_for(lock, std::chrono::milliseconds(timeout_ms), [pxQueue]{ return !pxQueue->queue.empty(); })) {
+      return false; // timeout
   }
-  std::lock_guard<std::mutex> guard(pxQueue->mutex);
   uint8_t* buf = reinterpret_cast<uint8_t* const>(pvBuffer);
   *buf = pxQueue->queue.at(0);
   pxQueue->queue.erase(pxQueue->queue.begin());
+  pxQueue->cv.notify_one();
   return true;
 }
 
diff --git a/sim/queue.h b/sim/queue.h
index a0b895c..c45a53b 100644
--- a/sim/queue.h
+++ b/sim/queue.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include <condition_variable>
 #include <mutex>
 #include <vector>
 
@@ -12,13 +13,15 @@ typedef void* QueueHandle_t;
 
 struct Queue_t {
   std::mutex mutex;
+  std::condition_variable cv;
   std::vector<uint8_t> queue;
+  size_t max_length;
 
-  Queue_t() {
-  }
+  Queue_t() = default;
 
   Queue_t(const Queue_t& o) {
     queue = o.queue;
+    max_length = o.max_length;
   }
 
   Queue_t& operator=(const Queue_t& o) {