You could try: Experimental PID improvement changes Especially this Experimental PID improvement changes - #21 by DanS