Fixed weird infinite loop problem in uart output function by adding critical section code. Also shortened/optimized SysTickHandler