Bmag incl gps rettelse
Dependencies: mbed WDT MODSERIAL BME280
NMEA/NMEA.cpp@1:f347c4ef25fa, 2017-02-23 (annotated)
- Committer:
- MAA
- Date:
- Thu Feb 23 07:32:05 2017 +0000
- Revision:
- 1:f347c4ef25fa
- Parent:
- 0:b3313c5ffca3
uden debug, virker ikke helt korrekt uden seriel forbindelse til pc
Who changed what in which revision?
User | Revision | Line number | New 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 | 1:f347c4ef25fa | 10 | currentLongitudeFromGPRMC.resize(20); |
MAA | 1:f347c4ef25fa | 11 | currentLatitudeFromGPRMC.resize(20); |
MAA | 0:b3313c5ffca3 | 12 | chkSumGps.resize(2); |
MAA | 0:b3313c5ffca3 | 13 | |
MAA | 0:b3313c5ffca3 | 14 | iterationIndex = 0; |
MAA | 0:b3313c5ffca3 | 15 | }; |
MAA | 0:b3313c5ffca3 | 16 | |
MAA | 0:b3313c5ffca3 | 17 | //Returns true if GPS CRC corresponds to calculated CRC, else false. |
MAA | 1:f347c4ef25fa | 18 | bool NMEA::ValidateData(string cstStr){ |
MAA | 0:b3313c5ffca3 | 19 | |
MAA | 0:b3313c5ffca3 | 20 | //debg->printf("Starting data validation!\r\n"); |
MAA | 0:b3313c5ffca3 | 21 | |
MAA | 0:b3313c5ffca3 | 22 | //Temporary variables used locally only |
MAA | 0:b3313c5ffca3 | 23 | chkSum = 0; |
MAA | 0:b3313c5ffca3 | 24 | |
MAA | 0:b3313c5ffca3 | 25 | //If for some reason the index is wrong, and the first character is not |
MAA | 0:b3313c5ffca3 | 26 | //the beginning dollar sign of a gps string, return false at once. |
MAA | 0:b3313c5ffca3 | 27 | if(cstStr[0] != '$'){ |
MAA | 0:b3313c5ffca3 | 28 | return false; |
MAA | 0:b3313c5ffca3 | 29 | } |
MAA | 0:b3313c5ffca3 | 30 | //debg->printf("First character of gps string is $\r\n"); |
MAA | 0:b3313c5ffca3 | 31 | |
MAA | 0:b3313c5ffca3 | 32 | iterationIndex = 0; |
MAA | 0:b3313c5ffca3 | 33 | |
MAA | 0:b3313c5ffca3 | 34 | //initializing calculated checksum |
MAA | 0:b3313c5ffca3 | 35 | chkSumGps = " "; |
MAA | 0:b3313c5ffca3 | 36 | |
MAA | 0:b3313c5ffca3 | 37 | //debg->printf("Rx checksum string cleared!\r\n"); |
MAA | 0:b3313c5ffca3 | 38 | |
MAA | 0:b3313c5ffca3 | 39 | //Calculating checksum to verify string integrity |
MAA | 0:b3313c5ffca3 | 40 | for(char i = 1; i < cstStr.size(); i++){ |
MAA | 0:b3313c5ffca3 | 41 | //If at end of line for crc check, break |
MAA | 0:b3313c5ffca3 | 42 | if(cstStr[i] == '*' || cstStr[i] == '\0'){ |
MAA | 0:b3313c5ffca3 | 43 | iterationIndex = i; |
MAA | 0:b3313c5ffca3 | 44 | break; |
MAA | 0:b3313c5ffca3 | 45 | } |
MAA | 0:b3313c5ffca3 | 46 | //XOR crc calculation |
MAA | 0:b3313c5ffca3 | 47 | chkSum ^= cstStr[i]; |
MAA | 0:b3313c5ffca3 | 48 | |
MAA | 0:b3313c5ffca3 | 49 | } |
MAA | 0:b3313c5ffca3 | 50 | |
MAA | 0:b3313c5ffca3 | 51 | //debg->printf("The calculated checksum is: %d \r\n", chkSum); |
MAA | 0:b3313c5ffca3 | 52 | |
MAA | 0:b3313c5ffca3 | 53 | //Moving the checksum received into chkSumGps |
MAA | 0:b3313c5ffca3 | 54 | iterationIndex++; |
MAA | 0:b3313c5ffca3 | 55 | chkSumGps[0] = cstStr[iterationIndex]; |
MAA | 0:b3313c5ffca3 | 56 | iterationIndex++; |
MAA | 0:b3313c5ffca3 | 57 | chkSumGps[1] = cstStr[iterationIndex]; |
MAA | 0:b3313c5ffca3 | 58 | |
MAA | 0:b3313c5ffca3 | 59 | GpsCheck = strtol(chkSumGps.c_str(), NULL ,16); |
MAA | 0:b3313c5ffca3 | 60 | |
MAA | 0:b3313c5ffca3 | 61 | //debg->printf("The received checksum is: %d \r\n", GpsCheck); |
MAA | 0:b3313c5ffca3 | 62 | |
MAA | 0:b3313c5ffca3 | 63 | //Verify received and calculated crc equality |
MAA | 0:b3313c5ffca3 | 64 | if(GpsCheck == chkSum){ |
MAA | 0:b3313c5ffca3 | 65 | return true; |
MAA | 0:b3313c5ffca3 | 66 | } |
MAA | 0:b3313c5ffca3 | 67 | |
MAA | 0:b3313c5ffca3 | 68 | //If unequal crc bytes found return false |
MAA | 0:b3313c5ffca3 | 69 | return false; |
MAA | 0:b3313c5ffca3 | 70 | }; |
MAA | 0:b3313c5ffca3 | 71 | |
MAA | 0:b3313c5ffca3 | 72 | |
MAA | 0:b3313c5ffca3 | 73 | //Store function to move GGA to currentGGAString, |
MAA | 0:b3313c5ffca3 | 74 | //or GPRMC to currentGPRMCString |
MAA | 1:f347c4ef25fa | 75 | void NMEA::StoreString(string cstStr){ |
MAA | 0:b3313c5ffca3 | 76 | static std::string gga = "$GPGGA"; |
MAA | 0:b3313c5ffca3 | 77 | static std::string rmc = "$GPRMC"; |
MAA | 0:b3313c5ffca3 | 78 | |
MAA | 0:b3313c5ffca3 | 79 | //debg->printf("Storing received strings\r\n"); |
MAA | 0:b3313c5ffca3 | 80 | |
MAA | 0:b3313c5ffca3 | 81 | //If received string is a GPGGA string |
MAA | 0:b3313c5ffca3 | 82 | //copy GPGGA string to currentGGAString |
MAA | 0:b3313c5ffca3 | 83 | if(cstStr.compare(0, 6, gga) == 0){ |
MAA | 0:b3313c5ffca3 | 84 | //replace current valid gga string with new valid gga string |
MAA | 0:b3313c5ffca3 | 85 | currentGGAString.replace(0, 128, cstStr); |
MAA | 0:b3313c5ffca3 | 86 | //debg->printf("GGA string replaced with: "); |
MAA | 0:b3313c5ffca3 | 87 | //debg->printf(currentGGAString.c_str()); |
MAA | 0:b3313c5ffca3 | 88 | //debg->printf("\r\n"); |
MAA | 0:b3313c5ffca3 | 89 | } |
MAA | 0:b3313c5ffca3 | 90 | |
MAA | 0:b3313c5ffca3 | 91 | //If received string is a GPRMC string |
MAA | 0:b3313c5ffca3 | 92 | //copy GPRMC string to currentGPRMCString |
MAA | 0:b3313c5ffca3 | 93 | if(cstStr.compare(0, 6, rmc) == 0){ |
MAA | 0:b3313c5ffca3 | 94 | //replace current valid rmc string with new valid rmc string |
MAA | 0:b3313c5ffca3 | 95 | currentGPRMCString.replace(0, 128, cstStr); |
MAA | 0:b3313c5ffca3 | 96 | //debg->printf("GPRMC string replaced with: "); |
MAA | 0:b3313c5ffca3 | 97 | //debg->printf(currentGPRMCString.c_str()); |
MAA | 0:b3313c5ffca3 | 98 | //debg->printf("\r\n"); |
MAA | 0:b3313c5ffca3 | 99 | } |
MAA | 0:b3313c5ffca3 | 100 | |
MAA | 0:b3313c5ffca3 | 101 | }; |
MAA | 0:b3313c5ffca3 | 102 | |
MAA | 0:b3313c5ffca3 | 103 | |
MAA | 1:f347c4ef25fa | 104 | //Get current UTC timestamp from GPRMC |
MAA | 1:f347c4ef25fa | 105 | void NMEA::GetCurrentUTCFromGPRMC(){ |
MAA | 1:f347c4ef25fa | 106 | char commaCount = 0; |
MAA | 1:f347c4ef25fa | 107 | char index = 0; |
MAA | 1:f347c4ef25fa | 108 | char charCount = 0; |
MAA | 1:f347c4ef25fa | 109 | while((commaCount < 2) && (currentGPRMCString[index] != '\0')){ |
MAA | 1:f347c4ef25fa | 110 | |
MAA | 1:f347c4ef25fa | 111 | if(currentGPRMCString[index] == ','){ |
MAA | 1:f347c4ef25fa | 112 | commaCount += 1; |
MAA | 1:f347c4ef25fa | 113 | index += 1; |
MAA | 1:f347c4ef25fa | 114 | } |
MAA | 1:f347c4ef25fa | 115 | |
MAA | 1:f347c4ef25fa | 116 | if(commaCount == 1){ |
MAA | 1:f347c4ef25fa | 117 | currentUTCFromGPRMC[charCount] = currentGPRMCString[index]; |
MAA | 1:f347c4ef25fa | 118 | charCount +=1; |
MAA | 1:f347c4ef25fa | 119 | } |
MAA | 1:f347c4ef25fa | 120 | |
MAA | 1:f347c4ef25fa | 121 | index += 1; |
MAA | 1:f347c4ef25fa | 122 | } |
MAA | 1:f347c4ef25fa | 123 | |
MAA | 1:f347c4ef25fa | 124 | //debg->printf("Current UTC for GPS string: "); |
MAA | 1:f347c4ef25fa | 125 | //debg->printf(currentUTCFromGPRMC.c_str()); |
MAA | 1:f347c4ef25fa | 126 | //debg->printf("\r\n"); |
MAA | 1:f347c4ef25fa | 127 | |
MAA | 0:b3313c5ffca3 | 128 | }; |
MAA | 0:b3313c5ffca3 | 129 | |
MAA | 1:f347c4ef25fa | 130 | //Get current Date from GPRMC |
MAA | 1:f347c4ef25fa | 131 | void NMEA::GetCurrentDateFromGPRMC(){ |
MAA | 1:f347c4ef25fa | 132 | |
MAA | 1:f347c4ef25fa | 133 | char commaCount = 0; |
MAA | 1:f347c4ef25fa | 134 | char index = 0; |
MAA | 1:f347c4ef25fa | 135 | char charCount = 0; |
MAA | 1:f347c4ef25fa | 136 | while((commaCount < 10) && (currentGPRMCString[index] != '\0')){ |
MAA | 0:b3313c5ffca3 | 137 | |
MAA | 1:f347c4ef25fa | 138 | if(currentGPRMCString[index] == ','){ |
MAA | 1:f347c4ef25fa | 139 | commaCount += 1; |
MAA | 1:f347c4ef25fa | 140 | index += 1; |
MAA | 1:f347c4ef25fa | 141 | } |
MAA | 1:f347c4ef25fa | 142 | |
MAA | 1:f347c4ef25fa | 143 | if(commaCount == 9){ |
MAA | 1:f347c4ef25fa | 144 | currentDATEFromGPRMC[charCount] = currentGPRMCString[index]; |
MAA | 1:f347c4ef25fa | 145 | charCount +=1; |
MAA | 1:f347c4ef25fa | 146 | } |
MAA | 1:f347c4ef25fa | 147 | |
MAA | 1:f347c4ef25fa | 148 | index += 1; |
MAA | 1:f347c4ef25fa | 149 | } |
MAA | 1:f347c4ef25fa | 150 | |
MAA | 1:f347c4ef25fa | 151 | //debg->printf("Current Date from GPS string: "); |
MAA | 1:f347c4ef25fa | 152 | //debg->printf(currentDATEFromGPRMC.c_str()); |
MAA | 1:f347c4ef25fa | 153 | //debg->printf("\r\n"); |
MAA | 0:b3313c5ffca3 | 154 | }; |
MAA | 0:b3313c5ffca3 | 155 | |
MAA | 0:b3313c5ffca3 | 156 | //Verification of gps fix, returns gps fix value character in ascii |
MAA | 1:f347c4ef25fa | 157 | bool NMEA::GGAFixVerification(){ |
MAA | 0:b3313c5ffca3 | 158 | //accessing the string after the sixth comma, which is the gga fix column |
MAA | 1:f347c4ef25fa | 159 | //debg->printf("Commencing GGA fix verification\r\n"); |
MAA | 0:b3313c5ffca3 | 160 | |
MAA | 0:b3313c5ffca3 | 161 | char commaCount = 0; |
MAA | 0:b3313c5ffca3 | 162 | char index = 0; |
MAA | 0:b3313c5ffca3 | 163 | |
MAA | 0:b3313c5ffca3 | 164 | while((commaCount < 7) && (currentGGAString[index] != '\0')){ |
MAA | 0:b3313c5ffca3 | 165 | if(currentGGAString[index] == ','){ |
MAA | 0:b3313c5ffca3 | 166 | commaCount += 1; |
MAA | 0:b3313c5ffca3 | 167 | } |
MAA | 0:b3313c5ffca3 | 168 | |
MAA | 0:b3313c5ffca3 | 169 | |
MAA | 0:b3313c5ffca3 | 170 | //if gga fix is 1 or 2, gga fix is sufficient, return true |
MAA | 0:b3313c5ffca3 | 171 | if(commaCount == 6){ |
MAA | 0:b3313c5ffca3 | 172 | if(currentGGAString[index+1] == '1'){ |
MAA | 1:f347c4ef25fa | 173 | //debg->printf("Valid GGA fix present(1)!\r\n"); |
MAA | 0:b3313c5ffca3 | 174 | return true; |
MAA | 0:b3313c5ffca3 | 175 | } |
MAA | 0:b3313c5ffca3 | 176 | |
MAA | 0:b3313c5ffca3 | 177 | if(currentGGAString[index+1] == '2'){ |
MAA | 1:f347c4ef25fa | 178 | //debg->printf("Valid GGA fix present(2)!\r\n"); |
MAA | 0:b3313c5ffca3 | 179 | return true; |
MAA | 0:b3313c5ffca3 | 180 | } |
MAA | 0:b3313c5ffca3 | 181 | |
MAA | 0:b3313c5ffca3 | 182 | break; |
MAA | 0:b3313c5ffca3 | 183 | } |
MAA | 0:b3313c5ffca3 | 184 | |
MAA | 0:b3313c5ffca3 | 185 | index += 1; |
MAA | 0:b3313c5ffca3 | 186 | |
MAA | 0:b3313c5ffca3 | 187 | } |
MAA | 0:b3313c5ffca3 | 188 | |
MAA | 1:f347c4ef25fa | 189 | //debg->printf("Invalid GGA fix!\r\n"); |
MAA | 0:b3313c5ffca3 | 190 | return false; |
MAA | 1:f347c4ef25fa | 191 | }; |
MAA | 1:f347c4ef25fa | 192 | |
MAA | 1:f347c4ef25fa | 193 | //Get current Latitude from GPRMC |
MAA | 1:f347c4ef25fa | 194 | void NMEA::GetCurrentLatitudeFromGPRMC(){ |
MAA | 1:f347c4ef25fa | 195 | |
MAA | 1:f347c4ef25fa | 196 | char commaCount = 0; |
MAA | 1:f347c4ef25fa | 197 | char index = 0; |
MAA | 1:f347c4ef25fa | 198 | char charCount = 0; |
MAA | 1:f347c4ef25fa | 199 | while((commaCount < 4) && (currentGPRMCString[index] != '\0')){ |
MAA | 1:f347c4ef25fa | 200 | |
MAA | 1:f347c4ef25fa | 201 | if(currentGPRMCString[index] == ','){ |
MAA | 1:f347c4ef25fa | 202 | commaCount += 1; |
MAA | 1:f347c4ef25fa | 203 | index += 1; |
MAA | 1:f347c4ef25fa | 204 | } |
MAA | 1:f347c4ef25fa | 205 | |
MAA | 1:f347c4ef25fa | 206 | if(commaCount == 3){ |
MAA | 1:f347c4ef25fa | 207 | currentLatitudeFromGPRMC[charCount] = currentGPRMCString[index]; |
MAA | 1:f347c4ef25fa | 208 | charCount +=1; |
MAA | 1:f347c4ef25fa | 209 | } |
MAA | 1:f347c4ef25fa | 210 | |
MAA | 1:f347c4ef25fa | 211 | index += 1; |
MAA | 1:f347c4ef25fa | 212 | } |
MAA | 1:f347c4ef25fa | 213 | |
MAA | 1:f347c4ef25fa | 214 | //debg->printf("Current Latitude from GPS string: "); |
MAA | 1:f347c4ef25fa | 215 | //debg->printf(currentLatitudeFromGPRMC.c_str()); |
MAA | 1:f347c4ef25fa | 216 | //debg->printf("\r\n"); |
MAA | 1:f347c4ef25fa | 217 | }; |
MAA | 1:f347c4ef25fa | 218 | |
MAA | 1:f347c4ef25fa | 219 | //Get current Longitude from GPRMC |
MAA | 1:f347c4ef25fa | 220 | void NMEA::GetCurrentLongitudeFromGPRMC(){ |
MAA | 1:f347c4ef25fa | 221 | |
MAA | 1:f347c4ef25fa | 222 | char commaCount = 0; |
MAA | 1:f347c4ef25fa | 223 | char index = 0; |
MAA | 1:f347c4ef25fa | 224 | char charCount = 0; |
MAA | 1:f347c4ef25fa | 225 | while((commaCount < 6) && (currentGPRMCString[index] != '\0')){ |
MAA | 1:f347c4ef25fa | 226 | |
MAA | 1:f347c4ef25fa | 227 | if(currentGPRMCString[index] == ','){ |
MAA | 1:f347c4ef25fa | 228 | commaCount += 1; |
MAA | 1:f347c4ef25fa | 229 | index += 1; |
MAA | 1:f347c4ef25fa | 230 | } |
MAA | 1:f347c4ef25fa | 231 | |
MAA | 1:f347c4ef25fa | 232 | if(commaCount == 5){ |
MAA | 1:f347c4ef25fa | 233 | currentLongitudeFromGPRMC[charCount] = currentGPRMCString[index]; |
MAA | 1:f347c4ef25fa | 234 | charCount +=1; |
MAA | 1:f347c4ef25fa | 235 | } |
MAA | 1:f347c4ef25fa | 236 | |
MAA | 1:f347c4ef25fa | 237 | index += 1; |
MAA | 1:f347c4ef25fa | 238 | } |
MAA | 1:f347c4ef25fa | 239 | |
MAA | 1:f347c4ef25fa | 240 | //debg->printf("Current Longitude from GPS string: "); |
MAA | 1:f347c4ef25fa | 241 | //debg->printf(currentLongitudeFromGPRMC.c_str()); |
MAA | 1:f347c4ef25fa | 242 | //debg->printf("\r\n"); |
MAA | 0:b3313c5ffca3 | 243 | }; |