void NmeaParser::parse(char letter) {
bool commit = false; // flag to commit collected sentence
bool clear = false; // flag to clear collected sentence
// check for starting symbol
bool isStart = false;
if (letter == '$') {
isStart = true;
if (state != S_SOS) {
#ifdef GLOBPOS_PRINT_ERRORS
fprintf(stderr, "Unexpected start of sentence at state %d\n", state);
#endif
state = S_SOS;
commit = true; // maybe there was something valid
}
}
// check for ending symbol
if (letter == END[currentEnd]) {
currentEnd++;
} else {
currentEnd = 0;
// can be only \n or any wrong char
if (letter == END[1] || !isprint(letter)) {
currentEnd = 2;
}
}
bool isEnd = currentEnd > 0;
bool isSentence = false; // is it a meaningful character of the sentence
bool startData = false; // start new current data part
switch (state) {
case S_SOS:
if (isStart) {
state = S_ADDRESS;
clear = true;
} else {
#ifdef GLOBPOS_PRINT_ERRORS
fprintf(stderr, "Wrong starting character %d\n", (int)letter);
#endif
}
break;
case S_ADDRESS:
if (letter == ',') {
state = S_DATA;
startData = true;
} else {
addressLength++;
}
isSentence = true;
break;
case S_DATA:
if (letter == ',') {
data.add(currentData);
startData = true;
isSentence = true;
} else if (letter == '*') {
data.add(currentData);
state = S_CHECKSUM;
} else if (isEnd) {
data.add(currentData);
state = S_END;
} else {
currentData.size++;
isSentence = true;
}
break;
case S_CHECKSUM:
if (isEnd) {
state = S_END;
} else {
checksum.add(letter);
}
if (checksum.length == CHECKSUM_LENGTH) {
state = S_END;
}
break;
case S_END:
if (!isEnd) {
#ifdef GLOBPOS_PRINT_ERRORS
fprintf(stderr, "Wrong ending character %d\n", (int)letter);
#endif
}
break;
default:
fprintf(stderr, "Unknown state %d\n", state);
break;
}
if (isSentence) {
if (sentence.length >= MAX_SENTENCE_LENGTH) {
fprintf(stderr, "Length limit reached\n");
}
sentence.add(letter);
}
if (startData) {
currentData.offset = sentence.length; // should be next character after comma
currentData.size = 0;
}
if (isEnd) {
if (state != S_END) {
#ifdef GLOBPOS_PRINT_ERRORS
fprintf(stderr, "Unexpected end of sentence at state %d\n", state);
#endif
}
if (currentEnd == sizeof(END)) {
currentEnd = 0;
commit = true;
state = S_SOS;
}
}
if (commit) {
if (addressLength > 0 && data.length > 0) {
if (verifyChecksum) {
if (verifyCurrentSentence()) {
commitCurrentSentence();
} else {
std::string addressStr(sentence.content, addressLength);
std::string checksumStr(checksum.content, checksum.length);
fprintf(stderr, "Checksum %s verification failed for %s with %d fields\n",
checksumStr.c_str(), addressStr.c_str(), (int)data.length);
}
} else {
commitCurrentSentence();
}
}
clear = true;
}
if (clear) {
clearCurrentSentence();
}
}
void GPRMC::decode() {
//$GPRMC,162748,A,5214.0456,N,01103.4540,E,0.0,0.0,250309,0.0,E,A*17CRLF
if (m_message.size() > 65) {
uint32_t pos = 0;
string strGPRMC = m_message.substr(pos, 6);
if (strGPRMC != "$GPRMC") {
clog << "No $GPRMC message." << endl;
return;
}
////////////////////////////////////////////////////////
pos = pos + 6 + 1; // Consume value + ","
stringstream timeStr(m_message.substr(pos, 6));
uint32_t time;
timeStr >> time;
pos = pos + 6 + 1; // Consume value + ","
char type = m_message.at(pos);
if (type != 'A') {
clog << "No data message." << endl;
return;
}
////////////////////////////////////////////////////////
pos = pos + 1 + 1; // Consume value + ","
stringstream latitudeStr(m_message.substr(pos, 2));
pos = pos + 2; // Consume value
stringstream latitudeArcMinuteStr(m_message.substr(pos, 7));
double temp;
double latitude = 0;
latitudeStr >> temp;
latitude = temp;
latitudeArcMinuteStr >> temp;
latitude += (temp / 60);
pos = pos + 7 + 1; // Consume value + ","
char LAT = m_message.at(pos);
m_coordinate.setLatitude(latitude);
m_coordinate.setLATITUDE((LAT == 'N') ? WGS84Coordinate::NORTH : WGS84Coordinate::SOUTH);
////////////////////////////////////////////////////////
pos = pos + 1 + 1; // Consume value + ","
stringstream longitudeStr(m_message.substr(pos, 3));
pos = pos + 3; // Consume value
stringstream longitudeArcMinuteStr(m_message.substr(pos, 7));
double longitude = 0;
longitudeStr >> temp;
longitude = temp;
longitudeArcMinuteStr >> temp;
longitude += (temp / 60);
pos = pos + 7 + 1; // Consume value + ","
char LON = m_message.at(pos);
m_coordinate.setLongitude(longitude);
m_coordinate.setLONGITUDE((LON == 'W') ? WGS84Coordinate::WEST: WGS84Coordinate::EAST);
////////////////////////////////////////////////////////
pos = pos + 1 + 1; // Consume value + ","
stringstream velocityStr;
char c = m_message.at(pos);
while (c != ',') {
velocityStr << c;
pos++;
c = m_message.at(pos);
}
pos = pos + 1; // Consume ","
stringstream directionStr;
c = m_message.at(pos);
while (c != ',') {
directionStr << c;
pos++;
c = m_message.at(pos);
}
pos = pos + 1; // Consume ","
stringstream ddmmStr(m_message.substr(pos, 4));
pos = pos + 4; // Consume value
stringstream yyStr(m_message.substr(pos, 2));
uint32_t yy;
yyStr >> yy;
stringstream dateTimeStr;
dateTimeStr << ddmmStr.str() << GPRMC::BASE_YEAR + yy << timeStr.str();
m_timeStamp = TimeStamp(dateTimeStr.str());
pos = pos + 2 + 1; // Consume value + ","
stringstream magneticDeviationStr;
c = m_message.at(pos);
while (c != ',') {
magneticDeviationStr << c;
pos++;
c = m_message.at(pos);
}
pos = pos + 1; // Consume ","
LON = m_message.at(pos);
pos = pos + 1 + 1; // Consume value + ","
type = m_message.at(pos);
pos = pos + 1 + 1; // Consume value + "*"
const uint32_t messageEnd = pos;
stringstream checksumStr(m_message.substr(pos));
uint32_t checksum;
checksumStr >> hex >> checksum;
uint32_t XOR = 0;
for(uint32_t i = 0; i < messageEnd; i++) {
XOR ^= m_message.at(i);
}
if (XOR != checksum) {
clog << "GPRMC corrupt!" << endl;
}
}
