mirror of https://github.com/ArduPilot/ardupilot
29 lines
652 B
Plaintext
29 lines
652 B
Plaintext
#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()
|
|
{
|
|
}
|