Bmag incl gps rettelse

Dependencies:   mbed WDT MODSERIAL BME280

NMEA/NMEA.cpp

Committer:
MAA
Date:
2017-02-23
Revision:
2:39c4a85dc2a4
Parent:
0:b3313c5ffca3
Child:
3:38eabaa92552

File content as of revision 2:39c4a85dc2a4:

#include "NMEA.h"

//constructor with a MODSERIAL pointer to the GPS comms as argument
NMEA::NMEA(){    
    
    currentGGAString.resize(128);
    currentGPRMCString.resize(128);
    currentUTCFromGPRMC.resize(20);
    currentDATEFromGPRMC.resize(10);
    
    chkSumGps.resize(2);
       
    iterationIndex = 0;    
};

//Returns true if GPS CRC corresponds to calculated CRC, else false.
bool NMEA::ValidateData(string cstStr, Serial *debg){
    
    //debg->printf("Starting data validation!\r\n");
    
    //Temporary variables used locally only
    chkSum = 0;
    
    //If for some reason the index is wrong, and the first character is not 
    //the beginning dollar sign of a gps string, return false at once.
    if(cstStr[0] != '$'){
        return false;
    }
    //debg->printf("First character of gps string is $\r\n");
    
    iterationIndex = 0;
              
    //initializing calculated checksum
    chkSumGps = "  ";
    
    //debg->printf("Rx checksum string cleared!\r\n");
    
    //Calculating checksum to verify string integrity
    for(char i = 1; i < cstStr.size(); i++){
        //If at end of line for crc check, break
        if(cstStr[i] == '*' || cstStr[i] == '\0'){
            iterationIndex = i;
            break;    
        }
        //XOR crc calculation
        chkSum ^= cstStr[i];
            
    }
    
    //debg->printf("The calculated checksum is: %d \r\n", chkSum);
    
    //Moving the checksum received into chkSumGps
    iterationIndex++;
    chkSumGps[0] = cstStr[iterationIndex];
    iterationIndex++;
    chkSumGps[1] = cstStr[iterationIndex];
    
    GpsCheck = strtol(chkSumGps.c_str(), NULL ,16);

    //debg->printf("The received checksum is: %d \r\n", GpsCheck);
    
    //Verify received and calculated crc equality
    if(GpsCheck == chkSum){
        return true;    
    }     

    //If unequal crc bytes found return false
    return false;    
};

    
//Store function to move GGA to currentGGAString, 
//or GPRMC to currentGPRMCString
void NMEA::StoreString(string cstStr/*, Serial * debg*/){
    static std::string gga = "$GPGGA";
    static std::string rmc = "$GPRMC";
    
    //debg->printf("Storing received strings\r\n");
    
    //If received string is a GPGGA string
    //copy GPGGA string to currentGGAString
    if(cstStr.compare(0, 6, gga) == 0){
        //replace current valid gga string with new valid gga string
        currentGGAString.replace(0, 128, cstStr); 
        //debg->printf("GGA string replaced with: ");  
        //debg->printf(currentGGAString.c_str());  
        //debg->printf("\r\n");          
    }

    //If received string is a GPRMC string
    //copy GPRMC string to currentGPRMCString
    if(cstStr.compare(0, 6, rmc) == 0){
        //replace current valid rmc string with new valid rmc string
        currentGPRMCString.replace(0, 128, cstStr);
        //debg->printf("GPRMC string replaced with: ");  
        //debg->printf(currentGPRMCString.c_str());
        //debg->printf("\r\n");              
    }                 
        
};


//Parser / formatter function to get current UTC timestamp from GPRMC
void NMEA::ParseCurrentUTCFromGPRMC(Serial * dbg){
 
     //getting utc from GPRMC string, assign to tmpStr
     getXFromNMEAString(1,currentGPRMCString, dbg);
     
     currentUTCFromGPRMC = "";
     currentUTCFromGPRMC.resize(20);
     
     //if time format == 10 characters
     if(strlen(tmpStr.c_str()) == 10){
         
        //HH
        currentUTCFromGPRMC[0] = tmpStr[0];
        currentUTCFromGPRMC[1] = tmpStr[1];
        currentUTCFromGPRMC[2] = ':';
     
        //MM
        currentUTCFromGPRMC[3] = tmpStr[2];
        currentUTCFromGPRMC[4] = tmpStr[3];
        currentUTCFromGPRMC[5] = ':';
     
        //SS
        currentUTCFromGPRMC[6] = tmpStr[4];
        currentUTCFromGPRMC[7] = tmpStr[5];
        currentUTCFromGPRMC[8] = '.';     
     
        //FFF
        currentUTCFromGPRMC[9] = tmpStr[7];
        currentUTCFromGPRMC[10] = tmpStr[8];
        currentUTCFromGPRMC[11] = tmpStr[9];   
     }
     
     //if time format == 6 characters
     if(strlen(tmpStr.c_str()) == 6){
         
        //HH
        currentUTCFromGPRMC[0] = tmpStr[0];
        currentUTCFromGPRMC[1] = tmpStr[1];
        currentUTCFromGPRMC[2] = ':';
     
        //MM
        currentUTCFromGPRMC[3] = tmpStr[2];
        currentUTCFromGPRMC[4] = tmpStr[3];
        currentUTCFromGPRMC[5] = ':';
     
        //SS
        currentUTCFromGPRMC[6] = tmpStr[4];
        currentUTCFromGPRMC[7] = tmpStr[5];
        currentUTCFromGPRMC[8] = '.';     
     
        //FFF
        currentUTCFromGPRMC[9] = '0';
        currentUTCFromGPRMC[10] = '0';
        currentUTCFromGPRMC[11] = '0';  
     }    

};

//Parser getting current Date from GPRMC
void NMEA::ParseCurrentDateFromGPRMC(Serial * dbg){
    
    //Getting date from GPRMC string, assigning to tmpStr 
    getXFromNMEAString(9,currentGPRMCString, dbg);

    currentDATEFromGPRMC = "";
    currentDATEFromGPRMC.resize(20);  
    
    //if date string is the expected length
    if(strlen(tmpStr.c_str()) == 6){
            
        //Year
        currentDATEFromGPRMC[0] = '2';
        currentDATEFromGPRMC[1] = '0';
        currentDATEFromGPRMC[2] = tmpStr[4];
        currentDATEFromGPRMC[3] = tmpStr[5];
        currentDATEFromGPRMC[4] = '/';
    
        //Month
        currentDATEFromGPRMC[5] = tmpStr[2];
        currentDATEFromGPRMC[6] = tmpStr[3];
        currentDATEFromGPRMC[7] = '/';
    
        //Date
        currentDATEFromGPRMC[8] = tmpStr[0];
        currentDATEFromGPRMC[9] = tmpStr[1];       
    }
    

    dbg->printf("currentDATEFromGPRMC: ");
    dbg->printf(currentDATEFromGPRMC.c_str());
    dbg->printf("\r\n");
 
};

//Verification of gps fix, returns gps fix value character in ascii
bool  NMEA::GGAFixVerification(/*Serial * debg*/){
    //accessing the string after the sixth comma, which is the gga fix column
    //debg->printf("Commencing GGA fix verification\r\n");
    
    char commaCount = 0;
    char index = 0;
    
    while((commaCount < 7) && (currentGGAString[index] != '\0')){
        if(currentGGAString[index] == ','){
            commaCount += 1;    
        }
        
        
        //if gga fix is 1 or 2, gga fix is sufficient, return true
        if(commaCount == 6){   
            if(currentGGAString[index+1] == '1'){
                //debg->printf("Valid GGA fix present(1)!\r\n"); 
                return true;    
            }
            
            if(currentGGAString[index+1] == '2'){
                //debg->printf("Valid GGA fix present(2)!\r\n"); 
                return true;    
            }
            
            break;
        }
        
        index += 1;
        
    }
    
    //debg->printf("Invalid GGA fix!\r\n");           
    return false;
};

void NMEA::getXFromNMEAString(int desiredCommaCount, string stringToParse, Serial * dbg){
    
    //clearing tmpStr
    tmpStr = "";
    tmpStr.resize(10);
    
    
    int commaCnt = 0;
    int index = 0;
    int idx = 0;    
    
    while(commaCnt < (desiredCommaCount+1)){
        
        if(stringToParse[index] == ','){
            commaCnt += 1;
        }
        
        if((commaCnt == desiredCommaCount) && (stringToParse[index] != ',')){
            tmpStr[idx] = stringToParse[index];
            idx += 1;
        }
        
        index += 1;    
    }
    
    dbg->printf("tmpStr: ");
    dbg->printf(tmpStr.c_str());
    dbg->printf("\r\n");  
    
};