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

29 lines
617 B
Plaintext
Raw Normal View History

#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()
{
}