Added GPS Heading

GPS heading instead of magnetic!! Magnetic not useable...
Modified Autopilot to reach goal heading.