Added navigation code to calculate the forward azimuth to the destination, allowing the vehicle to follow the great circle arc between waypoints. Also added compass calibration function that finds the required offsets. This makes a dramatic difference in compass accuracy. Everything works well!
Added a function to the Navigator that uses the haversine formula to calculate the distance to the next waypoint along the great circle. This will be used to determine if waypoints have been achieved.