Improved calibration routine

Using internal registers to hold gyro and accelerometer bias calibrations