diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c
index 093b67ef72eb7cbbfdc2976c1f2b5832d89817b5..df4cd13a3180ba7fe76c125cc8a72ce89e9aaa8c 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c
@@ -5,12 +5,7 @@ struct UARTDriver create_zybo_uart(int devId) {
   uart.state = malloc(sizeof(struct ZyboUARTState));
   struct ZyboUARTState *state = uart.state;
   state->inst = malloc(sizeof(XUartPs));
-
-  // We are cheating. We put the Queue on the callback field,
-  // because that was the only way we could get the Queue
-  // into the interupt handler. And the callback field was a void
-  // pointer so we can do whatever we want with it...
-  state->inst->CallBackRef = queue_malloc(MAX_UART_BUFFER_SIZE);
+  state->queue = queue_malloc(MAX_UART_BUFFER_SIZE);
   state->devId = devId;
 
   uart.reset = zybo_uart_reset;
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c
index 3bfc6c08da35e515fdff5b6ef916b890e987a63b..67177ace77753696f8191f2b115d9c56bba9bdbd 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c
@@ -57,8 +57,7 @@ int mpu9150_read_mag(struct IMUDriver *self, gam_t* gam);
 int mpu9150_read_gyro_accel(gam_t* gam);
 
 int zybo_imu_reset(struct IMUDriver *self, gam_t *gam) {
-  // TODO: initialize gam
-	 memset(gam, 0, sizeof(gam_t));
+  memset(gam, 0, sizeof(gam_t));
 
   struct I2CDriver *i2c = self->i2c;
 
@@ -143,7 +142,7 @@ int mpu9150_write(struct I2CDriver *i2c, u8 register_addr, u8 data){
 
   // Check if within register range
   if(register_addr < 0 || register_addr > 0x75){
-    return;
+    return -1;
   }
 
   if(register_addr <= 0x12){
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 dd9a2ca17267ab3e4d9b583e8809b312d859fc84..940ef0abb03864de69dcc171a7cbe84d92ad6f43 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
@@ -4,9 +4,8 @@
 
 
 int XUartPs_SetBaudRate_ours(XUartPs *InstancePtr, u32 BaudRate);
-void uart_interrupt_handler(XUartPs *InstancePtr);
-int SetupInterruptSystem(XUartPs *UartInstancePtr, u16 UartIntrId, Xil_ExceptionHandler handler);
-void uart_interrupt_handler(XUartPs *InstancePtr);
+int SetupInterruptSystem(XUartPs *UartInstancePtr, u16 UartIntrId, Xil_ExceptionHandler handler, struct ZyboUARTState *state);
+void uart_interrupt_handler(struct ZyboUARTState *state);
 
 int zybo_uart_reset(struct UARTDriver *self) {
   // Ensure all required memory is allocated
@@ -33,7 +32,7 @@ int zybo_uart_reset(struct UARTDriver *self) {
   *uart_ctrl_reg |= 0x00000003; // clear TX & RX
 
   // Setup Interrupts
-  if (SetupInterruptSystem(inst, XPAR_PS7_UART_0_INTR, (Xil_ExceptionHandler) uart_interrupt_handler) != XST_SUCCESS) {
+  if (SetupInterruptSystem(inst, XPAR_PS7_UART_0_INTR, (Xil_ExceptionHandler) uart_interrupt_handler, state) != XST_SUCCESS) {
     return -1;
   }
 
@@ -60,16 +59,16 @@ int zybo_uart_reset(struct UARTDriver *self) {
   return 0;
 }
 
-int zybo_uart_write(struct UARTDriver *self, unsigned char c) {\
-  XUartPs *inst = self->state;
+int zybo_uart_write(struct UARTDriver *self, unsigned char c) {
+  struct ZyboUARTState *state = self->state;
+  XUartPs *inst = state->inst;
   XUartPs_SendByte(inst->Config.BaseAddress, c);
   return 0;
 }
 
 int zybo_uart_read(struct UARTDriver *self, unsigned char *c) {
   struct ZyboUARTState *state = self->state;
-  // We put the queue on the void pointer callback reference
-  struct Queue *queue = state->inst->CallBackRef;
+  struct Queue *queue = state->queue;
   if (queue_remove(queue, c)) return -1;
   else return 0;
 }
@@ -183,11 +182,10 @@ int XUartPs_SetBaudRate_ours(XUartPs *InstancePtr, u32 BaudRate)
 
 }
 
-void uart_interrupt_handler(XUartPs *InstancePtr) {
+void uart_interrupt_handler(struct ZyboUARTState *state) {
   u32 IsrStatus;
-
-  // We put the queue on the void pointer callback reference
-  struct Queue *queue = InstancePtr->CallBackRef;
+  XUartPs *InstancePtr = state->inst;
+  struct Queue *queue = state->queue;
 
   /*
    * Read the interrupt ID register to determine which
@@ -236,7 +234,7 @@ void uart_interrupt_handler(XUartPs *InstancePtr) {
 * @note		None.
 *
 ****************************************************************************/
-int SetupInterruptSystem(XUartPs *UartInstancePtr, u16 UartIntrId, Xil_ExceptionHandler handler)
+int SetupInterruptSystem(XUartPs *UartInstancePtr, u16 UartIntrId, Xil_ExceptionHandler handler, struct ZyboUARTState *state)
 {
 	int Status;
 	XScuGic xscugic;
@@ -269,7 +267,7 @@ int SetupInterruptSystem(XUartPs *UartInstancePtr, u16 UartIntrId, Xil_Exception
 	 */
 	Status = XScuGic_Connect(&xscugic, UartIntrId,
 				  handler,
-				  (void *) UartInstancePtr);
+				  (void *) state);
 	if (Status != XST_SUCCESS) {
 		return XST_FAILURE;
 	}
diff --git a/quad/xsdk_workspace/real_quad/src/main.c b/quad/xsdk_workspace/real_quad/src/main.c
index 599a602d3af15c87d09f0902c6595f949fb4b82b..ee7e493f4fe106f94aee9e14f955ee0f45f726bd 100644
--- a/quad/xsdk_workspace/real_quad/src/main.c
+++ b/quad/xsdk_workspace/real_quad/src/main.c
@@ -51,12 +51,12 @@ int main()
   //test_zybo_i2c_imu();
   //test_zybo_i2c_px4flow();
   //test_zybo_i2c_lidar();
-  test_zybo_i2c_all();
+  //test_zybo_i2c_all();
   //test_zybo_rc_receiver();
   //test_zybo_motors();
   //test_zybo_uart();
   //test_zybo_axi_timer();
-  //test_zybo_uart();
+  test_zybo_uart_comm();
   return 0;
 #endif