Diff: GwoopsGPS.cpp

Revision:

21:dfa20810d1d0

Child:

22:9551b2ce17c9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GwoopsGPS.cpp	Thu Mar 16 23:10:57 2017 +0000
@@ -0,0 +1,242 @@
+#include "GwoopsGPS.h"
+
+/******************************************************************************
+* INTERNAL HELPER FUNCTIONS
+******************************************************************************/
+double charStrToDouble(char* str) {
+    uint16_t decpos = 0;
+    double num = 0.0;
+    while((str[decpos]!='.')&&(str[decpos]!='\0')) decpos++;
+    double multiplier = 1.0;
+    for(uint16_t i = 1;(decpos-i)>=0;i++) {
+        num += (multiplier*(str[decpos-i]-48));
+        multiplier *= 10.0;
+    }
+    multiplier = (1.0/10.0);
+    for(uint16_t i = 1;str[decpos+i]!='\0';i++) {
+        num += (multiplier*(str[decpos+i]-48));
+        multiplier /= 10.0;
+    }
+    return num;
+}
+
+uint16_t parseStringToGPRMCStructEntry(char* gprmcstr, char* gprmcentry, uint16_t start, uint16_t length) {
+    int16_t i=0;
+    while((gprmcstr[start] != ',')  &&
+          (gprmcstr[start] != '\0') &&
+          (gprmcstr[start] != '\n') &&
+          (i < length)) {
+        gprmcentry[i] = gprmcstr[start];
+        start++;i++;
+    }
+    while(i<=length) {
+        gprmcentry[i] = '\0';
+        i++;
+    }
+    return start + 1;
+}
+
+void moveDecimalLeft(char* num, uint16_t places) {
+    if (places == 0) return;
+    else places--;
+    uint16_t i = 0;
+    while(num[i] != '\0') {
+        if ((i >= places) && (num[i] == '.')) {
+            uint16_t j;
+            for(j=i;j>(i-places);j--) num[j] = num[j-1];
+            num[j] = num[j-1];
+            num[j-1] = '.';
+            break;
+        }
+        else i++;
+    }
+}
+
+void negateCharStrNumber(char* num, uint16_t len) {
+    for(uint16_t i=(len-1); i>0;i--) num[i] = num[i-1];
+    num[0] = '-';
+}
+
+/******************************************************************************
+* CONSTRUCTORS & DESTRUCTOR
+******************************************************************************/
+
+GwoopsGPS::GwoopsGPS(Serial *_gps) {
+    rawgpsdata = new char[GPSBUFFSIZE];
+    rawindex = 0;
+    clearAllData();
+    gps = _gps;
+    gps->attach(this, &GwoopsGPS::parseCallback, Serial::RxIrq);
+    valid = 0;
+}
+
+GwoopsGPS::~GwoopsGPS() {
+    delete rawgpsdata;
+    return;
+}
+
+
+/**************************************************************************
+* GETTERS
+*************************************************************************/
+
+char*    GwoopsGPS::getrawgpsdata() {return rawgpsdata;}
+char*    GwoopsGPS::gettime()       {return time;}
+char     GwoopsGPS::getstatus()     {return status;}
+char*    GwoopsGPS::getlatitude()   {return latitude;}
+double   GwoopsGPS::getdlatitude()  {return dlatitude;}
+char     GwoopsGPS::getlatdir()     {return latdir;}
+char*    GwoopsGPS::getlongitude()  {return longitude;}
+double   GwoopsGPS::getdlongitude() {return dlongitude;}
+char     GwoopsGPS::getlondir()     {return longdir;}
+char*    GwoopsGPS::getspeed()      {return speed;}
+char*    GwoopsGPS::gettrackangle() {return trackangle;}
+char*    GwoopsGPS::getdate()       {return date;}
+char*    GwoopsGPS::getmagvar()     {return magvar;}
+char     GwoopsGPS::getmagvardir()  {return magvardir;}
+uint8_t  GwoopsGPS::isValid()       {return valid;}
+
+/**************************************************************************
+* MEMBER FUNCTIONS
+*************************************************************************/
+
+void GwoopsGPS::writeStructToObj(GPRMCSentence *rcrd) {
+    sprintf(time, "%s", rcrd->time);
+    status =  rcrd->status;
+    sprintf(latitude, "%s", rcrd->latitude);
+    dlatitude = rcrd->dlatitude;
+    latdir = rcrd->latdir;
+    sprintf(longitude, "%s", rcrd->longitude);
+    dlongitude = rcrd->dlongitude;
+    longdir = rcrd->longdir;
+    sprintf(speed, "%s", rcrd->speed);
+    sprintf(trackangle, "%s", rcrd->trackangle);
+    sprintf(date, "%s", rcrd->date);
+    sprintf(magvar, "%s", rcrd->magvar);
+    magvardir = rcrd->magvardir;
+    return;
+}
+
+void GwoopsGPS::parseCallback() {
+    while(((rawgpsdata[rawindex] = gps->getc()) != '\n' ) 
+            && 
+           (rawindex <(GPSBUFFSIZE-1))) {
+        rawindex++;
+    }
+    __disable_irq();
+    GPRMCSentence potential;
+    if (parseRawGPSData(rawgpsdata, &potential) >= 0) {
+        clearAllData();
+        valid = 1;
+        writeStructToObj(&potential);
+    }
+    else {clearRawGPSData(); valid = 0;}
+    __enable_irq();
+    return;
+}
+
+void GwoopsGPS::clearRawGPSData(){
+    for(int i=0;i<GPSBUFFSIZE;i++) rawgpsdata[i] = '\0';
+    rawindex = 0;
+    return;
+}
+
+void GwoopsGPS::clearAllData(){
+    clearRawGPSData();
+    for (int i=0;i<(TIMEMAX+1);i++) {time[i] = '\0';}
+    status = '\0';
+    for (int i=0;i<(LATMAX+1);i++) {latitude[i] = '\0';}
+    dlatitude = 0;
+    latdir = '\0';
+    for (int i=0;i<(LONGMAX+1);i++) {longitude[i] = '\0';}
+    dlongitude = 0;
+    longdir = '\0';
+    for (int i=0;i<(SPEEDMAX+1);i++) {speed[i] = '\0';}
+    for (int i=0;i<(TRACKMAX+1);i++) {trackangle[i] = '\0';}
+    for (int i=0;i<(DATEMAX+1);i++)  {date[i] = '\0';}
+    for (int i=0;i<(MAGVARMAX+1);i++) {magvar[i] = '\0';}
+    magvardir = '\0';
+    valid = 0;
+    return;
+}
+
+int8_t GwoopsGPS::parseRawGPSData(char* gprmcstr, GPRMCSentence* gprmstruct){
+    uint16_t pos = 7;
+    char devnull[DEVNULMAX+1];
+    if ((gprmcstr[0] == '$') &&
+        (gprmcstr[1] == 'G') &&
+        (gprmcstr[2] == 'P') &&
+        (gprmcstr[3] == 'R') &&
+        (gprmcstr[4] == 'M') &&
+        (gprmcstr[5] == 'C') &&
+        (gprmcstr[6] == ',')) {
+        pos = parseStringToGPRMCStructEntry(gprmcstr,gprmstruct->time,pos,TIMEMAX);
+        if (gprmcstr[pos] != ',') pos = parseStringToGPRMCStructEntry(gprmcstr,devnull,pos,DEVNULMAX); // to get rid of time decimal, needed for some gps modules
+        if (gprmcstr[pos] != ',') {gprmstruct->status = gprmcstr[pos];pos++;}
+        else gprmstruct->status = '\0';
+        pos++;
+
+        pos = parseStringToGPRMCStructEntry(gprmcstr,gprmstruct->latitude,pos,LATMAX);
+        moveDecimalLeft(gprmstruct->latitude,2);
+        gprmstruct->dlatitude = charStrToDouble(gprmstruct->latitude);
+        if (gprmcstr[pos] != ',') {gprmstruct->latdir = gprmcstr[pos];pos++;}
+        else gprmstruct->latdir = '\0';
+        if(gprmstruct->latdir == 'S') {
+            negateCharStrNumber(gprmstruct->latitude, LATMAX);
+            gprmstruct->dlatitude *= -1;
+        }
+        pos++;
+
+        pos = parseStringToGPRMCStructEntry(gprmcstr,gprmstruct->longitude,pos,LONGMAX);
+        moveDecimalLeft(gprmstruct->longitude,2);
+        gprmstruct->dlongitude = charStrToDouble(gprmstruct->longitude);
+        if (gprmcstr[pos] != ',') {gprmstruct->longdir = gprmcstr[pos];pos++;}
+        else gprmstruct->longdir = '\0';
+        if(gprmstruct->longdir == 'W') {
+            negateCharStrNumber(gprmstruct->longitude, LONGMAX);
+            gprmstruct->dlongitude *= -1;
+        }
+        pos++;
+
+        pos = parseStringToGPRMCStructEntry(gprmcstr,gprmstruct->speed,pos,SPEEDMAX);
+        pos = parseStringToGPRMCStructEntry(gprmcstr,gprmstruct->trackangle,pos,TRACKMAX);
+        pos = parseStringToGPRMCStructEntry(gprmcstr,gprmstruct->date,pos,DATEMAX);
+        pos = parseStringToGPRMCStructEntry(gprmcstr,gprmstruct->magvar,pos,MAGVARMAX);
+        if (gprmcstr[pos] != ',') {gprmstruct->magvardir = gprmcstr[pos];pos++;}
+        else gprmstruct->magvardir = '\0';
+        pos++;
+        if (gprmstruct->status == 'A') return 1;
+        else return 0;
+    }
+    else return -1;
+}
+
+void GwoopsGPS::printData(Serial *serial){
+    serial->printf("Time:    %s\n\r",time);
+    serial->printf("Status:  %c\n\r",status);
+    serial->printf("Lat:     %s\n\r",latitude);
+    serial->printf("LatDir:  %c\n\r",latdir);
+    serial->printf("Long:    %s\n\r",longitude);
+    serial->printf("LongDir: %c\n\r",longdir);
+    serial->printf("Speed:   %s\n\r",speed);
+    serial->printf("Angle:   %s\n\r",trackangle);
+    serial->printf("Date:    %s\n\r",date);
+    serial->printf("MagVar:  %s\n\r",magvar);
+    serial->printf("MagDir:  %c\n\r",magvardir);
+    return;
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+