diff --git a/software/firmware/src/peripherals/include/imu.h b/software/firmware/src/peripherals/include/imu.h
index 09b71a17..91b89ef7 100644
--- a/software/firmware/src/peripherals/include/imu.h
+++ b/software/firmware/src/peripherals/include/imu.h
@@ -189,6 +189,14 @@ typedef enum {
    POWER_MODE_SUSPEND = 0X02
 } bno055_powermode_t;
 
+typedef struct
+{
+    int sys;
+    int gyro;
+    int accel;
+    int mag;
+} bno55_calib_status_t;
+
 // Peripheral Type Definitions -----------------------------------------------------------------------------------------
 
 typedef void (*motion_change_callback_t)(bool in_motion);
@@ -206,5 +214,5 @@ void imu_read_quaternion_data(int16_t *w, int16_t *x, int16_t *y, int16_t *z);
 void imu_read_gyro_data(int16_t *x, int16_t *y, int16_t *z);
 void imu_read_temp(int8_t *temp);
 void imu_read_fw_version(uint8_t *msb, uint8_t *lsb);
-
+void imu_read_calibration_status(bno55_calib_status_t *status);
 #endif  // #ifndef __IMU_HEADER_H__
diff --git a/software/firmware/src/peripherals/src/imu.c b/software/firmware/src/peripherals/src/imu.c
index e10bcd7f..996782f5 100644
--- a/software/firmware/src/peripherals/src/imu.c
+++ b/software/firmware/src/peripherals/src/imu.c
@@ -273,4 +273,13 @@ void imu_read_temp(int8_t *temp){
 void imu_read_fw_version(uint8_t *msb, uint8_t *lsb){
     *msb = i2c_read8(BNO055_SW_REV_ID_MSB_ADDR);
     *lsb = i2c_read8(BNO055_SW_REV_ID_LSB_ADDR);
+}
+
+void imu_read_calibration_status(bno55_calib_status_t *status) {
+    uint8_t reg_value = i2c_read8(BNO055_CALIB_STAT_ADDR);
+
+    status->mag = reg_value & 0x03;
+    status->accel = (reg_value >> 2) & 0x03;
+    status->gyro = (reg_value >> 4) & 0x03;
+    status->sys = (reg_value >> 6) & 0x03;
 }
\ No newline at end of file
diff --git a/software/firmware/tests/peripherals/test_imu.c b/software/firmware/tests/peripherals/test_imu.c
index 6c33c692..529c5c65 100644
--- a/software/firmware/tests/peripherals/test_imu.c
+++ b/software/firmware/tests/peripherals/test_imu.c
@@ -18,6 +18,7 @@ int main(void)
    int16_t w, x, y, z;
    int8_t temp;
    uint8_t rev_msb, rev_lsb;
+   bno55_calib_status_t status;
    imu_register_motion_change_callback(motion_interrupt, OPERATION_MODE_NDOF);
 
    imu_read_fw_version(&rev_msb, &rev_lsb);
@@ -37,6 +38,10 @@ int main(void)
       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);
+
+      //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);
    }
 
    // Should never reach this point