![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Bmag incl gps rettelse
Dependencies: mbed WDT MODSERIAL BME280
SPS/SPS.cpp@7:872984a67d5b, 2017-03-15 (annotated)
- Committer:
- MAA
- Date:
- Wed Mar 15 13:56:53 2017 +0000
- Revision:
- 7:872984a67d5b
- Parent:
- 6:6d1683c8b26b
- Child:
- 8:d6287c33b54a
ERR og ERRS tilf?jet til SPS klassen.; ; Filnavn?ndring ved dato?ndring i GPRMC streng tilf?jet.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
MAA | 0:b3313c5ffca3 | 1 | #include "SPS.h" |
MAA | 0:b3313c5ffca3 | 2 | |
MAA | 0:b3313c5ffca3 | 3 | SPS::SPS(){ |
MAA | 2:39c4a85dc2a4 | 4 | |
MAA | 6:6d1683c8b26b | 5 | fileNameString = ""; |
MAA | 2:39c4a85dc2a4 | 6 | currentString = ""; |
MAA | 2:39c4a85dc2a4 | 7 | currentErrString = ""; |
MAA | 6:6d1683c8b26b | 8 | lastString = ""; |
MAA | 4:c70ef089a3fd | 9 | headerString = "/tag identifier_id group_id date time zzz | s1 source_id interpreter_id data_line_version encoding source_firmware_version interpreter_firmware_version | latitude longitude gpsFixFlag mag_time mag_nt mag_sq checksum"; |
MAA | 5:11782a2008c2 | 10 | crc_tab16_init = 0; |
MAA | 5:11782a2008c2 | 11 | errStatus = true; |
MAA | 2:39c4a85dc2a4 | 12 | |
MAA | 2:39c4a85dc2a4 | 13 | }; |
MAA | 2:39c4a85dc2a4 | 14 | |
MAA | 5:11782a2008c2 | 15 | void SPS::UpdateCurrentString(string tag, string identifier_id, string group_id, string date, string time, string ZZZ,string s1, string source_id, string interpreter_id, string data_line_version, string source_firmware_version, string interpreter_firmware_version, string latitude, string longitude, bool gpsFixFlag, string mag_time, string mag_nt, string mag_sq, Serial * dbg){ |
MAA | 2:39c4a85dc2a4 | 16 | |
MAA | 2:39c4a85dc2a4 | 17 | currentString = ""; |
MAA | 5:11782a2008c2 | 18 | currentString.resize(256); |
MAA | 5:11782a2008c2 | 19 | char checkSum[5]; |
MAA | 5:11782a2008c2 | 20 | memset(checkSum,'\0',5); |
MAA | 5:11782a2008c2 | 21 | unsigned short chkSum = 0; |
MAA | 0:b3313c5ffca3 | 22 | |
MAA | 5:11782a2008c2 | 23 | addToCurrentString(LINESTART); |
MAA | 5:11782a2008c2 | 24 | addToCurrentString(tag); |
MAA | 5:11782a2008c2 | 25 | addToCurrentString(SPACE); |
MAA | 5:11782a2008c2 | 26 | addToCurrentString(identifier_id); |
MAA | 5:11782a2008c2 | 27 | addToCurrentString(SPACE); |
MAA | 5:11782a2008c2 | 28 | addToCurrentString(group_id); |
MAA | 5:11782a2008c2 | 29 | addToCurrentString(SPACE); |
MAA | 5:11782a2008c2 | 30 | addToCurrentString(date); |
MAA | 5:11782a2008c2 | 31 | addToCurrentString(SPACE); |
MAA | 5:11782a2008c2 | 32 | addToCurrentString(time); |
MAA | 5:11782a2008c2 | 33 | addToCurrentString(SPACE); |
MAA | 5:11782a2008c2 | 34 | addToCurrentString("ZZZ"); |
MAA | 5:11782a2008c2 | 35 | addToCurrentString(SPACE); |
MAA | 5:11782a2008c2 | 36 | addToCurrentString(HEADEREND); |
MAA | 5:11782a2008c2 | 37 | addToCurrentString(SPACE); |
MAA | 5:11782a2008c2 | 38 | addToCurrentString(s1); |
MAA | 5:11782a2008c2 | 39 | addToCurrentString(SPACE); |
MAA | 5:11782a2008c2 | 40 | addToCurrentString(source_id); |
MAA | 5:11782a2008c2 | 41 | addToCurrentString(SPACE); |
MAA | 5:11782a2008c2 | 42 | addToCurrentString(interpreter_id); |
MAA | 5:11782a2008c2 | 43 | addToCurrentString(SPACE); |
MAA | 5:11782a2008c2 | 44 | addToCurrentString(data_line_version); |
MAA | 5:11782a2008c2 | 45 | addToCurrentString(SPACE); |
MAA | 5:11782a2008c2 | 46 | addToCurrentString(source_firmware_version); |
MAA | 5:11782a2008c2 | 47 | addToCurrentString(SPACE); |
MAA | 5:11782a2008c2 | 48 | addToCurrentString(interpreter_firmware_version); |
MAA | 5:11782a2008c2 | 49 | addToCurrentString(SPACE); |
MAA | 5:11782a2008c2 | 50 | addToCurrentString(HEADEREND); |
MAA | 5:11782a2008c2 | 51 | addToCurrentString(SPACE); |
MAA | 5:11782a2008c2 | 52 | addToCurrentString(latitude); |
MAA | 5:11782a2008c2 | 53 | addToCurrentString(SPACE); |
MAA | 5:11782a2008c2 | 54 | addToCurrentString(longitude); |
MAA | 5:11782a2008c2 | 55 | addToCurrentString(SPACE); |
MAA | 5:11782a2008c2 | 56 | |
MAA | 5:11782a2008c2 | 57 | if(gpsFixFlag){ |
MAA | 5:11782a2008c2 | 58 | addToCurrentString('1'); |
MAA | 5:11782a2008c2 | 59 | } |
MAA | 5:11782a2008c2 | 60 | if(!gpsFixFlag){ |
MAA | 5:11782a2008c2 | 61 | addToCurrentString('0'); |
MAA | 5:11782a2008c2 | 62 | } |
MAA | 5:11782a2008c2 | 63 | |
MAA | 5:11782a2008c2 | 64 | addToCurrentString(SPACE); |
MAA | 5:11782a2008c2 | 65 | addToCurrentString(mag_time); |
MAA | 5:11782a2008c2 | 66 | addToCurrentString(SPACE); |
MAA | 5:11782a2008c2 | 67 | addToCurrentString(mag_nt); |
MAA | 5:11782a2008c2 | 68 | addToCurrentString(SPACE); |
MAA | 5:11782a2008c2 | 69 | addToCurrentString(mag_sq); |
MAA | 5:11782a2008c2 | 70 | |
MAA | 6:6d1683c8b26b | 71 | //dbg->printf("CurrentStringBeforeChecksumCalc: "); |
MAA | 6:6d1683c8b26b | 72 | //dbg->printf(currentString.c_str()); |
MAA | 6:6d1683c8b26b | 73 | //dbg->printf("\r\n"); |
MAA | 2:39c4a85dc2a4 | 74 | |
MAA | 2:39c4a85dc2a4 | 75 | //calculate checksum |
MAA | 5:11782a2008c2 | 76 | for(int i = 0; i < strlen(currentString.c_str()); i++){ |
MAA | 5:11782a2008c2 | 77 | if(currentString[i] != '>' || currentString[i] != '<'){ |
MAA | 5:11782a2008c2 | 78 | |
MAA | 5:11782a2008c2 | 79 | chkSum = update_crc_16(chkSum, currentString[i]); |
MAA | 5:11782a2008c2 | 80 | |
MAA | 5:11782a2008c2 | 81 | } |
MAA | 5:11782a2008c2 | 82 | } |
MAA | 5:11782a2008c2 | 83 | |
MAA | 6:6d1683c8b26b | 84 | sprintf(checkSum, "%04X", chkSum); |
MAA | 2:39c4a85dc2a4 | 85 | |
MAA | 2:39c4a85dc2a4 | 86 | //append rest of string |
MAA | 5:11782a2008c2 | 87 | addToCurrentString(SPACE); |
MAA | 5:11782a2008c2 | 88 | addToCurrentString(checkSum); |
MAA | 5:11782a2008c2 | 89 | addToCurrentString(LINESTOP); |
MAA | 2:39c4a85dc2a4 | 90 | |
MAA | 2:39c4a85dc2a4 | 91 | }; |
MAA | 2:39c4a85dc2a4 | 92 | |
MAA | 7:872984a67d5b | 93 | //Updates current ERR string |
MAA | 7:872984a67d5b | 94 | void SPS::UpdateCurrentErrString(string tag, string identifier_id, string group_id, string date, string time, string ZZZ,string s1, string source_id, string interpreter_id, string data_line_version, string source_firmware_version, string interpreter_firmware_version, string latitude, string longitude, bool gpsFixFlag, string mag_time, string mag_nt, string mag_sq, Serial * dbg){ |
MAA | 7:872984a67d5b | 95 | |
MAA | 7:872984a67d5b | 96 | currentErrString = ""; |
MAA | 7:872984a67d5b | 97 | currentErrString.resize(256); |
MAA | 7:872984a67d5b | 98 | char checkSum[5]; |
MAA | 7:872984a67d5b | 99 | memset(checkSum,'\0',5); |
MAA | 7:872984a67d5b | 100 | unsigned short chkSum = 0; |
MAA | 7:872984a67d5b | 101 | |
MAA | 7:872984a67d5b | 102 | addToCurrentErrString(LINESTART); |
MAA | 7:872984a67d5b | 103 | addToCurrentErrString(tag); |
MAA | 7:872984a67d5b | 104 | addToCurrentErrString(SPACE); |
MAA | 7:872984a67d5b | 105 | addToCurrentErrString(identifier_id); |
MAA | 7:872984a67d5b | 106 | addToCurrentErrString(SPACE); |
MAA | 7:872984a67d5b | 107 | addToCurrentErrString(group_id); |
MAA | 7:872984a67d5b | 108 | addToCurrentErrString(SPACE); |
MAA | 7:872984a67d5b | 109 | addToCurrentErrString(date); |
MAA | 7:872984a67d5b | 110 | addToCurrentErrString(SPACE); |
MAA | 7:872984a67d5b | 111 | addToCurrentErrString(time); |
MAA | 7:872984a67d5b | 112 | addToCurrentErrString(SPACE); |
MAA | 7:872984a67d5b | 113 | addToCurrentErrString("ZZZ"); |
MAA | 7:872984a67d5b | 114 | addToCurrentErrString(SPACE); |
MAA | 7:872984a67d5b | 115 | addToCurrentErrString(HEADEREND); |
MAA | 7:872984a67d5b | 116 | addToCurrentErrString(SPACE); |
MAA | 7:872984a67d5b | 117 | addToCurrentErrString(s1); |
MAA | 7:872984a67d5b | 118 | addToCurrentErrString(SPACE); |
MAA | 7:872984a67d5b | 119 | addToCurrentErrString(source_id); |
MAA | 7:872984a67d5b | 120 | addToCurrentErrString(SPACE); |
MAA | 7:872984a67d5b | 121 | addToCurrentErrString(interpreter_id); |
MAA | 7:872984a67d5b | 122 | addToCurrentErrString(SPACE); |
MAA | 7:872984a67d5b | 123 | addToCurrentErrString(data_line_version); |
MAA | 7:872984a67d5b | 124 | addToCurrentErrString(SPACE); |
MAA | 7:872984a67d5b | 125 | addToCurrentErrString(source_firmware_version); |
MAA | 7:872984a67d5b | 126 | addToCurrentErrString(SPACE); |
MAA | 7:872984a67d5b | 127 | addToCurrentErrString(interpreter_firmware_version); |
MAA | 7:872984a67d5b | 128 | addToCurrentErrString(SPACE); |
MAA | 7:872984a67d5b | 129 | addToCurrentErrString(HEADEREND); |
MAA | 7:872984a67d5b | 130 | addToCurrentErrString(SPACE); |
MAA | 7:872984a67d5b | 131 | addToCurrentErrString("BMAG"); |
MAA | 7:872984a67d5b | 132 | addToCurrentErrString(SPACE); |
MAA | 7:872984a67d5b | 133 | addToCurrentErrString("C02"); |
MAA | 7:872984a67d5b | 134 | addToCurrentErrString(SPACE); |
MAA | 7:872984a67d5b | 135 | addToCurrentErrString("C03"); |
MAA | 7:872984a67d5b | 136 | |
MAA | 7:872984a67d5b | 137 | //calculate checksum |
MAA | 7:872984a67d5b | 138 | for(int i = 0; i < strlen(currentString.c_str()); i++){ |
MAA | 7:872984a67d5b | 139 | if(currentString[i] != '>' || currentString[i] != '<'){ |
MAA | 7:872984a67d5b | 140 | |
MAA | 7:872984a67d5b | 141 | chkSum = update_crc_16(chkSum, currentErrString[i]); |
MAA | 7:872984a67d5b | 142 | |
MAA | 7:872984a67d5b | 143 | } |
MAA | 7:872984a67d5b | 144 | } |
MAA | 7:872984a67d5b | 145 | |
MAA | 7:872984a67d5b | 146 | sprintf(checkSum, "%04X", chkSum); |
MAA | 7:872984a67d5b | 147 | |
MAA | 7:872984a67d5b | 148 | //append rest of string |
MAA | 7:872984a67d5b | 149 | addToCurrentErrString(SPACE); |
MAA | 7:872984a67d5b | 150 | addToCurrentErrString(checkSum); |
MAA | 7:872984a67d5b | 151 | addToCurrentErrString(LINESTOP); |
MAA | 7:872984a67d5b | 152 | |
MAA | 7:872984a67d5b | 153 | |
MAA | 7:872984a67d5b | 154 | }; |
MAA | 7:872984a67d5b | 155 | |
MAA | 4:c70ef089a3fd | 156 | //Fills the array for calculation of the CRC-16 with values |
MAA | 4:c70ef089a3fd | 157 | void SPS::init_crc16_tab() |
MAA | 4:c70ef089a3fd | 158 | { |
MAA | 4:c70ef089a3fd | 159 | int i, j; |
MAA | 4:c70ef089a3fd | 160 | unsigned short crc, c; |
MAA | 4:c70ef089a3fd | 161 | |
MAA | 4:c70ef089a3fd | 162 | for (i = 0; i < 256; i++) |
MAA | 4:c70ef089a3fd | 163 | { |
MAA | 4:c70ef089a3fd | 164 | crc = 0; |
MAA | 4:c70ef089a3fd | 165 | c = (unsigned short) i; |
MAA | 4:c70ef089a3fd | 166 | |
MAA | 4:c70ef089a3fd | 167 | for (j = 0; j < 8; j++) |
MAA | 4:c70ef089a3fd | 168 | { |
MAA | 4:c70ef089a3fd | 169 | if ((crc ^ c) & 0x0001) |
MAA | 4:c70ef089a3fd | 170 | crc = (crc >> 1) ^ P_16; |
MAA | 4:c70ef089a3fd | 171 | else |
MAA | 4:c70ef089a3fd | 172 | crc = crc >> 1; |
MAA | 4:c70ef089a3fd | 173 | |
MAA | 4:c70ef089a3fd | 174 | c = c >> 1; |
MAA | 4:c70ef089a3fd | 175 | } |
MAA | 4:c70ef089a3fd | 176 | crc_tab16[i] = crc; |
MAA | 4:c70ef089a3fd | 177 | } |
MAA | 4:c70ef089a3fd | 178 | crc_tab16_init = 1; |
MAA | 4:c70ef089a3fd | 179 | }; |
MAA | 4:c70ef089a3fd | 180 | |
MAA | 4:c70ef089a3fd | 181 | |
MAA | 4:c70ef089a3fd | 182 | // The function update_crc_16 calculates a new CRC-16 value based on the |
MAA | 4:c70ef089a3fd | 183 | // previous value of the CRC and the next byte of the data to be checked. |
MAA | 4:c70ef089a3fd | 184 | unsigned short SPS::update_crc_16(unsigned short crc, char c) |
MAA | 4:c70ef089a3fd | 185 | { |
MAA | 4:c70ef089a3fd | 186 | unsigned short tmp, short_c; |
MAA | 4:c70ef089a3fd | 187 | |
MAA | 4:c70ef089a3fd | 188 | short_c = 0x00ff & (unsigned short) c; |
MAA | 4:c70ef089a3fd | 189 | |
MAA | 4:c70ef089a3fd | 190 | if (!crc_tab16_init) |
MAA | 4:c70ef089a3fd | 191 | init_crc16_tab(); |
MAA | 4:c70ef089a3fd | 192 | |
MAA | 4:c70ef089a3fd | 193 | tmp = crc ^ short_c; |
MAA | 4:c70ef089a3fd | 194 | crc = (crc >> 8) ^ crc_tab16[tmp & 0xff]; |
MAA | 4:c70ef089a3fd | 195 | |
MAA | 4:c70ef089a3fd | 196 | return crc; |
MAA | 4:c70ef089a3fd | 197 | }; |
MAA | 5:11782a2008c2 | 198 | |
MAA | 5:11782a2008c2 | 199 | |
MAA | 5:11782a2008c2 | 200 | //Gets / returns current sps data string |
MAA | 5:11782a2008c2 | 201 | string SPS::getCurrentString(void){ |
MAA | 5:11782a2008c2 | 202 | return this->currentString; |
MAA | 5:11782a2008c2 | 203 | }; |
MAA | 5:11782a2008c2 | 204 | |
MAA | 5:11782a2008c2 | 205 | |
MAA | 5:11782a2008c2 | 206 | //Get error status |
MAA | 5:11782a2008c2 | 207 | bool SPS::getErrStatus(void){ |
MAA | 5:11782a2008c2 | 208 | return this->errStatus; |
MAA | 5:11782a2008c2 | 209 | }; |
MAA | 5:11782a2008c2 | 210 | |
MAA | 5:11782a2008c2 | 211 | //Set err status |
MAA | 5:11782a2008c2 | 212 | void SPS::setErrStatus(bool status){ |
MAA | 5:11782a2008c2 | 213 | this->errStatus = status; |
MAA | 5:11782a2008c2 | 214 | }; |
MAA | 5:11782a2008c2 | 215 | |
MAA | 5:11782a2008c2 | 216 | //get headerstring |
MAA | 5:11782a2008c2 | 217 | string SPS::getHeaderString(void){ |
MAA | 5:11782a2008c2 | 218 | return this->headerString; |
MAA | 5:11782a2008c2 | 219 | }; |
MAA | 5:11782a2008c2 | 220 | |
MAA | 5:11782a2008c2 | 221 | //add data to current string |
MAA | 5:11782a2008c2 | 222 | void SPS::addToCurrentString(string data){ |
MAA | 5:11782a2008c2 | 223 | int currentStartIndex = strlen(currentString.c_str()); |
MAA | 5:11782a2008c2 | 224 | char dataStrLen = 0; |
MAA | 5:11782a2008c2 | 225 | dataStrLen = strlen(data.c_str()); |
MAA | 5:11782a2008c2 | 226 | |
MAA | 5:11782a2008c2 | 227 | for(int i = 0; i < dataStrLen; i++){ |
MAA | 5:11782a2008c2 | 228 | currentString[currentStartIndex+i] = data[i]; |
MAA | 5:11782a2008c2 | 229 | } |
MAA | 5:11782a2008c2 | 230 | } |
MAA | 5:11782a2008c2 | 231 | |
MAA | 5:11782a2008c2 | 232 | void SPS::addToCurrentString(char data){ |
MAA | 5:11782a2008c2 | 233 | int currentStartIndex = strlen(currentString.c_str()); |
MAA | 5:11782a2008c2 | 234 | currentString[currentStartIndex] = data; |
MAA | 6:6d1683c8b26b | 235 | }; |
MAA | 6:6d1683c8b26b | 236 | |
MAA | 7:872984a67d5b | 237 | //add data to current Errstring |
MAA | 7:872984a67d5b | 238 | void SPS::addToCurrentErrString(string data){ |
MAA | 7:872984a67d5b | 239 | int currentStartIndex = strlen(currentErrString.c_str()); |
MAA | 7:872984a67d5b | 240 | char dataStrLen = 0; |
MAA | 7:872984a67d5b | 241 | dataStrLen = strlen(data.c_str()); |
MAA | 7:872984a67d5b | 242 | |
MAA | 7:872984a67d5b | 243 | for(int i = 0; i < dataStrLen; i++){ |
MAA | 7:872984a67d5b | 244 | currentErrString[currentStartIndex+i] = data[i]; |
MAA | 7:872984a67d5b | 245 | } |
MAA | 7:872984a67d5b | 246 | |
MAA | 7:872984a67d5b | 247 | }; |
MAA | 7:872984a67d5b | 248 | void SPS::addToCurrentErrString(char data){ |
MAA | 7:872984a67d5b | 249 | int currentStartIndex = strlen(currentErrString.c_str()); |
MAA | 7:872984a67d5b | 250 | currentErrString[currentStartIndex] = data; |
MAA | 7:872984a67d5b | 251 | |
MAA | 7:872984a67d5b | 252 | }; |
MAA | 7:872984a67d5b | 253 | |
MAA | 6:6d1683c8b26b | 254 | |
MAA | 6:6d1683c8b26b | 255 | void SPS::generateSpsFilename(string formattedDate){ |
MAA | 6:6d1683c8b26b | 256 | |
MAA | 7:872984a67d5b | 257 | fileNameString.resize(25); |
MAA | 6:6d1683c8b26b | 258 | |
MAA | 7:872984a67d5b | 259 | fileNameString.replace(0, 10, "/usb/BMAG_"); |
MAA | 6:6d1683c8b26b | 260 | |
MAA | 7:872984a67d5b | 261 | fileNameString[10] = formattedDate[0]; |
MAA | 7:872984a67d5b | 262 | fileNameString[11] = formattedDate[1]; |
MAA | 7:872984a67d5b | 263 | fileNameString[12] = formattedDate[2]; |
MAA | 7:872984a67d5b | 264 | fileNameString[13] = formattedDate[3]; |
MAA | 7:872984a67d5b | 265 | fileNameString[14] = '-'; |
MAA | 7:872984a67d5b | 266 | fileNameString[15] = formattedDate[5]; |
MAA | 7:872984a67d5b | 267 | fileNameString[16] = formattedDate[6]; |
MAA | 7:872984a67d5b | 268 | fileNameString[17] = '-'; |
MAA | 7:872984a67d5b | 269 | fileNameString[18] = formattedDate[8]; |
MAA | 7:872984a67d5b | 270 | fileNameString[19] = formattedDate[9]; |
MAA | 6:6d1683c8b26b | 271 | |
MAA | 7:872984a67d5b | 272 | fileNameString[20] = '.'; |
MAA | 7:872984a67d5b | 273 | fileNameString[21] = 's'; |
MAA | 7:872984a67d5b | 274 | fileNameString[22] = 'p'; |
MAA | 7:872984a67d5b | 275 | fileNameString[23] = 's'; |
MAA | 6:6d1683c8b26b | 276 | |
MAA | 6:6d1683c8b26b | 277 | }; |
MAA | 6:6d1683c8b26b | 278 | |
MAA | 6:6d1683c8b26b | 279 | |
MAA | 6:6d1683c8b26b | 280 | string SPS::getSpsFileName(void){ |
MAA | 6:6d1683c8b26b | 281 | |
MAA | 6:6d1683c8b26b | 282 | return this->fileNameString; |
MAA | 6:6d1683c8b26b | 283 | |
MAA | 7:872984a67d5b | 284 | }; |
MAA | 7:872984a67d5b | 285 | |
MAA | 7:872984a67d5b | 286 | //Gets / returns current sps err string |
MAA | 7:872984a67d5b | 287 | string SPS::getCurrentErrString(void){ |
MAA | 7:872984a67d5b | 288 | return this->currentErrString; |
MAA | 5:11782a2008c2 | 289 | }; |