diff --git a/software/firmware/src/peripherals/include/imu.h b/software/firmware/src/peripherals/include/imu.h
index aaf12c54..de2587a2 100644
--- a/software/firmware/src/peripherals/include/imu.h
+++ b/software/firmware/src/peripherals/include/imu.h
@@ -264,6 +264,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_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);
 void imu_read_gravity_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 08e1e7be..2b8535a3 100644
--- a/software/firmware/src/peripherals/src/imu.c
+++ b/software/firmware/src/peripherals/src/imu.c
@@ -244,6 +244,14 @@ void imu_register_motion_change_callback(motion_change_callback_t callback, bno0
    set_mode(mode);
 }
 
+void imu_set_power_mode(bno055_powermode_t power_mode)
+{
+   bno055_opmode_t saved_mode = get_mode();
+   set_mode(OPERATION_MODE_CONFIG);
+   i2c_write8(BNO055_PWR_MODE_ADDR, power_mode);
+   set_mode(saved_mode);
+}
+
 void imu_read_accel_data(bno055_acc_t *acc)
 {
    static int16_t accel_data[3];
diff --git a/software/firmware/tests/peripherals/test_imu.c b/software/firmware/tests/peripherals/test_imu.c
index fea0f999..425bbce1 100644
--- a/software/firmware/tests/peripherals/test_imu.c
+++ b/software/firmware/tests/peripherals/test_imu.c
@@ -27,6 +27,7 @@ int main(void)
    bno055_acc_t acc = {0};
 
    imu_register_motion_change_callback(motion_interrupt, OPERATION_MODE_NDOF);
+   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);