Updated accel and mag calibrate function