SkyTEM BMAG / Mbed 2 deprecated BMAGThrRev

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?

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