diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c
index 34ea395e787bbdd8b5307718b3cfa49b8c088a9d..85bf4f0e06563a53965d10ae7bc45e012f8b12ee 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_i2c.c
@@ -80,7 +80,8 @@ int zybo_i2c_write(struct I2CDriver *self,
                    unsigned short device_addr,
                    unsigned char *data,
                    unsigned int length) {
-  XIicPs *inst = self->state;
+  struct ZyboI2CState *state = self->state;
+  XIicPs *inst = state->inst;
   if (device_addr == PX4FLOW_DEVICE_ADDR) {
 	  // If we are sending a request to optical flow, drop down to 100kHz
 	  XIicPs_SetSClk(inst, 100000);
@@ -98,7 +99,8 @@ int zybo_i2c_read(struct I2CDriver *self,
                   unsigned short device_addr,
                   unsigned char *buff,
                   unsigned int length) {
-  XIicPs *inst = self->state;
+  struct ZyboI2CState *state = self->state;
+  XIicPs *inst = state->inst;
   if (device_addr == PX4FLOW_DEVICE_ADDR) {
 	  // If we are sending a request to optical flow, drop down to 100kHz
 	  XIicPs_SetSClk(inst, 100000);
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c
index bf7310ea5173078673971742da1b68a6f6d3dc56..a38e87306620ba54d308d61cd450cf7e817614cc 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c
@@ -65,9 +65,9 @@ int test_zybo_i2c() {
 int test_zybo_i2c_imu() {
   struct I2CDriver i2c = create_zybo_i2c(0);
   struct IMUDriver imu = create_zybo_imu(&i2c);
-  i2c.reset(&i2c);
 
   gam_t gam;
+  if (i2c.reset(&i2c)) return 0;
   if (imu.reset(&imu, &gam)) return 0;
 
   int status = 0;
diff --git a/quad/xsdk_workspace/real_quad/src/main.c b/quad/xsdk_workspace/real_quad/src/main.c
index ee7e493f4fe106f94aee9e14f955ee0f45f726bd..85ab2332ad13a372952c8d82508be08374b13209 100644
--- a/quad/xsdk_workspace/real_quad/src/main.c
+++ b/quad/xsdk_workspace/real_quad/src/main.c
@@ -48,7 +48,7 @@ int main()
 #ifdef RUN_TESTS
   //test_zybo_mio7_led_and_system();
   //test_zybo_i2c();
-  //test_zybo_i2c_imu();
+  test_zybo_i2c_imu();
   //test_zybo_i2c_px4flow();
   //test_zybo_i2c_lidar();
   //test_zybo_i2c_all();
@@ -56,7 +56,7 @@ int main()
   //test_zybo_motors();
   //test_zybo_uart();
   //test_zybo_axi_timer();
-  test_zybo_uart_comm();
+  //test_zybo_uart_comm();
   return 0;
 #endif