May be this can get you going:
a bit of terminology
- You start from the
From waypoint and want to go to the GOTO waypoint in the most direct way (Desired Track).
- This desired track is known as the ideal great circle path between those 2 points.
- You find yourself in the
Present Location and you want to calculate the shortest distance to the desired track, this is known as the Cross Track Distance (XTE in GPS terminology).
Here is an example:
You start near Calais and want to cross the English channel and land near Dover but on your way there you drift to A and then B. You want to know the Cross Track Distance for A and B based on their latitude and longitude.
Here is some code for this using some of the builtin functions of TinyGPS++ (and one extra function to calculate the XTE).
// formulas from https://www.movable-type.co.uk/scripts/latlong.html
// adjusted earth radius to match TinyGPS++ value for coherence
#include <TinyGPS++.h> // https://github.com/mikalhart/TinyGPSPlus
TinyGPSPlus gps;
#define gpsSerial Serial1
const uint32_t GPSBaud = 115200;
double fromLat = 50.95723461041729, fromLon = 1.8175379777021576; // start point = 50.95723461041729, 1.8175379777021576
double gotoLat = 51.13113565835111, gotoLon = 1.3260115342823147; // destination point = 51.13113565835111, 1.3260115342823147
double ALat = 50.97572382883768, ALon = 1.7230724414682619; // point A = 50.97572382883768, 1.7230724414682619
double BLat = 51.12242447725161, BLon = 1.3669750943475254; // point B = 51.12242447725161, 1.3669750943475254
double distance; // Distance between start and destination (orthodrome, hypothetical sphere of radius 6372795 meters)
double dtk; // Desired Track in degrees (forward azimuth)
// Cross-track distance (XTE)
// Shortest signed distance from a point (lat3, long3) to an ideal great circle path defined by (lat1, long1) and (lat2, long2)
// Negative if to left, positive if to right of path, with hypothetical sphere of radius 6372795 meters
double crossTrackDistanceFromTo(double lat3, double lon3, double lat1, double lon1, double lat2, double lon2) {
const double R = 6372795.0;
const double ad13 = gps.distanceBetween(lat1, lon1, lat3, lon3) / R;
const double rtk13 = radians(gps.courseTo(lat1, lon1, lat3, lon3));
const double rtk12 = radians(gps.courseTo(lat1, lon1, lat2, lon2));
const double dxt = asin(sin(ad13) * sin(rtk13 - rtk12));
return dxt * R;
}
void setup() {
Serial.begin(115200); Serial.println();
gpsSerial.begin(GPSBaud);
dtk = gps.courseTo(fromLat, fromLon, gotoLat, gotoLon);
distance = gps.distanceBetween(fromLat, fromLon, gotoLat, gotoLon);
Serial.print(F("Distance to destinatinon: "));
Serial.print(distance, 0); Serial.println(F(" m"));
Serial.print(F("Desired Track (DTK°): "));
Serial.print(dtk);
Serial.print(F("° (")); Serial.print(gps.cardinal(dtk)); Serial.println(F(")"));
double cmgF = gps.courseTo(fromLat, fromLon, fromLat, fromLon);
double xteF = crossTrackDistanceFromTo(fromLat, fromLon, fromLat, fromLon, gotoLat, gotoLon);
Serial.print(F("Point From: ")); Serial.print(fromLat, 6); Serial.write(','); Serial.print(fromLat, 6);
Serial.print(F(" - (CMG =")); Serial.print(cmgF); Serial.print(F("°), "));
Serial.print(F("Cross track distance = ")); Serial.print(xteF); Serial.print(F(" m, "));
if (xteF == 0) Serial.println(F("on ideal course"));
else Serial.println(xteF < 0 ? F("to the LEFT") : F("to the RIGHT"));
double cmgA = gps.courseTo(fromLat, fromLon, ALat, ALon);
double xteA = crossTrackDistanceFromTo(ALat, ALon, fromLat, fromLon, gotoLat, gotoLon);
Serial.print(F("Point A: ")); Serial.print(ALat, 6); Serial.write(','); Serial.print(ALon, 6);
Serial.print(F(" - (CMG =")); Serial.print(cmgA); Serial.print(F("°), "));
Serial.print(F("Cross track distance = ")); Serial.print(xteA); Serial.print(F(" m, "));
if (xteA == 0) Serial.println(F("on ideal course"));
else Serial.println(xteA < 0 ? F("to the LEFT") : F("to the RIGHT"));
double cmgB = gps.courseTo(fromLat, fromLon, BLat, BLon);
double xteB = crossTrackDistanceFromTo(BLat, BLon, fromLat, fromLon, gotoLat, gotoLon);
Serial.print(F("Point B: ")); Serial.print(BLat, 6); Serial.write(','); Serial.print(BLon, 6);
Serial.print(F(" - (CMG =")); Serial.print(cmgB); Serial.print(F("°), "));
Serial.print(F("Cross track distance = ")); Serial.print(xteB); Serial.print(F(" m, "));
if (xteB == 0) Serial.println(F("on ideal course"));
else Serial.println(xteB < 0 ? F("to the LEFT") : F("to the RIGHT"));
double cmgG = gps.courseTo(fromLat, fromLon, gotoLat, gotoLon);
double xteG = crossTrackDistanceFromTo(gotoLat, gotoLon, fromLat, fromLon, gotoLat, gotoLon);
Serial.print(F("Point GOTO: ")); Serial.print(fromLat, 6); Serial.write(','); Serial.print(fromLat, 6);
Serial.print(F(" - (CMG =")); Serial.print(cmgG); Serial.print(F("°), "));
Serial.print(F("Cross track distance = ")); Serial.print(xteG); Serial.print(F(" m, "));
if (xteG == 0) Serial.println(F("on ideal course"));
else Serial.println(xteG < 0 ? F("to the LEFT") : F("to the RIGHT"));
}
void loop() {
if ( gpsSerial.peek() != -1) {
if (gps.encode(gpsSerial.read())) {
if (gps.location.isValid()) {
double presentLat = gps.location.lat();
double presentLon = gps.location.lng();
double cmg = gps.courseTo(fromLat, fromLon, presentLat, presentLon);; // Course Made Good in degrees
double xte = crossTrackDistanceFromTo(presentLat, presentLon, fromLat, fromLon, gotoLat, gotoLon);
Serial.print(F("Position: ")); Serial.print(presentLat, 6); Serial.write(','); Serial.print(presentLon, 6); // print our position
Serial.print(F(" - (CMG=")); Serial.print(cmg); Serial.println(F("°)"));
Serial.print(F("Cross track distance = ")); Serial.print(xte); Serial.print(F(" m, "));
Serial.println(xte < 0 ? F("to the LEFT") : F("to the RIGHT"));
}
}
}
}
Running this on an Arduino Mega, the Serial Monitor (@ 115200 bauds) will show
Distance to destinatinon: 39441 m
Desired Track (DTK°): 299.56° (WNW)
Point From: 50.957233,50.957233 - (CMG =0.00°), Cross track distance = 0.00 m, on ideal course
Point A: 50.975723,1.723072 - (CMG =287.30°), Cross track distance = -1471.01 m, to the LEFT
Point B: 51.122425,1.366975 - (CMG =300.42°), Cross track distance = 548.94 m, to the RIGHT
Point GOTO: 50.957233,50.957233 - (CMG =299.56°), Cross track distance = 0.00 m, on ideal course
the loop does not print anything if you don't have a GPS connected to Serial1 otherwise it would constantly monitor your Cross track distance.
So you know if you are left or right of the ideal path and have the distance to that path - haptic feedback should be pretty easy to figure out instead of printing.
You'll need of course to find a way to enter the From and GOTO waypoints into the Arduino as those are hardwired at the moment. From could probably be set easily with the press of a button and reading the current location but the GOTO waypoint will require some sort of User Interface.
Running on an ESP32 or Teensy would be faster due to all the floating point maths that are required. I would not advise to stick with a UNO or MEGA.
I'm totally unsure of the maths (although they look realistic in the example above) and there are approximations like saying earth is an hypothetical sphere of radius 6372795 meters for example. Flying near the poles will break many of the approximations in the formulas as well and GPS precision might not be good enough...
As a side note, I would assume most advanced GPS system would probably know how to show these information automatically and probably be more trustworthy than this code...
