ardupilot/libraries/AP_Navigation/examples/Navigation_simple/Navigation_simple.pde

#include <Waypoints.h>
#include <Navigation.h>
#include <AP_GPS_IMU.h>


AP_GPS_IMU gps;
Navigation	nav((GPS *) & gps);

void setup()
{
	Serial.begin(38400);
	Serial.println("Navigation test");
	
	Waypoints::WP	location_A = {0, 0, 38.579633 * T7, -122.566265 * T7, 10000};
	Waypoints::WP	location_B = {0, 0, 38.578743 * T7, -122.572782 * T7, 5000};
	
	long distance 	= nav.get_distance(&location_A, &location_B);
	long bearing 	= nav.get_bearing(&location_A, &location_B);

	Serial.print("\tDistance = ");
	Serial.print(distance, DEC);	
	Serial.print(" Bearing = ");
	Serial.println(bearing, DEC);
}

void loop()
{
}