Calibrated magnetometer offset, 3d heading is now working