Increase time accuracy of task tick:
Changed from 100Hz RTOS tick to 500Hz.
Changed the I2C speed to a round 300KHz speed.