MEGR 3171 Fall 2018 Parachute Speedometer

// This #include statement was automatically added by the Particle IDE.
#include <TinyGPS++.h>

// This #include statement was automatically added by the Particle IDE.
#include <elapsedMillis.h>



//This Sketch decodes the GPS and outputs a frequency based on the speed, D0 and  D2 is a frequency based on # of satellites
//Mode can be changed from the cloud. 0 = gps based freq output
//test frequency can be changed from the cloud.  frequency is in hertz.

/*                                    +-----+
 *                          +----------| USB |----------+
 *                          |          +-----+       *  |
 *                          | [ ] VIN           3V3 [ ] |
 *                          | [ ] GND           RST [ ] |
 *                          | [ ] TX           VBAT [ ] |
 *                M8N GPS ->| [ ] RX  [S]   [R] GND [ ] |
 *                          | [ ] WKP            D7 [*] |<-LED triggers jumpered to pin D2
 *                          | [ ] DAC +-------+  D6 [*] |
 *                          | [ ] A5  |   *   |  D5 [*] |
 *                          | [ ] A4  |Photon |  D4 [*] |
 *                          | [ ] A3  |       |  D3 [*] |
 *                          | [ ] A2  +-------+  D2 [*] |<- #sats in hertz
 *                          | [*] A1             D1 [*] |
 *                          | [*] A0             D0 [ ] |<- speed out in pulses, calibrated to bike cluster
 *                          |                           |
 *                           \    []         [______]  /
 *                            \_______________________/
 *
 *
 */

/*
   This sample code demonstrates the normal use of a TinyGPS++ (TinyGPSPlus) object.
   It requires the use of SoftwareSerial, and assumes that you have a
   230400-baud serial GPS device hooked up on pins 4(rx) and 3(tx).
*/



/* PWM NOTES
On the Photon and Electron, this function works on pins D0, D1, D2, D3, A4, A5, WKP, RX and TX with a caveat: 
PWM timer peripheral is duplicated on two pins (A5/D2) and (A4/D3) for 7 total independent PWM outputs. 
For example: PWM may be used on A5 while D2 is used as a GPIO, or D2 as a PWM while A5 is used as an analog input. 
However A5 and D2 cannot be used as independently controlled PWM outputs at the same time.
Additionally on the Electron, this function works on pins B0, B1, B2, B3, C4, C5.

The PWM frequency must be the same for pins in the same timer group.
On the Core, the timer groups are D0/D1, A0/A1/RX/TX, A4/A5/A6/A7.
On the Photon, the timer groups are D0/D1, D2/D3/A4/A5, WKP, RX/TX.
On the P1, the timer groups are D0/D1, D2/D3/A4/A5/P1S0/P1S1, WKP, RX/TX.
On the Electron, the timer groups are D0/D1/C4/C5, D2/D3/A4/A5/B2/B3, WKP, RX/TX, B0/B1.
*/

static const uint32_t GPSBaud1 = 9600;
static const uint32_t GPSBaud2 = 230400;

// The TinyGPS++ object
TinyGPSPlus gps;

double freqout = 0;
double gpssats = 0;
double gpscal = 2.28;//2.2727 //400hz converts to 176mph
double gpsmph = 0;
double gpsfix = 0;
int caladdress = 0;
double gpseeprom = 0;

byte setgpsbaud1[] = {0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x00, 0x84, 0x03, 0x00, 0x07, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x84, 0xE8};
byte setgpsbaud2[] = {0xB5, 0x62, 0x06, 0x00, 0x01, 0x00, 0x01, 0x08, 0x22};
byte setgpsbaud3[] = {0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x00, 0x84, 0x03, 0x00, 0x07, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x84, 0xE8};
byte setgpsbaud4[] = {0xB5, 0x62, 0x06, 0x00, 0x01, 0x00, 0x01, 0x08, 0x22};

byte setgpsrate10a[] = {0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0x64, 0x00, 0x01, 0x00, 0x01, 0x00, 0x7A, 0x12};
byte setgpsrate10b[] = {0xB5, 0x62, 0x06, 0x08, 0x00, 0x00, 0x0E, 0x30};

byte setgpstalkera[] = {0xB5, 0x62, 0x06, 0x17, 0x0C, 0x00, 0x00, 0x23, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01 ,0x00, 0x50, 0xF9};
byte setgpstalkerb[] = {0xB5, 0x62, 0x06, 0x17, 0x00, 0x00, 0x1D, 0x5D};

unsigned long lastprint = 0;
unsigned int rssiinterval = 30000;
unsigned int gpsmphinterval = 5000;

unsigned int freqmode = 0; //mode to control freq out source
double freqtestval = 0;

elapsedMillis lastprintgps;
elapsedMillis rssitimer;
elapsedMillis gpsmphtimer;

//ApplicationWatchdog wd(5000, System.reset); //5 second application watchdog
SYSTEM_MODE(SEMI_AUTOMATIC);

struct MyObject {
  uint8_t version;
  float field1;  //field1 = gpscal eeprom storage object
  uint16_t field2;
  char name[10];
};
MyObject myObj;
int addr = 0;

void setup()
{
  Particle.variable("gpsfreq", freqout);     
  Particle.variable("gpsmph", gpsmph);
  //Particle.variable("gpseeprom", gpseeprom);
  //Particle.variable("freqmode", freqmode); 
  Particle.variable("gpscal", gpscal);
  Particle.variable("gpssats", gpssats);
  Particle.variable("gpsfix", gpsfix);
  Particle.function("setmode", setmode); //functions still come up even if not in loop
  Particle.function("setfreq", setfreq);
  Particle.function("setcal", setcal);
  pinMode(D7,INPUT);
  pinMode(D0,OUTPUT);
  pinMode(D2,OUTPUT);

  EEPROM.get(addr, myObj);

  if(myObj.version != 0) {
    // EEPROM was empty -> initialize myObj
    MyObject defaultObj = { 0, 2.28f, 0, "GPSCAL" }; //2.8 is a default calibration for new units.
    //myObj = defaultObj;
    EEPROM.put(addr, defaultObj); //write new values to eeprom
    delay(100);
    EEPROM.get(addr, myObj);
  }

  gpseeprom = myObj.field1; //assign gpseeprom the bootup eeprom value
  gpscal = gpseeprom; //assign gpscal to be equal to gpseeprom
  
  Serial.begin(230400);
  //begin adafruit ultimate gps 3.0 config, start at 9600 baud, send commands to change to 115200 and update rate to 10hz.
  Serial1.begin(GPSBaud1);
  delay(50);
  Serial1.println("$PMTK251,115200*1F");//for adafruit
  delay(50);
  Serial1.write(setgpsbaud1, sizeof(setgpsbaud1)); //for m8n
  delay(50);
  Serial1.write(setgpsbaud2, sizeof(setgpsbaud2)); //for m8n
  delay(50);
  Serial1.write(setgpsbaud3, sizeof(setgpsbaud3)); //for m8n
  delay(50);
  Serial1.write(setgpsbaud4, sizeof(setgpsbaud4)); //for m8n
  
  delay(50);
  Serial1.begin(GPSBaud2);
  Serial1.println("$PMTK220,100*2F*71");
  delay(50);
  Serial1.write(setgpsrate10a, sizeof(setgpsrate10a)); //for m8n
  delay(50);
  Serial1.write(setgpsrate10b, sizeof(setgpsrate10b)); //for m8n
  delay(50);
  Serial1.write(setgpstalkera, sizeof(setgpstalkera)); //for m8n
  delay(50);
  Serial1.write(setgpstalkerb, sizeof(setgpstalkerb)); //for m8n
  

  Serial.println(F("FullExample.ino"));
  Serial.println(F("An extensive example of many interesting TinyGPS++ features"));
  Serial.print(F("Testing TinyGPS++ library v. ")); Serial.println(TinyGPSPlus::libraryVersion());
  Serial.println(F("by Mikal Hart"));
  Serial.println();
  Serial.println(F("Sats HDOP Latitude   Longitude   Fix  Date       Time     Date Alt    Course Speed Card  Distance Course Card  Chars Sentences Checksum"));
  Serial.println(F("          (deg)      (deg)       Age                      Age  (m)    --- from GPS ----  ---- to London  ----  RX    RX        Fail"));
  Serial.println(F("---------------------------------------------------------------------------------------------------------------------------------------"));
  
Particle.subscribe("Reading_Confirmed", myHandler2);

pinMode(D5,OUTPUT);
 digitalWrite(D5,0);

}

void myHandler2(const char *event, const char *data)
{
   digitalWrite(D5,1);
   
   
   
   
 
}

void loop()
{
      if (System.buttonPushed() > 500) {
        //RGB.color(255, 255, 0); // YELLOW
        if (Particle.connected() == false) {  //if not connected, delay, 5000ms before attempting reconnect.  without delay was causing gps to fail.
            Serial.println("Connecting to wifi");
		    Particle.connect();
		    delay(1000);
        } else {
            //Particle.disconnect();
            WiFi.off();
            delay(1000);
        }   
    }
  
    if (rssitimer > rssiinterval)   //publish rssi signal
    {
        char publishString[40];
        rssitimer = 0;
        int rssival = WiFi.RSSI();
        sprintf(publishString,"%d",rssival);
        Particle.publish("RSSI",publishString);
    }   
     if (gpsmphtimer > gpsmphinterval)   //publish speed signal
    {
        char publishString[40];
        gpsmphtimer = 0;
        int gpsmphval = gps.speed.mph();
        sprintf(publishString,"%d",gpsmphval);
        Particle.publish("ParaSpeed3171",publishString);
    } 
            
  static const double LONDON_LAT = 51.508131, LONDON_LON = -0.128002;

  //if (lastprintgps> 500){
     //lastprintgps= 0;
    printInt(gps.satellites.value(), gps.satellites.isValid(), 5);
    printInt(gps.hdop.value(), gps.hdop.isValid(), 5);
    printFloat(gps.location.lat(), gps.location.isValid(), 11, 6);
    printFloat(gps.location.lng(), gps.location.isValid(), 12, 6);
    printInt(gps.location.age(), gps.location.isValid(), 5);
    printDateTime(gps.date, gps.time);
    printFloat(gps.altitude.meters(), gps.altitude.isValid(), 7, 2);
    printFloat(gps.course.deg(), gps.course.isValid(), 7, 2);
    printFloat(gps.speed.kmph(), gps.speed.isValid(), 6, 2);
    printStr(gps.course.isValid() ? TinyGPSPlus::cardinal(gps.course.value()) : "*** ", 6);
    Serial.println();
  //}
  //delay(1);
  //Serial.println("             ");
    
    if((gps.satellites.age()>1500)||(gps.satellites.isValid()==FALSE)){
        gpssats = 0;
    }else {
        gpssats = gps.satellites.value();
    }

    if (gps.altitude.isValid()==TRUE) {
      gpsfix = 3;
    }else if (gps.speed.isValid()==TRUE) {
      gpsfix = 2;
  } else {
      gpsfix = 0;
    }

    switch (freqmode) 
        {
            case 0:
                if (gps.speed.isValid()==TRUE && (millis() >8000)){    //if a valid speed is output, then update freq output
                    gpsmph = gps.speed.mph();
                    freqout = gpsmph*gpscal;
                } else { //if no valid speed
                    gpssats = min(gpssats,8);
                    freqout = (gpssats*11)*gpscal; //if no speed is valid, output # of satellites , 1sat = 11 2sat = 22 3sat =33
                }
                
                if (gps.speed.age() > 1000){ //if the fix is too old, in this mode turn off the outputs.
                gpsmph = 0;
                gpsfix = 0;
                freqout = 0;
                gpssats = 0;
                }
                
                break;
            case 1: //mode 1 is output the test value frequency directly
                freqout = freqtestval;
                break;
            case 2: //mode 2 is output the test value times the calibration factor. for verifying mph.
                    freqout = freqtestval*gpscal;
                break;
        } 
        
    if (freqout < .5)
    {
        digitalWrite(D0,LOW);  //if frequency output is less than .5hz, then silence the pulse output.
    } else {
        analogWrite(D0, 50, freqout);
    }


    if (gpssats < .1)
    {
        digitalWrite(D2,LOW);  //if no sats, the digitalwrite was required to silence the pwm output.
    } else {
        analogWrite(D2, 50, gpssats);
    }

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

// 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 printFloat(float val, bool valid, int len, int prec)
{
  if (!valid)
  {
    while (len-- > 1)
      Serial.print('*');
    Serial.print(' ');
  }
  else
  {
    Serial.print(val, prec);
    int vi = abs((int)val);
    int flen = prec + (val < 0.0 ? 2 : 1); // . and -
    flen += vi >= 1000 ? 4 : vi >= 100 ? 3 : vi >= 10 ? 2 : 1;
    for (int i=flen; i<len; ++i)
      Serial.print(' ');
  }
  smartDelay(0);
}

static void printInt(unsigned long val, bool valid, int len)
{
  char sz[32] = "*****************";
  if (valid)
    sprintf(sz, "%ld", val);
  sz[len] = 0;
  for (int i=strlen(sz); i<len; ++i)
    sz[i] = ' ';
  if (len > 0) 
    sz[len-1] = ' ';
  Serial.print(sz);
  smartDelay(0);
}

static void printDateTime(TinyGPSDate &d, TinyGPSTime &t)
{
  if (!d.isValid())
  {
    Serial.print(F("********** "));
  }
  else
  {
    char sz[32];
    sprintf(sz, "%02d/%02d/%02d ", d.month(), d.day(), d.year());
    Serial.print(sz);
  }
  
  if (!t.isValid())
  {
    Serial.print(F("******** "));
  }
  else
  {
    char sz[32];
    sprintf(sz, "%02d:%02d:%02d ", t.hour(), t.minute(), t.second());
    Serial.print(sz);
  }

  printInt(d.age(), d.isValid(), 5);
  smartDelay(0);
}

static void printStr(const char *str, int len)
{
  int slen = strlen(str);
  for (int i=0; i<len; ++i)
    Serial.print(i<slen ? str[i] : ' ');
  smartDelay(0);
}

int setmode(String controlstring) //function to set the gps freq output mode.
{
    int controlint = controlstring.toInt();
    freqmode = controlint;
    return controlint;
}

int setfreq(String controlstring) //function to set the gps freq output mode.
{
    int controlint = controlstring.toInt();
    freqtestval = controlint;
    return controlint;
}

int setcal(String controlstring) //function to set the gps freq output mode.
{
    float controlval = -1;
    if (controlstring.toFloat() > 0)
    {
        controlval = controlstring.toFloat();
        MyObject myObj; //declare new object to read eprom values, using a different object to be sure a read was succesful
        EEPROM.get(addr, myObj); //read eeprom
        float tmpfloat =myObj.field2+1;
        MyObject myObjx = { 0, controlval, tmpfloat, "GPSCAL" }; //declare new value to store values.
        gpscal = double(controlval); //copy controlval to gpscal, which is a double 
        EEPROM.put(addr, myObjx); //write new values to eeprom
        delay(100); //delay seemed to be necessary to allow write to occur
        EEPROM.get(addr, myObj); //read eeprom
        Serial.println();
        Serial.println(myObj.field1); //debug eeprom calibration print
        gpseeprom = myObj.field1; //allows for double checking that eeprom was correctly written.
        delay(100);
    }
    
    
    return controlval;
}