Fixed init bug MPU6000