Added magnetometer calibration routine