SkyTEM BMAG / Mbed 2 deprecated BMAGThrRev

Bmag incl gps rettelse

Dependencies:   mbed WDT MODSERIAL BME280

NMEA/NMEA.cpp@2:39c4a85dc2a4, 2017-02-23 (annotated)

Committer:
MAA
Date:
Thu Feb 23 13:01:35 2017 +0000
Revision:
2:39c4a85dc2a4
Parent:
0:b3313c5ffca3
Child:
3:38eabaa92552
DateParsing and UTC parsing now working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MAA 0:b3313c5ffca3 1 #include "NMEA.h"
MAA 0:b3313c5ffca3 2
MAA 0:b3313c5ffca3 3 //constructor with a MODSERIAL pointer to the GPS comms as argument
MAA 0:b3313c5ffca3 4 NMEA::NMEA(){
MAA 0:b3313c5ffca3 5
MAA 0:b3313c5ffca3 6 currentGGAString.resize(128);
MAA 0:b3313c5ffca3 7 currentGPRMCString.resize(128);
MAA 0:b3313c5ffca3 8 currentUTCFromGPRMC.resize(20);
MAA 0:b3313c5ffca3 9 currentDATEFromGPRMC.resize(10);
MAA 0:b3313c5ffca3 10
MAA 0:b3313c5ffca3 11 chkSumGps.resize(2);
MAA 0:b3313c5ffca3 12
MAA 0:b3313c5ffca3 13 iterationIndex = 0;
MAA 0:b3313c5ffca3 14 };
MAA 0:b3313c5ffca3 15
MAA 0:b3313c5ffca3 16 //Returns true if GPS CRC corresponds to calculated CRC, else false.
MAA 0:b3313c5ffca3 17 bool NMEA::ValidateData(string cstStr, Serial *debg){
MAA 0:b3313c5ffca3 18
MAA 0:b3313c5ffca3 19 //debg->printf("Starting data validation!\r\n");
MAA 0:b3313c5ffca3 20
MAA 0:b3313c5ffca3 21 //Temporary variables used locally only
MAA 0:b3313c5ffca3 22 chkSum = 0;
MAA 0:b3313c5ffca3 23
MAA 0:b3313c5ffca3 24 //If for some reason the index is wrong, and the first character is not
MAA 0:b3313c5ffca3 25 //the beginning dollar sign of a gps string, return false at once.
MAA 0:b3313c5ffca3 26 if(cstStr[0] != '$'){
MAA 0:b3313c5ffca3 27 return false;
MAA 0:b3313c5ffca3 28 }
MAA 0:b3313c5ffca3 29 //debg->printf("First character of gps string is $\r\n");
MAA 0:b3313c5ffca3 30
MAA 0:b3313c5ffca3 31 iterationIndex = 0;
MAA 0:b3313c5ffca3 32
MAA 0:b3313c5ffca3 33 //initializing calculated checksum
MAA 0:b3313c5ffca3 34 chkSumGps = " ";
MAA 0:b3313c5ffca3 35
MAA 0:b3313c5ffca3 36 //debg->printf("Rx checksum string cleared!\r\n");
MAA 0:b3313c5ffca3 37
MAA 0:b3313c5ffca3 38 //Calculating checksum to verify string integrity
MAA 0:b3313c5ffca3 39 for(char i = 1; i < cstStr.size(); i++){
MAA 0:b3313c5ffca3 40 //If at end of line for crc check, break
MAA 0:b3313c5ffca3 41 if(cstStr[i] == '*' || cstStr[i] == '\0'){
MAA 0:b3313c5ffca3 42 iterationIndex = i;
MAA 0:b3313c5ffca3 43 break;
MAA 0:b3313c5ffca3 44 }
MAA 0:b3313c5ffca3 45 //XOR crc calculation
MAA 0:b3313c5ffca3 46 chkSum ^= cstStr[i];
MAA 0:b3313c5ffca3 47
MAA 0:b3313c5ffca3 48 }
MAA 0:b3313c5ffca3 49
MAA 0:b3313c5ffca3 50 //debg->printf("The calculated checksum is: %d \r\n", chkSum);
MAA 0:b3313c5ffca3 51
MAA 0:b3313c5ffca3 52 //Moving the checksum received into chkSumGps
MAA 0:b3313c5ffca3 53 iterationIndex++;
MAA 0:b3313c5ffca3 54 chkSumGps[0] = cstStr[iterationIndex];
MAA 0:b3313c5ffca3 55 iterationIndex++;
MAA 0:b3313c5ffca3 56 chkSumGps[1] = cstStr[iterationIndex];
MAA 0:b3313c5ffca3 57
MAA 0:b3313c5ffca3 58 GpsCheck = strtol(chkSumGps.c_str(), NULL ,16);
MAA 0:b3313c5ffca3 59
MAA 0:b3313c5ffca3 60 //debg->printf("The received checksum is: %d \r\n", GpsCheck);
MAA 0:b3313c5ffca3 61
MAA 0:b3313c5ffca3 62 //Verify received and calculated crc equality
MAA 0:b3313c5ffca3 63 if(GpsCheck == chkSum){
MAA 0:b3313c5ffca3 64 return true;
MAA 0:b3313c5ffca3 65 }
MAA 0:b3313c5ffca3 66
MAA 0:b3313c5ffca3 67 //If unequal crc bytes found return false
MAA 0:b3313c5ffca3 68 return false;
MAA 0:b3313c5ffca3 69 };
MAA 0:b3313c5ffca3 70
MAA 0:b3313c5ffca3 71
MAA 0:b3313c5ffca3 72 //Store function to move GGA to currentGGAString,
MAA 0:b3313c5ffca3 73 //or GPRMC to currentGPRMCString
MAA 2:39c4a85dc2a4 74 void NMEA::StoreString(string cstStr/*, Serial * debg*/){
MAA 0:b3313c5ffca3 75 static std::string gga = "$GPGGA";
MAA 0:b3313c5ffca3 76 static std::string rmc = "$GPRMC";
MAA 0:b3313c5ffca3 77
MAA 0:b3313c5ffca3 78 //debg->printf("Storing received strings\r\n");
MAA 0:b3313c5ffca3 79
MAA 0:b3313c5ffca3 80 //If received string is a GPGGA string
MAA 0:b3313c5ffca3 81 //copy GPGGA string to currentGGAString
MAA 0:b3313c5ffca3 82 if(cstStr.compare(0, 6, gga) == 0){
MAA 0:b3313c5ffca3 83 //replace current valid gga string with new valid gga string
MAA 0:b3313c5ffca3 84 currentGGAString.replace(0, 128, cstStr);
MAA 0:b3313c5ffca3 85 //debg->printf("GGA string replaced with: ");
MAA 0:b3313c5ffca3 86 //debg->printf(currentGGAString.c_str());
MAA 0:b3313c5ffca3 87 //debg->printf("\r\n");
MAA 0:b3313c5ffca3 88 }
MAA 0:b3313c5ffca3 89
MAA 0:b3313c5ffca3 90 //If received string is a GPRMC string
MAA 0:b3313c5ffca3 91 //copy GPRMC string to currentGPRMCString
MAA 0:b3313c5ffca3 92 if(cstStr.compare(0, 6, rmc) == 0){
MAA 0:b3313c5ffca3 93 //replace current valid rmc string with new valid rmc string
MAA 0:b3313c5ffca3 94 currentGPRMCString.replace(0, 128, cstStr);
MAA 0:b3313c5ffca3 95 //debg->printf("GPRMC string replaced with: ");
MAA 0:b3313c5ffca3 96 //debg->printf(currentGPRMCString.c_str());
MAA 0:b3313c5ffca3 97 //debg->printf("\r\n");
MAA 0:b3313c5ffca3 98 }
MAA 0:b3313c5ffca3 99
MAA 0:b3313c5ffca3 100 };
MAA 0:b3313c5ffca3 101
MAA 0:b3313c5ffca3 102
MAA 0:b3313c5ffca3 103 //Parser / formatter function to get current UTC timestamp from GPRMC
MAA 2:39c4a85dc2a4 104 void NMEA::ParseCurrentUTCFromGPRMC(Serial * dbg){
MAA 0:b3313c5ffca3 105
MAA 2:39c4a85dc2a4 106 //getting utc from GPRMC string, assign to tmpStr
MAA 2:39c4a85dc2a4 107 getXFromNMEAString(1,currentGPRMCString, dbg);
MAA 2:39c4a85dc2a4 108
MAA 2:39c4a85dc2a4 109 currentUTCFromGPRMC = "";
MAA 2:39c4a85dc2a4 110 currentUTCFromGPRMC.resize(20);
MAA 2:39c4a85dc2a4 111
MAA 2:39c4a85dc2a4 112 //if time format == 10 characters
MAA 2:39c4a85dc2a4 113 if(strlen(tmpStr.c_str()) == 10){
MAA 2:39c4a85dc2a4 114
MAA 2:39c4a85dc2a4 115 //HH
MAA 2:39c4a85dc2a4 116 currentUTCFromGPRMC[0] = tmpStr[0];
MAA 2:39c4a85dc2a4 117 currentUTCFromGPRMC[1] = tmpStr[1];
MAA 2:39c4a85dc2a4 118 currentUTCFromGPRMC[2] = ':';
MAA 2:39c4a85dc2a4 119
MAA 2:39c4a85dc2a4 120 //MM
MAA 2:39c4a85dc2a4 121 currentUTCFromGPRMC[3] = tmpStr[2];
MAA 2:39c4a85dc2a4 122 currentUTCFromGPRMC[4] = tmpStr[3];
MAA 2:39c4a85dc2a4 123 currentUTCFromGPRMC[5] = ':';
MAA 2:39c4a85dc2a4 124
MAA 2:39c4a85dc2a4 125 //SS
MAA 2:39c4a85dc2a4 126 currentUTCFromGPRMC[6] = tmpStr[4];
MAA 2:39c4a85dc2a4 127 currentUTCFromGPRMC[7] = tmpStr[5];
MAA 2:39c4a85dc2a4 128 currentUTCFromGPRMC[8] = '.';
MAA 2:39c4a85dc2a4 129
MAA 2:39c4a85dc2a4 130 //FFF
MAA 2:39c4a85dc2a4 131 currentUTCFromGPRMC[9] = tmpStr[7];
MAA 2:39c4a85dc2a4 132 currentUTCFromGPRMC[10] = tmpStr[8];
MAA 2:39c4a85dc2a4 133 currentUTCFromGPRMC[11] = tmpStr[9];
MAA 2:39c4a85dc2a4 134 }
MAA 2:39c4a85dc2a4 135
MAA 2:39c4a85dc2a4 136 //if time format == 6 characters
MAA 2:39c4a85dc2a4 137 if(strlen(tmpStr.c_str()) == 6){
MAA 2:39c4a85dc2a4 138
MAA 2:39c4a85dc2a4 139 //HH
MAA 2:39c4a85dc2a4 140 currentUTCFromGPRMC[0] = tmpStr[0];
MAA 2:39c4a85dc2a4 141 currentUTCFromGPRMC[1] = tmpStr[1];
MAA 2:39c4a85dc2a4 142 currentUTCFromGPRMC[2] = ':';
MAA 2:39c4a85dc2a4 143
MAA 2:39c4a85dc2a4 144 //MM
MAA 2:39c4a85dc2a4 145 currentUTCFromGPRMC[3] = tmpStr[2];
MAA 2:39c4a85dc2a4 146 currentUTCFromGPRMC[4] = tmpStr[3];
MAA 2:39c4a85dc2a4 147 currentUTCFromGPRMC[5] = ':';
MAA 2:39c4a85dc2a4 148
MAA 2:39c4a85dc2a4 149 //SS
MAA 2:39c4a85dc2a4 150 currentUTCFromGPRMC[6] = tmpStr[4];
MAA 2:39c4a85dc2a4 151 currentUTCFromGPRMC[7] = tmpStr[5];
MAA 2:39c4a85dc2a4 152 currentUTCFromGPRMC[8] = '.';
MAA 2:39c4a85dc2a4 153
MAA 2:39c4a85dc2a4 154 //FFF
MAA 2:39c4a85dc2a4 155 currentUTCFromGPRMC[9] = '0';
MAA 2:39c4a85dc2a4 156 currentUTCFromGPRMC[10] = '0';
MAA 2:39c4a85dc2a4 157 currentUTCFromGPRMC[11] = '0';
MAA 2:39c4a85dc2a4 158 }
MAA 0:b3313c5ffca3 159
MAA 0:b3313c5ffca3 160 };
MAA 0:b3313c5ffca3 161
MAA 0:b3313c5ffca3 162 //Parser getting current Date from GPRMC
MAA 2:39c4a85dc2a4 163 void NMEA::ParseCurrentDateFromGPRMC(Serial * dbg){
MAA 2:39c4a85dc2a4 164
MAA 2:39c4a85dc2a4 165 //Getting date from GPRMC string, assigning to tmpStr
MAA 2:39c4a85dc2a4 166 getXFromNMEAString(9,currentGPRMCString, dbg);
MAA 0:b3313c5ffca3 167
MAA 2:39c4a85dc2a4 168 currentDATEFromGPRMC = "";
MAA 2:39c4a85dc2a4 169 currentDATEFromGPRMC.resize(20);
MAA 2:39c4a85dc2a4 170
MAA 2:39c4a85dc2a4 171 //if date string is the expected length
MAA 2:39c4a85dc2a4 172 if(strlen(tmpStr.c_str()) == 6){
MAA 2:39c4a85dc2a4 173
MAA 2:39c4a85dc2a4 174 //Year
MAA 2:39c4a85dc2a4 175 currentDATEFromGPRMC[0] = '2';
MAA 2:39c4a85dc2a4 176 currentDATEFromGPRMC[1] = '0';
MAA 2:39c4a85dc2a4 177 currentDATEFromGPRMC[2] = tmpStr[4];
MAA 2:39c4a85dc2a4 178 currentDATEFromGPRMC[3] = tmpStr[5];
MAA 2:39c4a85dc2a4 179 currentDATEFromGPRMC[4] = '/';
MAA 2:39c4a85dc2a4 180
MAA 2:39c4a85dc2a4 181 //Month
MAA 2:39c4a85dc2a4 182 currentDATEFromGPRMC[5] = tmpStr[2];
MAA 2:39c4a85dc2a4 183 currentDATEFromGPRMC[6] = tmpStr[3];
MAA 2:39c4a85dc2a4 184 currentDATEFromGPRMC[7] = '/';
MAA 2:39c4a85dc2a4 185
MAA 2:39c4a85dc2a4 186 //Date
MAA 2:39c4a85dc2a4 187 currentDATEFromGPRMC[8] = tmpStr[0];
MAA 2:39c4a85dc2a4 188 currentDATEFromGPRMC[9] = tmpStr[1];
MAA 2:39c4a85dc2a4 189 }
MAA 2:39c4a85dc2a4 190
MAA 2:39c4a85dc2a4 191
MAA 2:39c4a85dc2a4 192 dbg->printf("currentDATEFromGPRMC: ");
MAA 2:39c4a85dc2a4 193 dbg->printf(currentDATEFromGPRMC.c_str());
MAA 2:39c4a85dc2a4 194 dbg->printf("\r\n");
MAA 2:39c4a85dc2a4 195
MAA 0:b3313c5ffca3 196 };
MAA 0:b3313c5ffca3 197
MAA 0:b3313c5ffca3 198 //Verification of gps fix, returns gps fix value character in ascii
MAA 2:39c4a85dc2a4 199 bool NMEA::GGAFixVerification(/*Serial * debg*/){
MAA 0:b3313c5ffca3 200 //accessing the string after the sixth comma, which is the gga fix column
MAA 2:39c4a85dc2a4 201 //debg->printf("Commencing GGA fix verification\r\n");
MAA 0:b3313c5ffca3 202
MAA 0:b3313c5ffca3 203 char commaCount = 0;
MAA 0:b3313c5ffca3 204 char index = 0;
MAA 0:b3313c5ffca3 205
MAA 0:b3313c5ffca3 206 while((commaCount < 7) && (currentGGAString[index] != '\0')){
MAA 0:b3313c5ffca3 207 if(currentGGAString[index] == ','){
MAA 0:b3313c5ffca3 208 commaCount += 1;
MAA 0:b3313c5ffca3 209 }
MAA 0:b3313c5ffca3 210
MAA 0:b3313c5ffca3 211
MAA 0:b3313c5ffca3 212 //if gga fix is 1 or 2, gga fix is sufficient, return true
MAA 0:b3313c5ffca3 213 if(commaCount == 6){
MAA 0:b3313c5ffca3 214 if(currentGGAString[index+1] == '1'){
MAA 2:39c4a85dc2a4 215 //debg->printf("Valid GGA fix present(1)!\r\n");
MAA 0:b3313c5ffca3 216 return true;
MAA 0:b3313c5ffca3 217 }
MAA 0:b3313c5ffca3 218
MAA 0:b3313c5ffca3 219 if(currentGGAString[index+1] == '2'){
MAA 2:39c4a85dc2a4 220 //debg->printf("Valid GGA fix present(2)!\r\n");
MAA 0:b3313c5ffca3 221 return true;
MAA 0:b3313c5ffca3 222 }
MAA 0:b3313c5ffca3 223
MAA 0:b3313c5ffca3 224 break;
MAA 0:b3313c5ffca3 225 }
MAA 0:b3313c5ffca3 226
MAA 0:b3313c5ffca3 227 index += 1;
MAA 0:b3313c5ffca3 228
MAA 0:b3313c5ffca3 229 }
MAA 0:b3313c5ffca3 230
MAA 2:39c4a85dc2a4 231 //debg->printf("Invalid GGA fix!\r\n");
MAA 0:b3313c5ffca3 232 return false;
MAA 2:39c4a85dc2a4 233 };
MAA 2:39c4a85dc2a4 234
MAA 2:39c4a85dc2a4 235 void NMEA::getXFromNMEAString(int desiredCommaCount, string stringToParse, Serial * dbg){
MAA 2:39c4a85dc2a4 236
MAA 2:39c4a85dc2a4 237 //clearing tmpStr
MAA 2:39c4a85dc2a4 238 tmpStr = "";
MAA 2:39c4a85dc2a4 239 tmpStr.resize(10);
MAA 2:39c4a85dc2a4 240
MAA 2:39c4a85dc2a4 241
MAA 2:39c4a85dc2a4 242 int commaCnt = 0;
MAA 2:39c4a85dc2a4 243 int index = 0;
MAA 2:39c4a85dc2a4 244 int idx = 0;
MAA 2:39c4a85dc2a4 245
MAA 2:39c4a85dc2a4 246 while(commaCnt < (desiredCommaCount+1)){
MAA 2:39c4a85dc2a4 247
MAA 2:39c4a85dc2a4 248 if(stringToParse[index] == ','){
MAA 2:39c4a85dc2a4 249 commaCnt += 1;
MAA 2:39c4a85dc2a4 250 }
MAA 2:39c4a85dc2a4 251
MAA 2:39c4a85dc2a4 252 if((commaCnt == desiredCommaCount) && (stringToParse[index] != ',')){
MAA 2:39c4a85dc2a4 253 tmpStr[idx] = stringToParse[index];
MAA 2:39c4a85dc2a4 254 idx += 1;
MAA 2:39c4a85dc2a4 255 }
MAA 2:39c4a85dc2a4 256
MAA 2:39c4a85dc2a4 257 index += 1;
MAA 2:39c4a85dc2a4 258 }
MAA 2:39c4a85dc2a4 259
MAA 2:39c4a85dc2a4 260 dbg->printf("tmpStr: ");
MAA 2:39c4a85dc2a4 261 dbg->printf(tmpStr.c_str());
MAA 2:39c4a85dc2a4 262 dbg->printf("\r\n");
MAA 2:39c4a85dc2a4 263
MAA 0:b3313c5ffca3 264 };