diff --git a/software/firmware/src/peripherals/include/imu.h b/software/firmware/src/peripherals/include/imu.h
index e629e8f1..1a7d1472 100644
--- a/software/firmware/src/peripherals/include/imu.h
+++ b/software/firmware/src/peripherals/include/imu.h
@@ -12,6 +12,7 @@
 #define BNO055_ID 0xA0
 
 typedef void (*motion_change_callback_t)(bool in_motion);
+typedef void (*data_ready_callback_t)(uint8_t interrupt_status);
 
 typedef enum
 {
@@ -272,6 +273,7 @@ typedef struct
 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_register_data_ready_callback(data_ready_callback_t callback);
 void imu_set_power_mode(bno055_powermode_t power_mode);
 void imu_read_accel_data(bno055_acc_t *acc);
 void imu_read_linear_accel_data(bno055_acc_t *acc);
diff --git a/software/firmware/src/peripherals/src/imu.c b/software/firmware/src/peripherals/src/imu.c
index 039a92d1..7a0c0012 100644
--- a/software/firmware/src/peripherals/src/imu.c
+++ b/software/firmware/src/peripherals/src/imu.c
@@ -3,6 +3,7 @@
 #include "imu.h"
 #include "math.h"
 #include "system.h"
+#include "logging.h"
 
 
 // Static Global Variables ---------------------------------------------------------------------------------------------
@@ -10,6 +11,7 @@
 static void *i2c_handle;
 static volatile bool previously_in_motion;
 static motion_change_callback_t motion_change_callback;
+static data_ready_callback_t data_ready_callback;
 
 
 // Private Helper Functions --------------------------------------------------------------------------------------------
@@ -82,11 +84,18 @@ static void i2c_read(uint8_t reg_number, uint8_t *read_buffer, uint32_t buffer_l
 static void imu_isr(void *args)
 {
    // Read the device motion status and trigger the registered callback
-   const bool in_motion = i2c_read8(BNO055_INTR_STAT_ADDR) & 0x40;
-   i2c_write8(BNO055_SYS_TRIGGER_ADDR, 0xC0);
+   const uint8_t interrupt_status = i2c_read8(BNO055_INTR_STAT_ADDR);
+   const bool in_motion = interrupt_status & 0x40;
+
    if (in_motion != previously_in_motion)
       motion_change_callback(in_motion);
    previously_in_motion = in_motion;
+   if (interrupt_status && (0x01 | 0x02 | 0x10))
+   {
+      if (data_ready_callback!=NULL)
+         data_ready_callback(interrupt_status);
+   }
+   i2c_write8(BNO055_SYS_TRIGGER_ADDR, 0xC0);//reset all interrupt status bits, and INT output
 }
 
 static void read_int16_vector(uint8_t reg_number, int16_t *read_buffer, uint32_t byte_count){
@@ -141,16 +150,26 @@ static void enable_motion_interrupts(void)
 {
    // Set up interrupts for motion and non-motion events
    i2c_write8(BNO055_PAGE_ID_ADDR, 1);
-   i2c_write8(ACC_CONFIG, 0b00000000);
+   i2c_write8(ACC_CONFIG, 0b00000000); //operation mode/bandwidth/g range
    i2c_write8(ACC_AM_THRE, 0b00001010);
    i2c_write8(ACC_NM_THRE, 0b00001010);
    i2c_write8(ACC_NM_SET, 0b00001001);
-   i2c_write8(ACC_INT_SET, 0b00011111);
-   i2c_write8(INT_MSK, 0xC0);
-   i2c_write8(INT_EN, 0xC0);
+   i2c_write8(ACC_INT_SET, 0b00011111); //axis and duration for triggering motion interrupt
+   i2c_write8(INT_MSK, 0b11000000); //11010011
+   i2c_write8(INT_EN, 0b11000000);
    i2c_write8(BNO055_PAGE_ID_ADDR, 0);
 }
 
+static void enable_data_ready_interrupts(void)
+{
+  i2c_write8(BNO055_PAGE_ID_ADDR, 1);
+  uint8_t int_msk = i2c_read8(INT_MSK)|0b00010011;
+  uint8_t int_en = i2c_read8(INT_EN)|0b00010011;
+  i2c_write8(INT_MSK, int_msk); 
+  i2c_write8(INT_EN, int_en);
+  i2c_write8(BNO055_PAGE_ID_ADDR, 0);
+}
+
 // Math helper functions -----------------------------------------------------------------------------------------------
 void quaternion_to_euler(bno055_quaternion_t quaternion, bno055_euler_t *euler)
 {
@@ -171,6 +190,7 @@ void imu_init(void)
    // Initialize static variables
    previously_in_motion = false;
    motion_change_callback = NULL;
+   data_ready_callback = NULL;
 
    // Create an I2C configuration structure
    const am_hal_iom_config_t i2c_config =
@@ -249,6 +269,11 @@ void imu_register_motion_change_callback(motion_change_callback_t callback, bno0
    set_mode(mode);
 }
 
+void imu_register_data_ready_callback(data_ready_callback_t callback){
+   data_ready_callback = callback;
+   enable_data_ready_interrupts();
+}
+
 void imu_set_power_mode(bno055_powermode_t power_mode)
 {
    bno055_opmode_t saved_mode = get_mode();
diff --git a/software/firmware/tests/peripherals/test_imu.c b/software/firmware/tests/peripherals/test_imu.c
index b6d773f5..f30a7bc9 100644
--- a/software/firmware/tests/peripherals/test_imu.c
+++ b/software/firmware/tests/peripherals/test_imu.c
@@ -7,6 +7,36 @@ static void motion_interrupt(bool in_motion)
    print("Device is %s\n", in_motion ? "IN MOTION" : "STATIONARY");
 }
 
+static void read_data(uint8_t interrupt_status)
+{
+   const bool acc_fusion_data_ready = interrupt_status & 0x01;//ACC_BSX_DRDY
+   //const bool mag_data_ready = interrupt_status & 0x02;//MAG_DRDY
+   //const bool gyro_data_ready = interrupt_status & 0x10;//GYR_DRDY
+   if (acc_fusion_data_ready)
+   {
+      int8_t temp;
+      bno55_calib_status_t status = {0};
+      bno055_calib_offsets_t offsets = {0};
+      bno055_axis_remap_t remap = {0};
+      bno055_quaternion_t quaternion = {0};
+      bno055_euler_t euler = {0};
+      bno055_euler_t device_calculated_euler = {0};
+      bno055_acc_t acc = {0};
+      bno055_gyro_t gyro = {0};
+      imu_read_calibration_status(&status);
+      //imu_read_calibration_offsets(&offsets);
+      imu_read_linear_accel_data(&acc);
+      imu_read_quaternion_data(&quaternion);
+      quaternion_to_euler(quaternion, &euler);
+      imu_read_gyro_data(&gyro); 
+      //imu_read_temp(&temp);
+      //print("temp:%d\n", (int32_t)temp);
+      print("Calibration status: sys %u, gyro %u, accel %u, mag %u\n",status.sys, status.gyro, status.accel, status.mag);
+      //print("Calibration offsets: %d, %d, %d \n", offsets.gyro_offset_x, offsets.gyro_offset_y, offsets.gyro_offset_z);
+      print("Linear Accel X = %d, Y = %d, Z = %d, qw = %d, qx = %d, qy = %d, qz = %d, gx = %d, gy = %d, gz = %d\n", acc.x, acc.y, acc.z, quaternion.w, quaternion.x, quaternion.y, quaternion.z, gyro.x, gyro.y,gyro.z);
+   }
+}
+
 int main(void)
 {
    // Set up system hardware
@@ -14,60 +44,23 @@ int main(void)
    imu_init();
    system_enable_interrupts(true);
 
-   // Loop forever, waiting for IMU interrupts
-   int16_t x, y, z;
-   int8_t temp;
    uint8_t rev_msb, rev_lsb;
-   bno55_calib_status_t status = {0};
-   bno055_calib_offsets_t offsets = {0};
    //bno055_axis_remap_t remap = {.x_remap_val = 1, .y_remap_val = 0, .z_remap_val = 2};
    bno055_axis_remap_t remap = {0};
-   bno055_quaternion_t quaternion = {0};
-   bno055_euler_t euler = {0};
-   bno055_euler_t device_calculated_euler = {0};
-   bno055_acc_t acc = {0};
-   bno055_gyro_t gyro = {0};
 
    imu_register_motion_change_callback(motion_interrupt, OPERATION_MODE_NDOF);
+   //imu_register_data_ready_callback(read_data);
    imu_set_power_mode(POWER_MODE_NORMAL);
 
    imu_read_fw_version(&rev_msb, &rev_lsb);
-   print("BNO055 firmware version:%u.%u\n",rev_msb, rev_lsb);
+   print("BNO055 firmware version:%x.%x\n",rev_msb, rev_lsb);
 
    //imu_read_axis_remap(&remap);
-   if (imu_set_axis_remap(remap)){print("remap success!\n");}
+   //if (imu_set_axis_remap(remap)){print("remap success!\n");}
    //print("BNO055 X mapping:%u, Y mapping:%u, Z mapping:%u, X sign:%u, Y sign:%u, Z sign:%u\n", remap.x_remap_val, remap.y_remap_val, remap.z_remap_val, remap.x_remap_sign, remap.y_remap_sign, remap.z_remap_sign);
 
-   while (true)
-   {
-      am_hal_delay_us(20000);
-      //0: not calibrated; 3: fully calibrated 
-      imu_read_calibration_status(&status);
-      print("Calibration status: sys %u, gyro %u, accel %u, mag %u\n",status.sys, status.gyro, status.accel, status.mag);
-      //imu_read_calibration_offsets(&offsets);
-      //print("Calibration offsets: %d, %d, %d \n", offsets.gyro_offset_x, offsets.gyro_offset_y, offsets.gyro_offset_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);
-      memset(&acc, 0, sizeof(acc));
-      memset(&quaternion, 0, sizeof(quaternion));
-      memset(&euler, 0, sizeof(euler));
-      //memset(&device_calculated_euler, 0 , sizeof(device_calculated_euler));
-
-      imu_read_linear_accel_data(&acc);
-      imu_read_quaternion_data(&quaternion);
-      quaternion_to_euler(quaternion, &euler);
-	  imu_read_gyro_data(&gyro);
-      //imu_read_euler_data(&device_calculated_euler);
-      print("Linear Accel X = %d, Y = %d, Z = %d, qw = %d, qx = %d, qy = %d, qz = %d, gx = %d, gy = %d, gz = %d\n", acc.x, acc.y, acc.z, quaternion.w, quaternion.x, quaternion.y, quaternion.z, gyro.x, gyro.y,gyro.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);
-      //print("temp:%d\n", (int32_t)temp);
-
-
-
+   while (true){
+	   ;
    }
 
    // Should never reach this point