Minor cleanup
Simple magnetic saturation detection and demagnetization; the idea is stolen shamelessly from SimonK ESC