GPS with display

/* This sketch uses the Arduino MKR WiFi 1010, MKR GPS shield and Adafruit SSD1327 display. Date/time, GPS latitude, longitude, altitude, ground speed and 
distance are displayed on the SSD1327 along with battery voltage and distance traveled in meters, feet and miles. The real time clock on the MKR WiFi 1010 is
synchronized with GPS time and displays the date, hour, minute and second.*/

//Libraries for Adafruit SSD1327 display, Arduino MKR GPS and Arduino Real Time Clock
#include <Adafruit_SSD1327.h>
#include <Arduino_MKRGPS.h>
#include <RTCZero.h>

#define DTHRESH 1.0 //Distance change threshhold to calculate total distance
#define STHRESH 0.3 //Speed threshhold to calculate total distance

//Instantiate the display and real time clock objects
Adafruit_SSD1327 display(128, 128, &Wire, -1, 1000000);
RTCZero rtc;

//Declare the previous lat/long, distance and duration global variables
float plat;
float plon;
float dist;
float dur;
uint32_t tm0;
uint32_t tm1;

//Initial setup
void setup()   {
  //The following serial monitor display statements are commented out in case they are needed for troubleshooting when the board is connected to a computer              
  //Serial.begin(9600);
  //while (! Serial) delay(100);

  //Initialize the Adafruit SSD1327 128x128 pixel display
  if ( ! display.begin(0x3D) ) {
     //Serial.println("Unable to initialize OLED");
     while (1) yield();
  }

  //Set the SSD1327 text size, colors and clear the display
  display.display();
  display.setTextSize(1);
  display.setTextWrap(false);
  display.setTextColor(SSD1327_WHITE,SSD1327_BLACK);
  display.clearDisplay();
  display.display();

  //Initialize the real time clock
  rtc.begin();

  //Initialize the MKR GPS
  if (!GPS.begin(GPS_MODE_SHIELD)) {
    //display.println("Failed to initialize GPS!");
    while (1);
  }

  //Wait for the GPS to become available. This may take up to 15 minutes.
  while (!GPS.available()) {
    ;
  }

  //Declare the global previous latitude, longitude and cumulative distance variables
  plat = GPS.latitude();
  plon = GPS.longitude();
  tm0 = GPS.getTime();
  dist = 0.0;
  dur = 0.0;
  tm1 = 0; 
}

//Main loop
void loop() {
  //Wait for the GPS to become available then display GPS data
  if (GPS.available()) {
    uint32_t tm = GPS.getTime(); //Get GPS epoch time
    if (rtc.getEpoch() != tm) { //Sychronize the real time clock with GPS time
      rtc.setEpoch(tm);
    }
    float lat   = GPS.latitude(); //Get GPS latitude
    float lon  = GPS.longitude(); //Get GPS longitude
    float altm   = GPS.altitude(); //Get GPS altitude in meters
    float altf = 3.28084 * altm; //Convert altiude from meters to feet
    float kph      = GPS.speed(); //Get GPS ground speed in kph
    float mph = 0.621371 * kph; //Convert kph ground speed to mph
    int   satellites = GPS.satellites(); //Get number of GPS satellites received
    int Batt_int = analogRead(ADC_BATTERY); //Read the external battery voltage
    float Batt_int_v = Batt_int * (4.27/1023.0); //Convert the digital battery voltage to analog voltage
    float xkm; //Declare distance variable in km
    float xmt; //Declare distance variable in meters
    
    //Check real time clock every second and calculate distance traveled in km since GPS reading using the Haversine formula
    if (tm > tm1) {
      xkm = HaverSine(plat, plon, lat, lon);
      xmt = 1000.0 * xkm; //Convert km to meters
      /*Reduce false GPS readings by checking for non-numeric Haversine results, distance change greater than threshhold and 
      speed greater than threshhold.*/
      if (! isnan(xmt) && xmt > DTHRESH && mph > STHRESH) {
        dist += xmt; //Add distance reading in meters to cumulative distance
      }
    }

    //Calculate distance and duration
    float distmi = 0.000621371 * dist; //Convert distance in km to miles
    float distft = 3.28084 * dist; //Convert distance in meters to feet
    dur = (tm - tm0)/3600.0; //Hours duration

    //Display values on SSD1327
    display.display(); //Send yield message to SSD1327 display
    display.clearDisplay(); //Clear the SSD1327 display
    display.setCursor(0,0); //Position the SSD1327 cursor at top left
    printZero(rtc.getMonth()); //Display real time clock month with leading zero if needed
    display.print("-");
    printZero(rtc.getDay()); //Display real time clock day with leading zero if needed
    display.print("-");
    printZero(rtc.getYear()); //Display real time clock year with leading zero if needed
    display.print(" ");
    printZero(rtc.getHours()-4); //Display real time clock hour with leading zero if needed and correct for time zone
    display.print(":");
    printZero(rtc.getMinutes()); //Display real time clock minute with leading zero if needed 
    display.print(":");
    printZero(rtc.getSeconds()); //Display real time clock second with leading zero if needed
    display.println();
    display.print(lat,7); //Display GPS latitude as decimal degrees
    display.println(" lat");
    display.print(lon,7); //Display GPS longitude as decimal degrees
    display.println(" lon");
    display.print(altf); //Display GPS altitude in feet
    display.println(" alt ft");
    display.print(mph); //Display GPS ground speed in mph
    display.println(" mph");
    display.print(satellites); //Display number of GPS satellites received
    display.println(" satellites");
    display.print(Batt_int_v); //Display analog battery voltage
    display.println(" volts");
    display.print(distmi,3); //Display cumulative distance traveled in miles
    display.println(" dist miles");
    display.print(distft,1); //Display cumulative distance traveled in feet
    display.println(" dist feet");
    display.print(dur,3);
    display.println(" hours");
    display.display();

    /*Copy current lat/lon into previous lat/lon for next distance measurement
    and current time to previous time for next time measurement.*/
    plat = lat;
    plon = lon;
    tm1 = tm;
  }
}

//Function to display leading zero if single digit
void printZero(int x) {
  if (x < 10) {
    display.print("0");
  }
  display.print(x);
}

//Haversine formula to calculate distance between current and previous latitude/longitude points
float HaverSine(float lat1, float lon1, float lat2, float lon2)
{
  float ToRad = PI / 180.0;
  float R = 6371;   // radius earth in Km
  
  float dLat = (lat2-lat1) * ToRad;
  float dLon = (lon2-lon1) * ToRad; 
  
  float a = sin(dLat/2) * sin(dLat/2) +
        cos(lat1 * ToRad) * cos(lat2 * ToRad) * 
        sin(dLon/2) * sin(dLon/2); 
        
  float c = 2 * atan2(sqrt(a), sqrt(1-a)); 
  
  float d = R * c;
  return d;
}