Heading is now calculated from route wayponts (needs testing)