SkyTEM BMAG / Mbed 2 deprecated BMAGThrRev

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?

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 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 };