ESP32-dual-GPS-BTserial.ino

/*
  ESP32 dual-GPS-BT-server with watch dog timer

  By hooking up two multiGNSS GPS modules to ESP32 , 
  this program averages two GPS output and
  outputs NMEA to Bluetooth Serial Port Profile.
  NMEA can be monitored by ESP32's USB serial port and BT serial terminal on Android.
  Only $GNGGA and $GNRMC are regenerated by this program.

  When Serial.BT stops , watch dog timer reboot this program.
  This program requires Android phone has the Bluetooth Serial Port Profile
  to connect to ESP32-GPS.
  Application : run Lefebure to replace from Android phone's embeded GPS to your GPS

  https://play.google.com/store/apps/details?id=com.lefebure.ntripclient&hl=ja&gl=US

  GPS1 TX should be connected to GPIO_NUM_16 of ESP32
  GPS2 TX should be connected to GPIO_NUM_33 of ESP32
  GPS modules used to test : M5stack GPS module (using AT6558) , connected by 9600bps serial

  Ref. http://arduiniana.org/libraries/tinygpsplus/
      https://github.com/espressif/arduino-esp32/blob/master/libraries/BluetoothSerial/

  just tested in North , East region.

  Copyright 2021(C) coniferconifer

  MIT License

*/
//#include <Ticker.h>      // timer

#include <TinyGPS++.h>
#define VERSION "20210901"
TinyGPSPlus gps1;
TinyGPSPlus gps2;
#define GPS_BAUD 9600
// https://github.com/espressif/arduino-esp32/blob/master/libraries/BluetoothSerial/examples/SerialToSerialBTM/SerialToSerialBTM.ino
#include "BluetoothSerial.h"
BluetoothSerial SerialBT;
TaskHandle_t Task1;
#define PI 3.141592653589793
#define EARTH 6378137.0

boolean printRMS = false;
const int iIntervalTime = 3;  // watch dog interval in sec
long lastGPSread = 0;
int Alt = 0; //geoid height at your place

#define NMEA_BUFSIZE 200
void printChecksum( char buf[]) {
  int i = 0; unsigned int sum = 0;
  for (i = 1; i < NMEA_BUFSIZE; i++) {
    if ( buf[i] == 0) {
      //Serial.printf(" %d ",i);
      break;
    }
    sum = sum ^ (unsigned int)buf[i];
  }
  sum = sum % 256;
  Serial.printf("*%02x\r\n", sum);
  SerialBT.printf("*%02x\r\n", sum);

}

void printNMEA(int yy, byte month, byte dd, byte hh, byte mm, byte ss, byte centiss, double lat , double lon , \
               int height, float hdop, int numSat , double course, double speedKnot ) {

  char buf[NMEA_BUFSIZE];

  char NS = 'N';
  char EW = 'E';
  if ( lat < 0.0 ) {
    lat = -1.0 * lat;
    NS = 'S';
  }
  if ( lon < 0.0 ) {
    lon = -1.0 * lon;
    EW = 'W';
  }
  int lathigh = (int)lat;
  //Serial.printf("lathigh=%d ", lathigh);
  double d_minutes = (lat - (double)lathigh) * 60.0; //double minutes
  //  Serial.printf("d_minutes=%f", d_minutes);
  int min = (int)d_minutes;
  d_minutes = d_minutes - (double)min;
  d_minutes = d_minutes * 1000000.0;
  int lonhigh = (int)lon;
  double d_minutes_lon = (lon - (double)lonhigh) * 60.0;
  //  Serial.printf("lonhigh=%d ", lonhigh);
  //  Serial.printf("d_minutes_lon=%f\r\n", d_minutes_lon);
  int min_lon = (int)d_minutes_lon;
  d_minutes_lon = d_minutes_lon - (double)min_lon;
  d_minutes_lon = d_minutes_lon * 1000000.0;
  sprintf(buf, "$GNGGA,%02d%02d%02d.%03d,%d%02d.%06d,%c,%3d%02d.%06d,%c,1,%02d,%1.1f,%d.0,M,0.0,M,,0000\0", \
          (int)hh, (int)mm, (int)ss, (int)centiss * 10, lathigh, min, (int)d_minutes, NS, lonhigh, min_lon, (int)d_minutes_lon, \
          EW, numSat, hdop, height);
  // char buf2[]="$GPGGA,085120.307,3541.1493,N,13945.3994,E,1,08,1.0,6.9,M,35.9,M,,0000\0";
  Serial.print(buf);
  SerialBT.print(buf);
  printChecksum(buf);
  //Serial.printf("year=%02d\r\n",yy);
  //Serial.printf("course=%3.1f speed=%3.1f \r\n", course, speedKnot);
  sprintf(buf, "$GNRMC,%02d%02d%02d.%03d,A,%d%02d.%06d,%c,%3d%02d.%06d,%c,%3.1f,%3.1f,%02d%02d%02d,,,A\0", \
          (int)hh, (int)mm, (int)ss, (int)centiss * 10, lathigh, min, (int)d_minutes, NS, lonhigh, min_lon, (int)d_minutes_lon, \
          EW, speedKnot, course, (int)dd, (int)month, (int)yy - 2000);
  Serial.print(buf);
  SerialBT.print(buf);
  printChecksum(buf);
}
int num_sat1, num_sat2;
int height1, height2;
float hdop1 = 1.0;
float hdop2 = 1.0;
double Lat1 = 360.0;
double Lat2 = 0.0;
double Lon1 = 360.0;
double Lon2 = 0.0;
double speedKnot1 = 0.0;
double speedKnot2 = 0.0;
double course1 = 0.0;
double course2 = 0.0;

double averageLat = 0.0;
double averageLong = 0.0;
double sumLat = 0.0;
double sumLong = 0.0;
long   sumCounter = 0;
double sumLatSquare = 0.0;
double sumLongSquare = 0.0;
double squareLatVariant = 0.0;
double squareLongVariant = 0.0;
double xVariant, yVariant;


long ts1, ts2;
void getGPSInfo2()
{
  static long loopCount = 1;

  if (gps2.location.isValid() )
    //check if GPS is fixed
  {
    if (millis() - 1000 > ts2) { //update every second
      int Year = gps2.date.year();
      byte Month = gps2.date.month();
      byte Day = gps2.date.day();
      byte Hour = gps2.time.hour();
      byte Minute = gps2.time.minute();
      byte Second = gps2.time.second();
      byte centiss = gps2.time.centisecond();
      speedKnot2 = gps2.speed.knots();
      course2 = gps2.course.deg();
      num_sat2 = gps2.satellites.value();
      height2 = gps2.altitude.meters() - Alt;
      hdop2 = (float)gps2.hdop.value() / 100.0;
      Lat2 = gps2.location.lat();
      Lon2 = gps2.location.lng();

      if (printRMS == true) {
        sumLat = sumLat + Lat2;
        sumLong = sumLong + Lon2;
        sumCounter = sumCounter + 1;
        averageLat = sumLat / (double)sumCounter;
        averageLong = sumLong / (double)sumCounter;
        sumLatSquare = sumLatSquare + Lat2 * Lat2;
        sumLongSquare = sumLongSquare + Lon2 * Lon2;
        squareLatVariant = sumLatSquare / (double)sumCounter - averageLat * averageLat;
        squareLongVariant = sumLongSquare / (double)sumCounter - averageLong * averageLong;
        if ( squareLatVariant < 0.0 ) squareLatVariant = 0.0;
        if ( squareLongVariant < 0.0  ) squareLongVariant = 0.0;
        squareLatVariant = sqrt(squareLatVariant);
        squareLongVariant = sqrt(squareLongVariant);
        //      Serial.printf("%d, %d , %1.16f , %1.16f , %f, %f, %3.6f, %3.6f, %3.6f, %3.6f \r\n", \
        millis(), sumCounter, squareLatVariant, squareLongVariant, sumLatSquare, sumLongSquare, sumLat, sumLong, averageLat, averageLong);

        yVariant = EARTH * squareLatVariant * (PI / 180.0);
        xVariant = EARTH * cos(Lat2 * PI / 180) * squareLongVariant * (PI / 180.0);
        double dRMS =  sqrt(xVariant * xVariant + yVariant * yVariant);//dRMS
        Serial.printf("GPS RMS: %.1f m\r\n", dRMS);
        //        Serial.printf("%d, %d , %1.16f , %1.16f , %3.6f, %3.6f, %3.6f, %3.6f \r\n", millis(), sumCounter, squareLatVariant, squareLongVariant, sumLat, sumLong, averageLat, averageLong);
      }

      loopCount++;
      digitalWrite(GPIO_NUM_2, loopCount % 2);
      ts2 = millis();

      if (gps1.location.isValid()) {
        double k = (1.0 / hdop2) / (1.0 / hdop1 + 1.0 / hdop2); // weight factor
        // Serial.printf("hdop1=%f hdop2=%f k=%f\r\n",hdop1,hdop2,k);
        if (Lat1 != 360.0 ) {
#define WEIGHTED_AVERAGE
#ifndef WEIGHTED_AVERAGE
          printNMEA(Year, Month, Day, Hour, Minute, Second, centiss, \
                    (Lat2 + Lat1) / 2.0  , (Lon2 + Lon1) / 2.0 , \
                    (height1 + height2) / 2.0 , (hdop1 + hdop2) / 2.0, \
                    (num_sat1 + num_sat2) / 2 ,  (course1 + course2) / 2.0 ,  \
                    (speedKnot1 + speedKnot2) / 2.0 ) ;
#else
          printNMEA(Year, Month, Day, Hour, Minute, Second, centiss, \
                    k * Lat2 + (1.0 - k)*Lat1  , k * Lon2 + (1.0 - k)*Lon1 , \
                    k * height1 + (1.0 - k)*height2 , (hdop1 + hdop2) / 2.0, \
                    (num_sat1 + num_sat2) / 2 ,  k * course1 + (1.0 - k)*course2 ,  \
                    k * speedKnot1 + (1.0 - k)*speedKnot2 ) ;
#endif

        }
      }
    }
  }

}

void getGPSInfo1()
{
  static long loopCount = 1;

  if (gps1.location.isValid() )
    //check if GPS is fixed
  {
    // if (millis() - 1000 > ts1) { //update every second
    /*    int Year = gps1.date.year();
        byte Month = gps1.date.month();
        byte Day = gps1.date.day();
        byte Hour = gps1.time.hour();
        byte Minute = gps1.time.minute();
        byte Second = gps1.time.second();
        byte centiSecond = gps1.time.centisecond();
        double speedKnot1 = gps1.speed.knots();
        double course1 = gps1.course.deg();
    *///not used
    num_sat1 = gps1.satellites.value();
    height1 = gps1.altitude.meters() - Alt;
    hdop1 = (float)gps1.hdop.value() / 100.0;
    Lat1 = gps1.location.lat();
    Lon1 = gps1.location.lng();

    loopCount++;
    ts1 = millis();
    // }
  }
}




//  Watch dog for BT serial
void codeForBTserial(void * parameter)
{
  portTickType xLastWakeTime;
  const portTickType xFrequency = 1000;//run here every 1000msec
  while (1) {
    xLastWakeTime = xTaskGetTickCount();// Initialise the xLastWakeTime variable with the current time.
    if ( (millis() - lastGPSread) > iIntervalTime * 1000  ) {
      Serial.println();
      Serial.println();
      Serial.printf("Serial BT hang? rebooting now %d \r\n", lastGPSread);
      ESP.restart();
    }
    vTaskDelayUntil( &xLastWakeTime, xFrequency );
  }
}

void setup() {
  Serial.begin(115200);

  pinMode(GPIO_NUM_2, OUTPUT); //LED
  digitalWrite(GPIO_NUM_2, LOW);

  Serial.print("ESP32-dual-GPS-BTserial.ino "); Serial.println(VERSION);
  Serial.println("Setup Android terminal to pair with GPS-BT-server");
  SerialBT.begin("GPS-BT-server");
  Serial.println("The device started, now you can pair it with bluetooth!");
  //GPS port definition
  Serial1.begin(GPS_BAUD, SERIAL_8N1 , GPIO_NUM_33, GPIO_NUM_34);
  Serial2.begin(GPS_BAUD);// GPIO_NUM_16 for RX (GPS TX) ,GPIO_NUM_17 for TX (GPS RX)
  // start timer interrupt
  // ticker.attach(iIntervalTime, kickRoutine);
  lastGPSread = millis();
  ts1 = millis();
  ts2 = millis();
  xTaskCreatePinnedToCore( codeForBTserial, "BTserialWatch", 4000, NULL, 1, &Task1, 1); //core 1
}

void loop() {
  char c;
  char buf[NMEA_BUFSIZE];

  //#define PRINTNMEA
#ifdef PRINTNMEA
  if (Serial2.available() > 0) {
    c = Serial2.read();
    Serial.print(c);
    lastGPSread = millis();
  }
#else
  static int i = 0;
  if (Serial2.available() > 0) {
    c = Serial2.read();
    if ( i > NMEA_BUFSIZE - 2) {
      //     Serial.println("buffer overflow");
      i = 0;
    }
    buf[i++] = c;
    //Serial.print(c);
    if (c == '\n') {
      buf[i] = 0x00; //terminate by 0x00
      char work[NMEA_BUFSIZE]; //seems necessary to avoid buf error while Serial.printf
      memcpy( work, buf, i + 1 );
      i = 0; //pass through $??GSV  $??GSA $??TXT
      if ( work[0] == '$' && work[4] == 'S' && work[5] == 'V') {
        Serial.printf("%s", work); //Serial.printf(":%s",work); //check pass through data
        SerialBT.printf("%s", work);
      }
      if ( work[0] == '$' && work[4] == 'S' && work[5] == 'A') {
        Serial.printf("%s", work);
        SerialBT.printf("%s", work);
      }
      if ( work[0] == '$' && work[4] == 'X' && work[5] == 'T') {
        Serial.printf("%s", work);
        SerialBT.printf("%s", work);
      }
    }
    // Serial.write(c); // monitor for GPS application
    lastGPSread = millis();
    //SerialBT.write(c); // NMEA can be monitored by BT-Serial application for Android
    if (gps2.encode(c)) {
      getGPSInfo2();
    }
  }
  if (Serial1.available()) {
    c = Serial1.read();
    lastGPSread = millis();
    //Serial.write(c); // monitor for GPS application
    if (gps1.encode(c)) {
      getGPSInfo1();
    }

  }
#endif
}