Added magnetometer initialization and read test