Fix the delay time issue
Block the gyro calibration code
Expand the stack of the task
Create new I2C read function
Use new i2c read function to read the who am i data