drift correction and stabilization in firmware

DUE I2C remains stubborn