Fix bug in I2C callback function