GPSDistance

include "TinyGPS++.h"
include "SoftwareSerial.h"

static const int RXPin = 6, TXPin = 5;
static const uint32_t GPSBaud = 9600;

TinyGPSPlus gps;
SoftwareSerial ss(RXPin, TXPin);

unsigned long last = 0UL;

int counter = 0;

float distance = 0.0;
float old_lat = 0.000000;
float old_lng = 0.000000;
float fastestSpeed = 0.0;
float gpsSpeed;

boolean doOnce = false;
boolean newLat = false;
boolean newLng = false;

void setup()
{
  Serial.begin(115200);
  while (!Serial) {
    ;
  }
  ss.begin(GPSBaud);
  Serial.println("Ready!");
}

void loop()
{
  // Dispatch incoming characters
  while (ss.available() > 0)
    gps.encode(ss.read());

  if (gps.speed.isUpdated())
  {
    gpsSpeed = gps.speed.kmph();

    if (gps.speed.kmph() > fastestSpeed) {
      fastestSpeed = gps.speed.kmph();
      //      Serial.print("Fastest Speed: ");
      Serial.print(fastestSpeed);
      Serial.println("km/h");
    }
  }

  // VERSION 1
  if (millis() - last > 5000)
  {
    //Serial.println();

    if (gps.location.isValid())
    {

      if (counter < 100) {
        counter++;
        distance = 0;
      }

      double distanceToLondon =
        TinyGPSPlus::distanceBetween(
          gps.location.lat(),
          gps.location.lng(),
          old_lat,
          old_lng);

      if (gpsSpeed > 1.0) {
        if (distanceToLondon < 100) {
          distance = distance + distanceToLondon;
        }
        if (newLat == true || newLng == true) {
          float correctDistance = distance * 2;
          //Serial.print(F("TRAVELED     Distance="));
          //Serial.print(distanceToLondon, 6);
          Serial.print("Distance: ");
          float distancekm = distance / 1000;
          Serial.print(distancekm, 2);
          Serial.print("km");
          Serial.print(", ");
          Serial.print(distance, 2);

          Serial.print("m, ");
          /*Serial.print(correctDistance);

            Serial.print(", ");*/
          Serial.print(gpsSpeed);
          Serial.println("km/h");
        }
      }
      float dasLat = gps.location.lat();
      if (old_lat != dasLat) {
        old_lat = gps.location.lat();
        newLat = true;
      } else {
        newLat = false;
      }
      float dasLng = gps.location.lng();
      if (old_lng != dasLng) {
        old_lng = gps.location.lng();
        newLng = true;
      } else {
        newLng = false;
      }
    }
  }
}