diff --git a/Src/mpu6050.c b/Src/mpu6050.c
index 9160540..d1e2818 100644
--- a/Src/mpu6050.c
+++ b/Src/mpu6050.c
@@ -39,6 +39,7 @@
 #define WHO_AM_I_REG 0x75
 #define PWR_MGMT_1_REG 0x6B
 #define SMPLRT_DIV_REG 0x19
+#define CONFIG_REG 0x1A
 #define ACCEL_CONFIG_REG 0x1C
 #define ACCEL_XOUT_H_REG 0x3B
 #define TEMP_OUT_H_REG 0x41
@@ -51,6 +52,7 @@ const uint16_t i2c_timeout = 100;
 const double Accel_Z_corrector = 14418.0;
 
 uint32_t timer;
+static uint8_t MPU6050_Initialized = 0U;
 
 Kalman_t KalmanX = {
     .Q_angle = 0.001f,
@@ -67,9 +69,10 @@ uint8_t MPU6050_Init(I2C_HandleTypeDef *I2Cx)
 {
     uint8_t check;
     uint8_t Data;
-
+    // power management register 0X6B we should write all 0's to wake the sensor up
+    uint16_t reg_value = (PWR_MGMT_1_REG << 8U) | 0x00U;
+    HAL_I2C_Master_Transmit(I2Cx, MPU6050_ADDR, (uint8_t *)&reg_value, 2, 100);
     // check device ID WHO_AM_I
-
     HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, WHO_AM_I_REG, 1, &check, 1, i2c_timeout);
 
     if (check == 104) // 0x68 will be returned by the sensor if everything goes well
@@ -78,6 +81,10 @@ uint8_t MPU6050_Init(I2C_HandleTypeDef *I2Cx)
         Data = 0;
         HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, PWR_MGMT_1_REG, 1, &Data, 1, i2c_timeout);
 
+        // Low pass filter
+        Data = 0x04;
+        HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, CONFIG_REG, 1, &Data, 1, 100);
+
         // Set DATA RATE of 1KHz by writing SMPLRT_DIV register
         Data = 0x07;
         HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, SMPLRT_DIV_REG, 1, &Data, 1, i2c_timeout);
@@ -91,6 +98,7 @@ uint8_t MPU6050_Init(I2C_HandleTypeDef *I2Cx)
         // XG_ST=0,YG_ST=0,ZG_ST=0, FS_SEL=0 -> � 250 �/s
         Data = 0x00;
         HAL_I2C_Mem_Write(I2Cx, MPU6050_ADDR, GYRO_CONFIG_REG, 1, &Data, 1, i2c_timeout);
+        MPU6050_Initialized = 1U;
         return 0;
     }
     return 1;
@@ -100,6 +108,7 @@ void MPU6050_Read_Accel(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
 {
     uint8_t Rec_Data[6];
 
+    if (MPU6050_Initialized == 0U) return;
     // Read 6 BYTES of data starting from ACCEL_XOUT_H register
 
     HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, Rec_Data, 6, i2c_timeout);
@@ -122,6 +131,7 @@ void MPU6050_Read_Gyro(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
 {
     uint8_t Rec_Data[6];
 
+    if (MPU6050_Initialized == 0U) return;
     // Read 6 BYTES of data starting from GYRO_XOUT_H register
 
     HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, GYRO_XOUT_H_REG, 1, Rec_Data, 6, i2c_timeout);
@@ -145,6 +155,7 @@ void MPU6050_Read_Temp(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
     uint8_t Rec_Data[2];
     int16_t temp;
 
+    if (MPU6050_Initialized == 0U) return;
     // Read 2 BYTES of data starting from TEMP_OUT_H_REG register
 
     HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, TEMP_OUT_H_REG, 1, Rec_Data, 2, i2c_timeout);
@@ -157,7 +168,7 @@ void MPU6050_Read_All(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
 {
     uint8_t Rec_Data[14];
     int16_t temp;
-
+    if (MPU6050_Initialized == 0U) return;
     // Read 14 BYTES of data starting from ACCEL_XOUT_H register
 
     HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, ACCEL_XOUT_H_REG, 1, Rec_Data, 14, i2c_timeout);