diff --git a/quad/src/quad_app/initialize_components.c b/quad/src/quad_app/initialize_components.c
index 3ef3451500ad1bd2fbb2478c5acdc9577511040c..fa09ea2cfa2e78efc134383aa92033b6235fe7aa 100644
--- a/quad/src/quad_app/initialize_components.c
+++ b/quad/src/quad_app/initialize_components.c
@@ -67,7 +67,7 @@ int init_structs(modular_structs_t *structs) {
     return -1;
   }
 
-  // Initialize I2C controller and start the sensor board
+  // Initialize I2C controller
   struct I2CDriver *i2c = &structs->hardware_struct.i2c;
   if (i2c->reset(i2c)) {
     return -1;
@@ -85,11 +85,9 @@ int init_structs(modular_structs_t *structs) {
   //manual flight mode
   structs->user_defined_struct.flight_mode = MANUAL_FLIGHT_MODE;
 
-#ifndef BENCH_TEST
   // Get the first loop data from accelerometer for the gyroscope to use
   if(sensor_init(&structs->raw_sensor_struct, &structs->sensor_struct) == -1)
     return -1;
-#endif
 
   return 0;
 }
diff --git a/quad/xsdk_workspace/modular_quad_pid/src/hw_impl_zybo_tests.c b/quad/xsdk_workspace/modular_quad_pid/src/hw_impl_zybo_tests.c
index cf36cb0afeab045c36d06747b7c9d95c716962cc..8fa79cbb283532fed1a65e8c37a80d1c4c489e28 100644
--- a/quad/xsdk_workspace/modular_quad_pid/src/hw_impl_zybo_tests.c
+++ b/quad/xsdk_workspace/modular_quad_pid/src/hw_impl_zybo_tests.c
@@ -70,6 +70,22 @@ int test_zybo_i2c() {
   return 0;
 }
 
+int test_zybo_i2c_imu() {
+  struct I2CDriver i2c = create_zybo_i2c();
+  struct SystemDriver sys = create_zybo_system();
+  i2c.reset(&i2c);
+  sys.reset(&sys);
+
+  gam_t gam = { };
+  iic_set_globals(&i2c, &sys);
+  if (iic0_mpu9150_start()) return 0;
+  short x;
+  while (1) {
+    iic0_mpu9150_read_gam(&gam);
+  }
+  return 0;
+}
+
 /**
  * Test for the PWMInputDriver.
  *
diff --git a/quad/xsdk_workspace/modular_quad_pid/src/main.c b/quad/xsdk_workspace/modular_quad_pid/src/main.c
index fe31f0fb683d269ac188b3a0e06c55c23b7d5d11..337212a68b5c4829b95de861b9a97f08d4bb93be 100644
--- a/quad/xsdk_workspace/modular_quad_pid/src/main.c
+++ b/quad/xsdk_workspace/modular_quad_pid/src/main.c
@@ -26,6 +26,7 @@ int main()
 #ifdef RUN_TESTS
   //test_zybo_mio7_led_and_system();
   //test_zybo_i2c();
+  //test_zybo_i2c_imu();
   //test_zybo_pwm_inputs();
   //test_zybo_pwm_outputs();
   //test_zybo_uart();