Code based on i2cdevlib by Jeff Rowberg