Bmag incl gps rettelse
Dependencies: mbed WDT MODSERIAL BME280
NMEA/NMEA.cpp
- Committer:
- MAA
- Date:
- 2017-03-09
- Revision:
- 5:11782a2008c2
- Parent:
- 4:c70ef089a3fd
- Child:
- 18:87f243168f05
File content as of revision 5:11782a2008c2:
#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); currentLongitude.resize(15); currentLatitude.resize(15); chkSumGps.resize(2); memset(tmpArrDeg,'\0',7); memset(tmpArrMin, '\0', 7); memset(tmpLongitude, '\0', 15); memset(tmpLatitude, '\0', 15); iterationIndex = 0; longitudeDD = 0.0; latitudeDD = 0.0; north = false; south = false; east = false; west = false; }; //Returns true if GPS CRC corresponds to calculated CRC, else false. bool NMEA::ValidateData(string cstStr){ //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; } iterationIndex = 0; //initializing calculated checksum chkSumGps = " "; //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]; } //Moving the checksum received into chkSumGps iterationIndex++; chkSumGps[0] = cstStr[iterationIndex]; iterationIndex++; chkSumGps[1] = cstStr[iterationIndex]; GpsCheck = strtol(chkSumGps.c_str(), NULL ,16); //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){ static std::string gga = "$GPGGA"; static std::string rmc = "$GPRMC"; //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); } //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); } }; //Parser / formatter function to get current UTC timestamp from GPRMC void NMEA::ParseCurrentUTCFromGPRMC(){ //getting utc from GPRMC string, assign to tmpStr getXFromNMEAString(1,currentGPRMCString); 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(){ //Getting date from GPRMC string, assigning to tmpStr getXFromNMEAString(9,currentGPRMCString); 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]; } }; //Verification of gps fix, returns gps fix value character in ascii bool NMEA::GGAFixVerification(){ //accessing the string after the sixth comma, which is the gga fix column char commaCount = 0; char index = 0; char stringLength = strlen(currentGGAString.c_str()); while((commaCount < 7) && (currentGGAString[index] != '\0') && (index < stringLength) ){ 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'){ return true; } if(currentGGAString[index+1] == '2'){ return true; } break; } index += 1; } return false; }; //Parser getting current Latitude void NMEA::ParseCurrentLatitudeFromGPRMC(){ north = false; south = false; memset(tmpArrDeg,'\0',7); memset(tmpArrMin, '\0', 7); memset(tmpLatitude, '\0', 15); currentLatitude = ""; currentLatitude.resize(20); //Getting Latitude from GPRMC string, assigning to tmpStr getXFromNMEAString(3,currentGPRMCString); tmpArrDeg[0] = tmpStr[0]; tmpArrDeg[1] = tmpStr[1]; tmpArrMin[0] = tmpStr[2]; tmpArrMin[1] = tmpStr[3]; tmpArrMin[2] = tmpStr[4]; tmpArrMin[3] = tmpStr[5]; tmpArrMin[4] = tmpStr[6]; //getting N/S getXFromNMEAString(4,currentGPRMCString); if(tmpStr[0] == 'N' || tmpStr[0] == 'n'){ north = true; currentLatitude[0] = '+'; } if(tmpStr[0] == 'S' || tmpStr[0] == 's'){ south = true; currentLatitude[0] = '-'; } latitudeDD = ((atof(tmpArrDeg) + (atof(tmpArrMin)/60))); sprintf(tmpLatitude, "%0.6f",latitudeDD); for(int i = 0; i < strlen(tmpLatitude); i++){ currentLatitude[i+1] = tmpLatitude[i]; } }; //Parser getting current Longitude void NMEA::ParseCurrentLongitudeFromGPRMC(){ east = false; west = false; memset(tmpArrDeg,'\0',7); memset(tmpArrMin, '\0', 7); memset(tmpLongitude, '\0', 15); currentLongitude = ""; currentLongitude.resize(20); //Getting Longitude from GPRMC string, assigning to tmpStr getXFromNMEAString(5,currentGPRMCString); tmpArrDeg[0] = tmpStr[0]; tmpArrDeg[1] = tmpStr[1]; tmpArrDeg[2] = tmpStr[2]; tmpArrMin[0] = tmpStr[3]; tmpArrMin[1] = tmpStr[4]; tmpArrMin[2] = tmpStr[5]; tmpArrMin[3] = tmpStr[6]; tmpArrMin[4] = tmpStr[7]; tmpArrMin[5] = tmpStr[8]; tmpArrMin[6] = tmpStr[9]; //getting E/W getXFromNMEAString(6,currentGPRMCString); if(tmpStr[0] == 'E' || tmpStr[0] == 'e'){ east = true; currentLongitude[0] = '+'; } if(tmpStr[0] == 'W' || tmpStr[0] == 'w'){ west = true; currentLongitude[0] = '-'; } longitudeDD = ((atof(tmpArrDeg) + (atof(tmpArrMin)/60))); sprintf(tmpLongitude, "%0.6f",longitudeDD); for(int i = 0; i < strlen(tmpLongitude); i++){ currentLongitude[i+1] = tmpLongitude[i]; } }; void NMEA::getXFromNMEAString(int desiredCommaCount, string stringToParse){ //clearing tmpStr tmpStr = ""; tmpStr.resize(15); char stringLength = strlen(stringToParse.c_str()); int commaCnt = 0; int index = 0; int idx = 0; while(commaCnt < (desiredCommaCount+1) && (index < stringLength)){ if(stringToParse[index] == ','){ commaCnt += 1; } if((commaCnt == desiredCommaCount) && (stringToParse[index] != ',')){ tmpStr[idx] = stringToParse[index]; idx += 1; } index += 1; } };