diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_uart.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_uart.c
index 18c17af0a045f70686b001c0de488eacd6efbeaf..0baaab96cd8ccea7597396b321ee4060336f6934 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_uart.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_uart.c
@@ -8,22 +8,28 @@ void uart_interrupt_handler(XUartPs *InstancePtr);
 int SetupInterruptSystem(XUartPs *UartInstancePtr, u16 UartIntrId, Xil_ExceptionHandler handler);
 void uart_interrupt_handler(XUartPs *InstancePtr);
 
-// Queue has to be global to be accessible to ISR
-static struct Queue queue;
-static volatile unsigned char buff[MAX_UART_BUFFER_SIZE];
-static XScuGic xscugic;
+struct UARTState {
+  struct Queue *queue;
+  XUartPs *inst;
+};
 
 int zybo_uart_reset(struct UARTDriver *self) {
   // Instantiate the Driver state
   if (self->state == NULL) {
-    self->state = malloc(sizeof(XUartPs));
-    if (self->state == NULL) {
-      return -1;
-    }
-    queue_init(&queue, buff, MAX_UART_BUFFER_SIZE);
+    self->state = malloc(sizeof(struct UARTState));
+    if (self->state == NULL) return -1;
+    self->state->inst = malloc(sizeof(XUartPs));
+    if (self->state->inst == NULL) return -1;
+
+    // We are cheating. We put the Queue on the callback field,
+    // because that was the only way we could get the Queue
+    // into this interupt handler. And the callback field was a void
+    // pointer so we can do whatever we want with it...
+    self->state->inst->CallBackRef = queue_malloc(MAX_UART_BUFFER_SIZE);
+    if (self->state->inst->CallBackRef == NULL) return -1;
   }
 
-  XUartPs *inst = self->state;;
+  XUartPs *inst = self->state->inst;
 
   // Configure XUartPs instance
   XUartPs_Config* config = XUartPs_LookupConfig(XPAR_PS7_UART_0_DEVICE_ID);
@@ -75,7 +81,9 @@ int zybo_uart_write(struct UARTDriver *self, unsigned char c) {\
 }
 
 int zybo_uart_read(struct UARTDriver *self, unsigned char *c) {
-  if (queue_remove(&queue, c)) return -1;
+  // We put the queue on the void pointer callback reference
+  struct Queue *queue = InstancePtr->CallBackRef;
+  if (queue_remove(queue, c)) return -1;
   else return 0;
 }
 
@@ -191,6 +199,9 @@ int XUartPs_SetBaudRate_ours(XUartPs *InstancePtr, u32 BaudRate)
 void uart_interrupt_handler(XUartPs *InstancePtr) {
   u32 IsrStatus;
 
+  // We put the queue on the void pointer callback reference
+  struct Queue *queue = InstancePtr->CallBackRef;
+
   /*
    * Read the interrupt ID register to determine which
    * interrupt is active
@@ -211,7 +222,7 @@ void uart_interrupt_handler(XUartPs *InstancePtr) {
 
   while (0 == (CsrRegister & XUARTPS_SR_RXEMPTY)) {
     u8 byte = XUartPs_ReadReg(InstancePtr->Config.BaseAddress, XUARTPS_FIFO_OFFSET);
-    queue_add(&queue, byte);
+    queue_add(queue, byte);
     CsrRegister = XUartPs_ReadReg(InstancePtr->Config.BaseAddress, XUARTPS_SR_OFFSET);
   }
 
@@ -241,7 +252,7 @@ void uart_interrupt_handler(XUartPs *InstancePtr) {
 int SetupInterruptSystem(XUartPs *UartInstancePtr, u16 UartIntrId, Xil_ExceptionHandler handler)
 {
 	int Status;
-
+	XScuGic xscugic;
 	XScuGic_Config *IntcConfig; /* Config for interrupt controller */
 
 	/* Initialize the interrupt controller driver */