Pid fix, should fix position hold in EASY/AUTO