SkyTEM BMAG / Mbed 2 deprecated BMAGThrRev

Bmag incl gps rettelse

Dependencies:   mbed WDT MODSERIAL BME280

SPS/SPS.cpp@5:11782a2008c2, 2017-03-09 (annotated)

Committer:
MAA
Date:
Thu Mar 09 12:03:55 2017 +0000
Revision:
5:11782a2008c2
Parent:
4:c70ef089a3fd
Child:
6:6d1683c8b26b
fungerende *.sps linie generator, mangler lidt fintuning.; fungerende tilf?jelse af sps linier til fil p? usb pen. ; ; TODO:; Dokumentation; forbedring af opstart.; data opsamling fra MAG; udbedring af uhensigtsm?ssig "opf?rsel"

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 2:39c4a85dc2a4 5 currentString = "";
MAA 2:39c4a85dc2a4 6 currentErrString = "";
MAA 4:c70ef089a3fd 7 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 8 crc_tab16_init = 0;
MAA 5:11782a2008c2 9 errStatus = true;
MAA 2:39c4a85dc2a4 10
MAA 2:39c4a85dc2a4 11 };
MAA 2:39c4a85dc2a4 12
MAA 5:11782a2008c2 13 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 14
MAA 2:39c4a85dc2a4 15 currentString = "";
MAA 5:11782a2008c2 16 currentString.resize(256);
MAA 5:11782a2008c2 17 char checkSum[5];
MAA 5:11782a2008c2 18 memset(checkSum,'\0',5);
MAA 5:11782a2008c2 19 unsigned short chkSum = 0;
MAA 0:b3313c5ffca3 20
MAA 5:11782a2008c2 21 dbg->printf("Commencing sps line generation\r\n");
MAA 2:39c4a85dc2a4 22 //create current string
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 5:11782a2008c2 71 dbg->printf("CurrentStringBeforeChecksumCalc: ");
MAA 5:11782a2008c2 72 dbg->printf(currentString.c_str());
MAA 5:11782a2008c2 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 5:11782a2008c2 84 sprintf(checkSum, "%4x", 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 4:c70ef089a3fd 93 //Fills the array for calculation of the CRC-16 with values
MAA 4:c70ef089a3fd 94 void SPS::init_crc16_tab()
MAA 4:c70ef089a3fd 95 {
MAA 4:c70ef089a3fd 96 int i, j;
MAA 4:c70ef089a3fd 97 unsigned short crc, c;
MAA 4:c70ef089a3fd 98
MAA 4:c70ef089a3fd 99 for (i = 0; i < 256; i++)
MAA 4:c70ef089a3fd 100 {
MAA 4:c70ef089a3fd 101 crc = 0;
MAA 4:c70ef089a3fd 102 c = (unsigned short) i;
MAA 4:c70ef089a3fd 103
MAA 4:c70ef089a3fd 104 for (j = 0; j < 8; j++)
MAA 4:c70ef089a3fd 105 {
MAA 4:c70ef089a3fd 106 if ((crc ^ c) & 0x0001)
MAA 4:c70ef089a3fd 107 crc = (crc >> 1) ^ P_16;
MAA 4:c70ef089a3fd 108 else
MAA 4:c70ef089a3fd 109 crc = crc >> 1;
MAA 4:c70ef089a3fd 110
MAA 4:c70ef089a3fd 111 c = c >> 1;
MAA 4:c70ef089a3fd 112 }
MAA 4:c70ef089a3fd 113 crc_tab16[i] = crc;
MAA 4:c70ef089a3fd 114 }
MAA 4:c70ef089a3fd 115 crc_tab16_init = 1;
MAA 4:c70ef089a3fd 116 };
MAA 4:c70ef089a3fd 117
MAA 4:c70ef089a3fd 118
MAA 4:c70ef089a3fd 119 // The function update_crc_16 calculates a new CRC-16 value based on the
MAA 4:c70ef089a3fd 120 // previous value of the CRC and the next byte of the data to be checked.
MAA 4:c70ef089a3fd 121 unsigned short SPS::update_crc_16(unsigned short crc, char c)
MAA 4:c70ef089a3fd 122 {
MAA 4:c70ef089a3fd 123 unsigned short tmp, short_c;
MAA 4:c70ef089a3fd 124
MAA 4:c70ef089a3fd 125 short_c = 0x00ff & (unsigned short) c;
MAA 4:c70ef089a3fd 126
MAA 4:c70ef089a3fd 127 if (!crc_tab16_init)
MAA 4:c70ef089a3fd 128 init_crc16_tab();
MAA 4:c70ef089a3fd 129
MAA 4:c70ef089a3fd 130 tmp = crc ^ short_c;
MAA 4:c70ef089a3fd 131 crc = (crc >> 8) ^ crc_tab16[tmp & 0xff];
MAA 4:c70ef089a3fd 132
MAA 4:c70ef089a3fd 133 return crc;
MAA 4:c70ef089a3fd 134 };
MAA 5:11782a2008c2 135
MAA 5:11782a2008c2 136
MAA 5:11782a2008c2 137 //Gets / returns current sps data string
MAA 5:11782a2008c2 138 string SPS::getCurrentString(void){
MAA 5:11782a2008c2 139 return this->currentString;
MAA 5:11782a2008c2 140 };
MAA 5:11782a2008c2 141
MAA 5:11782a2008c2 142
MAA 5:11782a2008c2 143 //Get error status
MAA 5:11782a2008c2 144 bool SPS::getErrStatus(void){
MAA 5:11782a2008c2 145 return this->errStatus;
MAA 5:11782a2008c2 146 };
MAA 5:11782a2008c2 147
MAA 5:11782a2008c2 148 //Set err status
MAA 5:11782a2008c2 149 void SPS::setErrStatus(bool status){
MAA 5:11782a2008c2 150 this->errStatus = status;
MAA 5:11782a2008c2 151 };
MAA 5:11782a2008c2 152
MAA 5:11782a2008c2 153 //get headerstring
MAA 5:11782a2008c2 154 string SPS::getHeaderString(void){
MAA 5:11782a2008c2 155 return this->headerString;
MAA 5:11782a2008c2 156 };
MAA 5:11782a2008c2 157
MAA 5:11782a2008c2 158 //add data to current string
MAA 5:11782a2008c2 159 void SPS::addToCurrentString(string data){
MAA 5:11782a2008c2 160 int currentStartIndex = strlen(currentString.c_str());
MAA 5:11782a2008c2 161 char dataStrLen = 0;
MAA 5:11782a2008c2 162 dataStrLen = strlen(data.c_str());
MAA 5:11782a2008c2 163
MAA 5:11782a2008c2 164 for(int i = 0; i < dataStrLen; i++){
MAA 5:11782a2008c2 165 currentString[currentStartIndex+i] = data[i];
MAA 5:11782a2008c2 166 }
MAA 5:11782a2008c2 167 }
MAA 5:11782a2008c2 168
MAA 5:11782a2008c2 169 void SPS::addToCurrentString(char data){
MAA 5:11782a2008c2 170 int currentStartIndex = strlen(currentString.c_str());
MAA 5:11782a2008c2 171 currentString[currentStartIndex] = data;
MAA 5:11782a2008c2 172 };