Improved calibration routine

Using internal registers to hold gyro and accelerometer bias calibration