diff --git a/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp b/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp
index f287b2d13e..52a5a17560 100644
--- a/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp
+++ b/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp
@@ -94,6 +94,10 @@ enum CONFIG_BIT : uint8_t {
 
 // GYRO_CONFIG
 enum GYRO_CONFIG_BIT : uint8_t {
+	XG_ST           = Bit7,
+	YG_ST           = Bit6,
+	ZG_ST           = Bit5,
+
 	// FS_SEL [4:3]
 	FS_SEL_250_DPS	= 0,           // 0b00000
 	FS_SEL_500_DPS	= Bit3,        // 0b01000
@@ -103,6 +107,10 @@ enum GYRO_CONFIG_BIT : uint8_t {
 
 // ACCEL_CONFIG
 enum ACCEL_CONFIG_BIT : uint8_t {
+	XA_ST       = Bit7,
+	YA_ST       = Bit6,
+	ZA_ST       = Bit5,
+
 	// AFS_SEL [4:3]
 	AFS_SEL_2G  = 0,           // 0b00000
 	AFS_SEL_4G  = Bit3,        // 0b01000
diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp
index 2d5180013a..ac54452e25 100644
--- a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp
+++ b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp
@@ -158,8 +158,8 @@ private:
 	static constexpr uint8_t size_register_cfg{7};
 	register_config_t _register_cfg[size_register_cfg] {
 		// Register               | Set bits, Clear bits
-		{ Register::GYRO_CONFIG,   GYRO_CONFIG_BIT::FS_SEL_2000_DPS, 0 },
-		{ Register::ACCEL_CONFIG,  ACCEL_CONFIG_BIT::AFS_SEL_16G, 0 },
+		{ Register::GYRO_CONFIG,   GYRO_CONFIG_BIT::FS_SEL_2000_DPS, GYRO_CONFIG_BIT::XG_ST | GYRO_CONFIG_BIT::YG_ST | GYRO_CONFIG_BIT::ZG_ST },
+		{ Register::ACCEL_CONFIG,  ACCEL_CONFIG_BIT::AFS_SEL_16G, ACCEL_CONFIG_BIT::XA_ST | ACCEL_CONFIG_BIT::YA_ST | ACCEL_CONFIG_BIT::ZA_ST },
 		{ Register::FIFO_EN,       FIFO_EN_BIT::XG_FIFO_EN | FIFO_EN_BIT::YG_FIFO_EN | FIFO_EN_BIT::ZG_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN, FIFO_EN_BIT::TEMP_FIFO_EN },
 		{ Register::INT_PIN_CFG,   INT_PIN_CFG_BIT::INT_LEVEL, 0 },
 		{ Register::INT_ENABLE,    INT_ENABLE_BIT::DATA_RDY_INT_EN, 0 },