Fixed compass to point at goal waypoint.
Uses GPS only, removed accelerometer and magnemometer.