Bmag incl gps rettelse
Dependencies: mbed WDT MODSERIAL BME280
NMEA/NMEA.cpp
- Committer:
- MAA
- Date:
- 2019-02-15
- Branch:
- MbedBMAGThrRev
- Revision:
- 55:06c5f76e1a8c
- Parent:
- 34:c6c5e7ec9163
- Child:
- 58:6545ef27c228
File content as of revision 55:06c5f76e1a8c:
#include "NMEA.h" //! NMEA, default constructor NMEA::NMEA(){ currentGGAString.resize(128); currentGPRMCString.resize(128); currentUTCFromGPRMC.resize(20); currentDATEFromGPRMC.resize(10); currentLongitude.resize(15); currentLatitude.resize(15); currentLongitude = "NaN"; currentLatitude = "NaN"; 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; cnt_GPRMC = 0; north = false; south = false; east = false; west = false; }; //! ValidateData, method taking a text string for NMEA validation /*! ValidateData validates that the NMEA data string isn't corrupted. (Verification of crc) \param cstStr An NMEA ascii string. \return boolean return value, true if crc corresponds to own 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; }; //! StoreString, method taking a text string as argument for further handling /*! StoreString, replaces current GPRMC or GGA string, with new valid GPRMC or GGA string. \param cstStr An NMEA ascii string. */ 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){ if(cnt_GPRMC > 1000){ cnt_GPRMC = 0; } cnt_GPRMC += 1; //replace current valid rmc string with new valid rmc string currentGPRMCString.replace(0, 128, cstStr); } }; //! ParseCurrentUTCFromGPRMC, grabs UTC timestamp from current stored GPRMC string. /*! ParseCurrentUTCFromGPRMC, grabs UTC timestamp from current stored GPRMC string. Stores the UTC timestamp in local text string. Depending on the format it will be saved as: HH:MM:SS.FFF In this case FFF are read values fom the string. eg. 14:10:22.007 or HH:MM:SS.FFF In this case FFF are predetermined values set to 0. eg. 14:10:22.000 */ void NMEA::ParseCurrentUTCFromGPRMC(){ //getting utc from GPRMC string, assigned to tmpStr getXFromNMEAString(1,currentGPRMCString); currentUTCFromGPRMC = ""; currentUTCFromGPRMC.resize(20); //if time format == 10 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] = 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'; } }; //! ParseCurrentDateFromGPRMC, grabs current date from current GPRMC string. /*! ParseCurrentDateFromGPRMC, grabs current date from current GPRMC string. Stores the Date in a local text string. will be stored in the format: YYYY/MM/DD */ 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]; } }; //! ParseCUrrentAltitudeFromGGA, grabs current altitude from current GGA string. /*! ParseCUrrentAltitudeFromGGA, grabs current altitude from current GGA string. Stores the Altitude in a local string. */ void NMEA::ParseCurrentAltitudeFromGGA(void){ //Getting date from GPRMC string, assigning to tmpStr getXFromNMEAString(9, currentGGAString); if(GGAFixVerification()){ currentAltitude = tmpStr; } else{ currentAltitude = "NaN"; } }; //! GGAFixVerification, returns gps fix indication. /*! GGAFixVerification verifies gga fix status in current GGA string. \return boolean value indicating GGA fix if true, else false. */ 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; }; //! ParseCurrentLatitudeFromGPRMC, grabs current latitude from current GPRMC string. /*! Converts the latitude into decimaldegrees and stores the 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]; } }; //! ParseCurrentLongitudeFromGPRMC, grabs current longitude from current GPRMC string. /*! Converts the longitude into decimaldegrees and stores the 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]; } }; //! getXFromNMEAString, grabs desired data column from NMEA string, stores it for further manipulation in tmpStr /*! \param desiredCommaCount integer designating which column to store \param stringToParse string parameter containing the NMEA string. */ 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; } }; //! getCurrentTime, getter method returning current time from gps /*! \return returns current time from gps in format "HH:MM:SS.FFF" */ string NMEA::getCurrentTime(void){ return this->currentUTCFromGPRMC; };