diff --git a/software/firmware/src/peripherals/include/imu.h b/software/firmware/src/peripherals/include/imu.h
index 82279c26..76c05a01 100644
--- a/software/firmware/src/peripherals/include/imu.h
+++ b/software/firmware/src/peripherals/include/imu.h
@@ -230,6 +230,12 @@ typedef struct {
    int16_t y;
    int16_t z;
 } bno055_quaternion_t;
+
+typedef struct {
+   int16_t x;
+   int16_t y;
+   int16_t z;
+} bno055_acc_t;
 // Peripheral Type Definitions -----------------------------------------------------------------------------------------
 
 typedef void (*motion_change_callback_t)(bool in_motion);
@@ -240,9 +246,9 @@ typedef void (*motion_change_callback_t)(bool in_motion);
 void imu_init(void);
 void imu_deinit(void);
 void imu_register_motion_change_callback(motion_change_callback_t callback, bno055_opmode_t mode);
-void imu_read_accel_data(int16_t *x, int16_t *y, int16_t *z);
-void imu_read_linear_accel_data(int16_t *x, int16_t *y, int16_t *z);
-void imu_read_gravity_accel_data(int16_t *x, int16_t *y, int16_t *z);
+void imu_read_accel_data(bno055_acc_t *acc);
+void imu_read_linear_accel_data(bno055_acc_t *acc);
+void imu_read_gravity_accel_data(bno055_acc_t *acc);
 void imu_read_quaternion_data(bno055_quaternion_t *quaternion);
 void imu_read_gyro_data(int16_t *x, int16_t *y, int16_t *z);
 void imu_read_temp(int8_t *temp);
diff --git a/software/firmware/src/peripherals/src/imu.c b/software/firmware/src/peripherals/src/imu.c
index 028e2f5b..687496c6 100644
--- a/software/firmware/src/peripherals/src/imu.c
+++ b/software/firmware/src/peripherals/src/imu.c
@@ -234,29 +234,29 @@ void imu_register_motion_change_callback(motion_change_callback_t callback, bno0
    set_mode(mode);
 }
 
-void imu_read_accel_data(int16_t *x, int16_t *y, int16_t *z)
+void imu_read_accel_data(bno055_acc_t *acc)
 {
    static int16_t accel_data[3];
-   i2c_read(BNO055_ACCEL_DATA_X_LSB_ADDR, (uint8_t*)accel_data, sizeof(accel_data));
-   *x = (int16_t)(accel_data[0] << 2) / 4;
-   *y = (int16_t)(accel_data[1] << 2) / 4;
-   *z = (int16_t)(accel_data[2] << 2) / 4;
+   read_int16_vector(BNO055_ACCEL_DATA_X_LSB_ADDR, accel_data, sizeof(accel_data));
+   acc->x = accel_data[0];
+   acc->y = accel_data[1];
+   acc->z = accel_data[2];
 }
 
-void imu_read_linear_accel_data(int16_t *x, int16_t *y, int16_t *z){
+void imu_read_linear_accel_data(bno055_acc_t *acc){
    static int16_t accel_data[3];
-   i2c_read(BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR, (uint8_t*)accel_data, sizeof(accel_data));
-   *x = (int16_t)(accel_data[0] << 2) / 4;
-   *y = (int16_t)(accel_data[1] << 2) / 4;
-   *z = (int16_t)(accel_data[2] << 2) / 4;
+   read_int16_vector(BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR, accel_data, sizeof(accel_data));
+   acc->x = accel_data[0];
+   acc->y = accel_data[1];
+   acc->z = accel_data[2];
 }
 
-void imu_read_gravity_accel_data(int16_t *x, int16_t *y, int16_t *z){
+void imu_read_gravity_accel_data(bno055_acc_t *acc){
    static int16_t accel_data[3];
-   i2c_read(BNO055_GRAVITY_DATA_X_LSB_ADDR, (uint8_t*)accel_data, sizeof(accel_data));
-   *x = (int16_t)(accel_data[0] << 2) / 4;
-   *y = (int16_t)(accel_data[1] << 2) / 4;
-   *z = (int16_t)(accel_data[2] << 2) / 4;
+   read_int16_vector(BNO055_GRAVITY_DATA_X_LSB_ADDR, accel_data, sizeof(accel_data));
+   acc->x = accel_data[0];
+   acc->y = accel_data[1];
+   acc->z = accel_data[2];
 }
 
 void imu_read_quaternion_data(bno055_quaternion_t *quaternion){
diff --git a/software/firmware/tests/peripherals/test_imu.c b/software/firmware/tests/peripherals/test_imu.c
index 3e017f5c..b1ba820d 100644
--- a/software/firmware/tests/peripherals/test_imu.c
+++ b/software/firmware/tests/peripherals/test_imu.c
@@ -22,6 +22,7 @@ int main(void)
    bno055_calib_offsets_t offsets;
    bno055_axis_remap_t remap;
    bno055_quaternion_t quaternion;
+   bno055_acc_t acc;
 
    imu_register_motion_change_callback(motion_interrupt, OPERATION_MODE_NDOF);
 
@@ -34,14 +35,13 @@ int main(void)
    while (true)
    {
       am_hal_delay_us(20000);
-      //imu_read_accel_data(&x, &y, &z);
-      //print("Accel X = %d, Y = %d, Z = %d\n", (int32_t)x, (int32_t)y, (int32_t)z);
-      imu_read_linear_accel_data(&x, &y, &z);
-	  imu_read_quaternion_data(&quaternion);
-      print("Linear Accel X = %d, Y = %d, Z = %d, qw = %d, qx = %d, qy = %d, qz = %d\n", (int32_t)x, (int32_t)y, (int32_t)z, (int32_t)quaternion.w, (int32_t)quaternion.x, (int32_t)quaternion.y, (int32_t)quaternion.z);
-      //imu_read_gravity_accel_data(&x, &y, &z);
-      //print("Gravity Accel X = %d, Y = %d, Z = %d\n", (int32_t)x, (int32_t)y, (int32_t)z);
-      //print("Quaternion W = %d, X = %d, Y = %d, Z = %d\n", (int32_t)w, (int32_t)x, (int32_t)y, (int32_t)z);
+      //imu_read_accel_data(&acc);
+      //print("Accel X = %d, Y = %d, Z = %d\n", (int32_t)acc.x, (int32_t)acc.y, (int32_t)acc.z);
+      imu_read_linear_accel_data(&acc);
+      imu_read_quaternion_data(&quaternion);
+      print("Linear Accel X = %d, Y = %d, Z = %d, qw = %d, qx = %d, qy = %d, qz = %d\n", (int32_t)acc.x, (int32_t)acc.y, (int32_t)acc.z, (int32_t)quaternion.w, (int32_t)quaternion.x, (int32_t)quaternion.y, (int32_t)quaternion.z);
+      //imu_read_gravity_accel_data(&acc);
+      //print("Gravity Accel X = %d, Y = %d, Z = %d\n", (int32_t)acc.x, (int32_t)acc.y, (int32_t)acc.z);
       //imu_read_gyro_data(&x, &y, &z);
       //print("gyro 1 = %d, gyro 2 = %d, gyro 3 = %d\n", (int32_t)x, (int32_t)y, (int32_t)z);
       //imu_read_temp(&temp);