Fix calibration, swap X/Y axis on magnetometer, fix Ki/Kp const