Performance fix, removed as many calls to Standard Peripheral library as possible to improve performance. They usually add some overhead which is not needed in some cases