XMega: Implement magnetometer reading and diagnostics