Move heading calculation for efficiency.
Getting simple goto-straightforward working.