Bmag incl gps rettelse
Dependencies: mbed WDT MODSERIAL BME280
SPS/SPS.cpp
- Committer:
- MAA
- Date:
- 2017-03-09
- Revision:
- 5:11782a2008c2
- Parent:
- 4:c70ef089a3fd
- Child:
- 6:6d1683c8b26b
File content as of revision 5:11782a2008c2:
#include "SPS.h" SPS::SPS(){ currentString = ""; currentErrString = ""; 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"; crc_tab16_init = 0; errStatus = true; }; 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){ currentString = ""; currentString.resize(256); char checkSum[5]; memset(checkSum,'\0',5); unsigned short chkSum = 0; dbg->printf("Commencing sps line generation\r\n"); //create current string addToCurrentString(LINESTART); addToCurrentString(tag); addToCurrentString(SPACE); addToCurrentString(identifier_id); addToCurrentString(SPACE); addToCurrentString(group_id); addToCurrentString(SPACE); addToCurrentString(date); addToCurrentString(SPACE); addToCurrentString(time); addToCurrentString(SPACE); addToCurrentString("ZZZ"); addToCurrentString(SPACE); addToCurrentString(HEADEREND); addToCurrentString(SPACE); addToCurrentString(s1); addToCurrentString(SPACE); addToCurrentString(source_id); addToCurrentString(SPACE); addToCurrentString(interpreter_id); addToCurrentString(SPACE); addToCurrentString(data_line_version); addToCurrentString(SPACE); addToCurrentString(source_firmware_version); addToCurrentString(SPACE); addToCurrentString(interpreter_firmware_version); addToCurrentString(SPACE); addToCurrentString(HEADEREND); addToCurrentString(SPACE); addToCurrentString(latitude); addToCurrentString(SPACE); addToCurrentString(longitude); addToCurrentString(SPACE); if(gpsFixFlag){ addToCurrentString('1'); } if(!gpsFixFlag){ addToCurrentString('0'); } addToCurrentString(SPACE); addToCurrentString(mag_time); addToCurrentString(SPACE); addToCurrentString(mag_nt); addToCurrentString(SPACE); addToCurrentString(mag_sq); dbg->printf("CurrentStringBeforeChecksumCalc: "); dbg->printf(currentString.c_str()); dbg->printf("\r\n"); //calculate checksum for(int i = 0; i < strlen(currentString.c_str()); i++){ if(currentString[i] != '>' || currentString[i] != '<'){ chkSum = update_crc_16(chkSum, currentString[i]); } } sprintf(checkSum, "%4x", chkSum); //append rest of string addToCurrentString(SPACE); addToCurrentString(checkSum); addToCurrentString(LINESTOP); }; //Fills the array for calculation of the CRC-16 with values void SPS::init_crc16_tab() { int i, j; unsigned short crc, c; for (i = 0; i < 256; i++) { crc = 0; c = (unsigned short) i; for (j = 0; j < 8; j++) { if ((crc ^ c) & 0x0001) crc = (crc >> 1) ^ P_16; else crc = crc >> 1; c = c >> 1; } crc_tab16[i] = crc; } crc_tab16_init = 1; }; // The function update_crc_16 calculates a new CRC-16 value based on the // previous value of the CRC and the next byte of the data to be checked. unsigned short SPS::update_crc_16(unsigned short crc, char c) { unsigned short tmp, short_c; short_c = 0x00ff & (unsigned short) c; if (!crc_tab16_init) init_crc16_tab(); tmp = crc ^ short_c; crc = (crc >> 8) ^ crc_tab16[tmp & 0xff]; return crc; }; //Gets / returns current sps data string string SPS::getCurrentString(void){ return this->currentString; }; //Get error status bool SPS::getErrStatus(void){ return this->errStatus; }; //Set err status void SPS::setErrStatus(bool status){ this->errStatus = status; }; //get headerstring string SPS::getHeaderString(void){ return this->headerString; }; //add data to current string void SPS::addToCurrentString(string data){ int currentStartIndex = strlen(currentString.c_str()); char dataStrLen = 0; dataStrLen = strlen(data.c_str()); for(int i = 0; i < dataStrLen; i++){ currentString[currentStartIndex+i] = data[i]; } } void SPS::addToCurrentString(char data){ int currentStartIndex = strlen(currentString.c_str()); currentString[currentStartIndex] = data; };