TinyGPS++ using distanceBetween

I’m working with an electron and working from an example using TinyGPS++ where they get the distance between two points. The examples with TinyGPS though use a fixed point, London, for one point and the current location pulled from the GPS. I’m attempting to capture a point and when I pull the next point 60 seconds later, I want to know the distance between the two points. My intent is that if the points are too close together that I will wait another 60 seconds and try again.

The coordinates that I am getting out seem to be right and I’m getting the same correct value at each iteration. But when I try to store the values to calculate distance, the values are changing significantly and the distanceBetween is returning many thousands of Kilometers. I think I am doing something simple wrong but I’m not spotting it, I’m hoping someone will spot it.

/*
   Adapted from the example file "FullExample" included with the TinyGPS++ library
   
*/
#include <TinyGPS++/TinyGPS++.h>

// Change GPSBaud based on GPS unit
// 9600 for Adafruit's Ultimate GPS
static const uint32_t GPSBaud = 9600;

// The TinyGPS++ object
TinyGPSPlus gps;
char nullCoord[21];


// This custom version of delay() ensures that the gps object
// is being "fed".
static void smartDelay(unsigned long ms)
{
  unsigned long start = millis();
  do 
  {
    while (Serial1.available())
      gps.encode(Serial1.read());
  } while (millis() - start < ms);
}

static void makeCoord(char *coord1, char *coord2, char *coords)
{
    strcpy(coords,coord1);
    strcat(coords,",");
    strcat(coords,coord2);
}

void setup()
{
  // Begin serial at 115200 Baud
  Serial.begin(115200);
  Serial.println(F("Hello world!"));
  // Serial1 reads from the Electron's TX/RX
  Serial1.begin(GPSBaud);
  // Make the 0 coordinate
  makeCoord("0.000000", "0.000000", nullCoord);
}

void loop()
{

  // Now we prepare to publish location data to our webhook

  // Make strings of the Latitude and Longitude readings
  char coord1[10];
  sprintf(coord1, "%f", (gps.location.lat(), gps.location.isValid(), 11, 6));
  char coord2[10];
  sprintf(coord2, "%f", (gps.location.lng(), gps.location.isValid(), 12, 6));
  char speed[32];
  sprintf(speed, "%ld", (gps.speed.mph(), gps.location.isValid()));
  char course[32];
  sprintf(course, "%ld", (gps.course.deg(), gps.location.isValid()));

  
  // Combine these strings into the coordinate format accepted by Initial State
  char curCoord[21];
  makeCoord(coord1, coord2, curCoord);

  // Send the data
  if (strcmp(curCoord, nullCoord) != 0){
    // Particle.publish("gpsData",curCoord);
  }
  
  // Save to last position variables
  char lastCoord[21];
  strcpy(lastCoord, curCoord);
  double lastLat;
  lastLat = gps.location.lat();
  double lastLong;
  lastLong = gps.location.lng();
  double currLat;
  double currLong;
  char longchar[150];
  sprintf(longchar, "last lat %d, last lng %d", lastLat, lastLong);
  Serial.println(longchar);

  // Wait 60 seconds
  do {
    smartDelay(60000);

    currLat = gps.location.lat();
    currLong = gps.location.lng();

    sprintf(longchar, "GPS Lat %d, lng %d",currLat, currLong);
    Serial.println(longchar);
    Serial.println(TinyGPSPlus::distanceBetween(currLat, currLong, lastLat, lastLong));
    
  } while (TinyGPSPlus::distanceBetween(currLat, currLong, lastLat, lastLong) < 100);

  if (millis() > 5000 && gps.charsProcessed() < 10)
  {
    Serial.println(F("No GPS data received: check wiring"));
    Particle.publish("gpsData",nullCoord);
  }
}

I’m not sure but it may be because these variables are not global variables so they are not saved on each loop:

  double lastLat;
  lastLat = gps.location.lat();
  double lastLong;
  lastLong = gps.location.lng();
1 Like

You need an obscure thing called a haversine function :smile:

I created this one and have tested it with much success in tracking applications.

I see that TinyGPS++ has this built in, but maybe there is some confusion between radians vs degrees. My notes should be clear what is expected and maybe that will help you out.

#include <math.h>
// Returns the great-circle distance (in meters) between two points on a sphere
// lat1, lat2, lon1, lon2 must be provided in Degrees.  (Radians = Degrees * PI / 180, Degrees = Radians / PI * 180)
double haversine(double lat1, double lon1, double lat2, double lon2) {
    const double rEarth = 6371000.0; // in meters
    double x = pow( sin( ((lat2 - lat1)*M_PI/180.0) / 2.0), 2.0 );
    double y = cos(lat1*M_PI/180.0) * cos(lat2*M_PI/180.0);
    double z = pow( sin( ((lon2 - lon1)*M_PI/180.0) / 2.0), 2.0 );
    double a = x + y * z;
    double c = 2.0 * atan2(sqrt(a), sqrt(1.0-a));
    double d = rEarth * c;
    // Serial.printlnf("%12.9f, %12.9f, %12.9f, %12.9f, %12.9f, %12.9f", x, y, z, a, c, d);
    return d; // in meters
}

For reference here’s the TinyGPS++ version. Not sure why they use 6372795 m :shrug:

/* static */
double TinyGPSPlus::distanceBetween(double lat1, double long1, double lat2, double long2)
{
  // returns distance in meters between two positions, both specified
  // as signed decimal-degrees latitude and longitude. Uses great-circle
  // distance computation for hypothetical sphere of radius 6372795 meters.
  // Because Earth is no exact sphere, rounding errors may be up to 0.5%.
  // Courtesy of Maarten Lamers
  double delta = radians(long1-long2);
  double sdlong = sin(delta);
  double cdlong = cos(delta);
  lat1 = radians(lat1);
  lat2 = radians(lat2);
  double slat1 = sin(lat1);
  double clat1 = cos(lat1);
  double slat2 = sin(lat2);
  double clat2 = cos(lat2);
  delta = (clat1 * slat2) - (slat1 * clat2 * cdlong);
  delta = sq(delta);
  delta += sq(clat2 * sdlong);
  delta = sqrt(delta);
  double denom = (slat1 * slat2) + (clat1 * clat2 * cdlong);
  delta = atan2(delta, denom);
  return delta * 6372795;
}
4 Likes

Very nice!

The figure of 6371000 m comes from the WGS84 ellipsoid which is designed to minimize the mean squared error. There are several other possible Earth radii but I don’t recognize their figure of 6372795 m either.

There is more information here:

1 Like

Thank you! The numbers from the haversine function make a lot more sense than what I was getting from the distanceBetwtween function.

1 Like

Regarding TinyGPS++, this may be the same bug that I fixed last year. The library may not have been released since the bug fix was merged. I’ll follow up with the library author.

2 Likes

Hmm. Is it possible that you’re the one who is confused here? Your haversine function has as comments written above it that “lat1, lat2, lon1, lon2 must be provided in Radians”, but then inside the function you go on to convert the given values from degrees to radians.

I’m not sure this will help lessen the confusion :wink:

Hi @mmcarvin

I don’t think @BDub is confused here. The C/C++ functions sin, cos and atan2 all work on radians so I interpret his comments and code as saying that even though longitude and latitude are always given in degrees, the code has to convert to radians in order for the trig functions to work.

Hi @mmcarvin yes thank you I started with the function taking radians and converted to degrees and forgot to update a couple words :blush: I posted because it was confusing to me and I empathized with the OP … sorry I’ll fix things up! IIRC, the calculations work best when done in radians, and most GPS units spit out degrees easily so things work pretty naturally. But you must pay attention to what units you are working with, which is why the comments were meant to explain degrees are expected, and the formulas to convert between radians and degrees to help you recognize those patterns in case your LAT/LON has been pre-converted to radians.

Usually, comments above a function will tell you what the function expects. This is so that you don’t have to read the code in order to use the function. So when a function tells you that the lat, long, etc. must be “provided in Radians”, this is wrong. They must be provided in degrees.

I see your note now, @BDub, thanks for clearing that up - it may help a future reader of your post!!

1 Like