How to find the correct navigation course by GPS navigation with Arduino?

Question

I have an RC plane and I want to make it autonomous via GPS navigation usung an Arduino.

I have an Arduino Mega 2560 onboard connected to a GPS Ublox-Neo-M8N (GPS module) and there is the problem.

I want my RC-Plane to read its position and go to waypoints. The coordinates of two waypoints are defined before in a matrix variable like in this example :

a [1] [3] =
{
 {38.884675, -116.671035, 
 245}
};
b [1] [3] =
{
 {38.725019, -116.580951, 
 1180}
};

(1st is latitude, 2nd is longitude, and 3rd is height).

So I want my Arduino to read its current position from the GPS module's data and calculate the correct azimuth (correct horizontal angle) and the correct pitch angle (vertical angle) to navigate my plane to the waypoints.

When it reaches waypoint "a" with a small error (miss distance = 4 meters), it should be able to ignore navigation to the waypoint of variable "a" and then navigate to waypoint "b".

I am looking for the exact command from any library to calculate those two angles for navigation (azimuth, direction angle, and pitch angle) to navigate the RC plane from anywhere to the waypoints.

• PMF: The track (course over ground) is typically calculated by the GPS receiver and included in the data it sends. So you do not need to calculate anything there.
• PMF: The "navigation" part of your question is complex, though. Particularly since your plane does not have 6 degrees of freedom (like a drone) calculating a path to a waypoint includes a curve.
• jsotola: very good question, however it is about navigation, not about Arduino, so it is off topic here ... downvote

Answer 1

The computation is not difficult. Since your drone is unlikely to travel for thousands of kilometers, you can assume the Earth is locally flat, and the latitude and longitude are an orthogonal coordinate system. First, compute the vector to the next waypoint in Cartesian coordinates (dx, dy, dz):

// Defined at global scope.
const float radians_per_degree = M_PI / 180;
const float degrees_per_radians = 180 / M_PI;
const float meters_per_degree = 1e7 / 90;
// For every reading of the GPS:
float dx = (waypoint.longitude - current.longitude) * meters_per_degree
 * cos(current.latitude * radians_per_degree);
float dy = (waypoint.latitude - current.latitude) * meters_per_degree;
float dz = waypoint.height - current.height;

Then, from this vector you can get the required angles:

float azimuth = atan2(dx, dy) * degrees_per_radians;
float pitch_angle = atan(dz/sqrt(dx*dx + dy*dy)) * degrees_per_radians;

Note that the approximation breaks down if the drone has to travel for a distance that is a significant fraction of the distance to the closest pole. Note also that the approximation errors are inconsequential if you periodically update your estimate of azimuth and pitch angle: the errors will be corrected along the way as the drone gets closer and closer to the waypoint.

Edit: timemage posted a very interesting comment, and I would want to further expand on the precision issue. The AVR floating point support is indeed limited to single precision. For latitudes and longitudes in the range shown in the question, the numerical resolution (formally, the unit in the last place) is (×ばつ10⁻⁶)° for the latitude and (×ばつ10⁻⁶)° for the longitude. This translates to about 42 cm and 66 cm respectively on the surface of the Earth. It may be just good enough to hit the waypoint to within 4 m.

You could increase the resolution by storing the latitude and longitude in microdegrees, as 32-bit integers. This should be easy if the GPS always gives 6 digits after the decimal point: just ignore that decimal point. Then, once the quantities waypoint.longitude - current.longitude and waypoint.latitude - current.latitude are computed, you can safely use floating point for the rest of the computations.

• timemage: I was 80% of the way through writing something like this only with TinyGPSPlus when the phone rang, after which this was posted. So, I'll just mention here that if they're being literal about wanting a library, that one will do the course and distance part (which is probably haversine based). The pitch I'd synthesized in pretty much exact same way as this. The only other difference of note I had in mine is just that the user says they're using an Mega, so AVR, so only single precision float is available which is accurate to about 6 decimal places, so they should expect errors at close range.
• Edgar Bonet: @timemage: Thanks for this very relevant comment. I edited my answer to address the accuracy issue.
• PMF: If I understand the OP correctly, he has a wing-type RC plane, not a drone. This can't just move in a specific direction.
• Edgar Bonet: @Hamid'Smith'Salehi: The approximation error would be quite complex to compute, and depend on the exact start and end points of the path. Not anything as simple as an "error per kilometer". If you get GPS updates periodically, the approximation error will only make the path slightly curved, instead of being a straight line (or rather, a great circle). The error created by wind is likely to be many orders of magnitude larger. M_PI is π (pi).
• Dave X: M_PI = π =3.14259.... You could divide the 44 and 66cm resolution of each endpoint by the number of kilometers of distance between the two points if you want. The inaccuracy of using the flat-earth approximation versus calculating a great circle is that the great circle course doesn't have a constant azimuth that the flat-earth approximation would. How far are your waypoints from each other?