Added heading calculation and other features to magnetometer