HEAD_HOLD without mag sensor - optimization