Relocate and use some of the common MPU code from MPU6500 into