Fixed desired heading calculation