Fixed up speed and heading, these still need to be tested