void QgsNMEAConnection::processVTGSentence( const char* data, int len )
{
nmeaGPVTG result;
if ( nmea_parse_GPVTG( data, len, &result ) )
{
mLastGPSInformation.speed = result.spk;
}
}
/**
* Parse a string and store the results in the nmeaINFO structure
*
* @param parser a pointer to the parser
* @param s the string
* @param len the length of the string
* @param info a pointer to the nmeaINFO structure
* @return the number of packets that were parsed
*/
int nmea_parse(nmeaPARSER * parser, const char * s, int len, nmeaINFO * info) {
int sentences_count = 0;
int charIndex = 0;
assert(parser);
assert(s);
assert(info);
for (charIndex = 0; charIndex < len; charIndex++) {
bool sentence_read_successfully = nmea_parse_sentence_character(parser, &s[charIndex]);
if (sentence_read_successfully) {
enum nmeaPACKTYPE sentence_type = nmea_parse_get_sentence_type(&parser->buffer.buffer[1], parser->buffer.length - 1);
switch (sentence_type) {
case GPGGA:
if (nmea_parse_GPGGA(parser->buffer.buffer, parser->buffer.length, parser->sentence_parser.has_checksum, &parser->sentence.gpgga)) {
sentences_count++;
nmea_GPGGA2info(&parser->sentence.gpgga, info);
}
break;
case GPGSA:
if (nmea_parse_GPGSA(parser->buffer.buffer, parser->buffer.length, parser->sentence_parser.has_checksum, &parser->sentence.gpgsa)) {
sentences_count++;
nmea_GPGSA2info(&parser->sentence.gpgsa, info);
}
break;
case GPGSV:
if (nmea_parse_GPGSV(parser->buffer.buffer, parser->buffer.length, parser->sentence_parser.has_checksum, &parser->sentence.gpgsv)) {
sentences_count++;
nmea_GPGSV2info(&parser->sentence.gpgsv, info);
}
break;
case GPRMC:
if (nmea_parse_GPRMC(parser->buffer.buffer, parser->buffer.length, parser->sentence_parser.has_checksum, &parser->sentence.gprmc)) {
sentences_count++;
nmea_GPRMC2info(&parser->sentence.gprmc, info);
}
break;
case GPVTG:
if (nmea_parse_GPVTG(parser->buffer.buffer, parser->buffer.length, parser->sentence_parser.has_checksum, &parser->sentence.gpvtg)) {
sentences_count++;
nmea_GPVTG2info(&parser->sentence.gpvtg, info);
}
break;
case GPNON:
default:
break;
}
}
}
return sentences_count;
}
int nmea_parser_real_push(nmeaPARSER *parser, const char *buff, int buff_sz)
{
int nparsed = 0, crc, sen_sz, ptype;
nmeaParserNODE *node = 0;
NMEA_ASSERT(parser && parser->buffer);
/* clear unuse buffer (for debug) */
/*
memset(
parser->buffer + parser->buff_use, 0,
parser->buff_size - parser->buff_use
);
*/
/* add */
if(parser->buff_use + buff_sz >= parser->buff_size)
nmea_parser_buff_clear(parser);
memcpy(parser->buffer + parser->buff_use, buff, buff_sz);
parser->buff_use += buff_sz;
/* parse */
for(;;node = 0)
{
sen_sz = nmea_find_tail(
(const char *)parser->buffer + nparsed,
(int)parser->buff_use - nparsed, &crc);
if(!sen_sz)
{
if(nparsed)
memcpy(
parser->buffer,
parser->buffer + nparsed,
parser->buff_use -= nparsed);
break;
}
else if(crc >= 0)
{
ptype = nmea_pack_type(
(const char *)parser->buffer + nparsed + 1,
parser->buff_use - nparsed - 1);
if(0 == (node = malloc(sizeof(nmeaParserNODE))))
goto mem_fail;
node->pack = 0;
switch(ptype)
{
case GPGGA:
if(0 == (node->pack = malloc(sizeof(nmeaGPGGA))))
goto mem_fail;
node->packType = GPGGA;
if(!nmea_parse_GPGGA(
(const char *)parser->buffer + nparsed,
sen_sz, (nmeaGPGGA *)node->pack))
{
free(node->pack);
free(node);
node = 0;
}
break;
case GPGSA:
if(0 == (node->pack = malloc(sizeof(nmeaGPGSA))))
goto mem_fail;
node->packType = GPGSA;
if(!nmea_parse_GPGSA(
(const char *)parser->buffer + nparsed,
sen_sz, (nmeaGPGSA *)node->pack))
{
free(node->pack);
free(node);
node = 0;
}
break;
case GPGSV:
if(0 == (node->pack = malloc(sizeof(nmeaGPGSV))))
goto mem_fail;
node->packType = GPGSV;
if(!nmea_parse_GPGSV(
(const char *)parser->buffer + nparsed,
sen_sz, (nmeaGPGSV *)node->pack))
{
free(node->pack);
free(node);
node = 0;
}
break;
case GPRMC:
if(0 == (node->pack = malloc(sizeof(nmeaGPRMC))))
goto mem_fail;
node->packType = GPRMC;
if(!nmea_parse_GPRMC(
(const char *)parser->buffer + nparsed,
sen_sz, (nmeaGPRMC *)node->pack))
{
free(node->pack);
free(node);
node = 0;
}
break;
case GPVTG:
if(0 == (node->pack = malloc(sizeof(nmeaGPVTG))))
goto mem_fail;
node->packType = GPVTG;
if(!nmea_parse_GPVTG(
(const char *)parser->buffer + nparsed,
sen_sz, (nmeaGPVTG *)node->pack))
{
free(node->pack);
free(node);
node = 0;
}
break;
default:
free(node);
node = 0;
break;
};
if(node)
{
if(parser->end_node)
((nmeaParserNODE *)parser->end_node)->next_node = node;
parser->end_node = node;
if(!parser->top_node)
parser->top_node = node;
node->next_node = 0;
}
}
nparsed += sen_sz;
}
return nparsed;
mem_fail:
if(node)
free(node);
nmea_error("Insufficient memory!");
return -1;
}
int nmea_parser_real_push(nmeaPARSER *parser, const char *buff, int buff_sz)
{
int nparsed = 0, crc, sen_sz, ptype;
nmeaParserNODE *node = 0;
NMEA_ASSERT(parser && parser->buffer);
/* clear unuse buffer (for debug) */
/*
memset(
parser->buffer + parser->buff_use, 0,
parser->buff_size - parser->buff_use
);
*/
/* add */
if(parser->buff_use + buff_sz >= parser->buff_size)
nmea_parser_buff_clear(parser);
memcpy(parser->buffer + parser->buff_use, buff, buff_sz);
parser->buff_use += buff_sz;
/* parse */
for(;;node = 0)
{
// return number of byte to packet tail. If find an invalid trailer or and invalid crc return 0.
// if it is a valid message the function return > 0 with a crc >= 0
sen_sz = nmea_find_tail(
(const char *)parser->buffer + nparsed,
(int)parser->buff_use - nparsed, &crc);
//printf("sen_sz: %i\n", sen_sz);
if(!sen_sz)
{
if(nparsed)
memcpy(
parser->buffer,
parser->buffer + nparsed,
parser->buff_use -= nparsed);
break;
}
else if(crc >= 0)
{
ptype = nmea_pack_type(
(const char *)parser->buffer + nparsed + 1,
parser->buff_use - nparsed - 1);
if(0 == (node = malloc(sizeof(nmeaParserNODE))))
goto mem_fail;
node->pack = 0;
switch(ptype) // ADD: add packet type case here
{
case GPGGA:
if(0 == (node->pack = malloc(sizeof(nmeaGPGGA))))
goto mem_fail;
node->packType = GPGGA;
if(!nmea_parse_GPGGA(
(const char *)parser->buffer + nparsed,
sen_sz, (nmeaGPGGA *)node->pack))
{
free(node);
node = 0;
}
break;
case GPGSA:
if(0 == (node->pack = malloc(sizeof(nmeaGPGSA))))
goto mem_fail;
node->packType = GPGSA;
if(!nmea_parse_GPGSA(
(const char *)parser->buffer + nparsed,
sen_sz, (nmeaGPGSA *)node->pack))
{
free(node);
node = 0;
}
break;
case GPGSV:
if(0 == (node->pack = malloc(sizeof(nmeaGPGSV))))
goto mem_fail;
node->packType = GPGSV;
if(!nmea_parse_GPGSV(
(const char *)parser->buffer + nparsed,
sen_sz, (nmeaGPGSV *)node->pack))
{
free(node);
node = 0;
}
break;
case GPRMC:
if(0 == (node->pack = malloc(sizeof(nmeaGPRMC))))
goto mem_fail;
node->packType = GPRMC;
if(!nmea_parse_GPRMC(
(const char *)parser->buffer + nparsed,
sen_sz, (nmeaGPRMC *)node->pack))
{
free(node);
node = 0;
}
break;
case GPVTG:
if(0 == (node->pack = malloc(sizeof(nmeaGPVTG))))
goto mem_fail;
node->packType = GPVTG;
if(!nmea_parse_GPVTG(
(const char *)parser->buffer + nparsed,
sen_sz, (nmeaGPVTG *)node->pack))
{
free(node);
node = 0;
}
break;
case HCHDG:
if(0 == (node->pack = malloc(sizeof(nmeaHCHDG))))
goto mem_fail;
node->packType = HCHDG;
if(!nmea_parse_HCHDG(
(const char *)parser->buffer + nparsed,
sen_sz, (nmeaHCHDG *)node->pack))
{
free(node);
node = 0;
}
break;
case HCHDT:
if(0 == (node->pack = malloc(sizeof(nmeaHCHDT))))
goto mem_fail;
node->packType = HCHDT;
if(!nmea_parse_HCHDT(
(const char *)parser->buffer + nparsed,
sen_sz, (nmeaHCHDT *)node->pack))
{
free(node);
node = 0;
}
break;
case TIROT:
if(0 == (node->pack = malloc(sizeof(nmeaTIROT))))
goto mem_fail;
node->packType = TIROT;
if(!nmea_parse_TIROT(
(const char *)parser->buffer + nparsed,
sen_sz, (nmeaTIROT *)node->pack))
{
free(node);
node = 0;
}
break;
case YXXDR:
if(0 == (node->pack = malloc(sizeof(nmeaYXXDR))))
goto mem_fail;
node->packType = YXXDR;
if(!nmea_parse_YXXDR(
(const char *)parser->buffer + nparsed,
sen_sz, (nmeaYXXDR *)node->pack))
{
free(node);
node = 0;
}
break;
default:
free(node);
node = 0;
break;
};
if(node)
{
if(parser->end_node)
((nmeaParserNODE *)parser->end_node)->next_node = node;
parser->end_node = node;
if(!parser->top_node)
parser->top_node = node;
node->next_node = 0;
}
}
nparsed += sen_sz;
}
return nparsed;
mem_fail:
if(node)
free(node);
nmea_error("Insufficient memory!");
return -1;
}