##// END OF EJS Templates
jro_hard » gen_acq
RSS
aaguilar - - Load All Authors

File last commit:

r96:97
r107:108
Show More
nmeaParser.cpp
309 lines | 6.0 KiB | text/x-c | CppLexer
/ trunk / firmware / sources / saigd / nmealib++ / nmeaParser.cpp
History | Annotation | Raw |Copy content |Copy permalink
/*
* nmeaParser.cpp
*
* Created on: Oct 21, 2014
* Author: Alan Aguilar Sologuren
*/
#include "nmea_defs.h"
#include "nmeaParser.h"
/**
* \brief Initialization of parser object
*/
nmeaParser::nmeaParser():
top_node(nullptr),end_node(nullptr),
buffer(nullptr),buff_size(0),
buff_use(0){
int buff_sz = nmea_property()->parse_buff_size;
NMEA_ASSERT(this);
if(buff_sz < NMEA_MIN_PARSEBUFF)
buff_sz = NMEA_MIN_PARSEBUFF;
if(0 == (buffer = malloc(buff_size)))
nmea_error("Insufficient memory!");
delete this;
else
{
buff_size = buff_sz;
}
}
/**
* \brief Destroy parser object
*/
nmeaParser::~nmeaParser() {
NMEA_ASSERT(this && buffer);
free(buffer);
nmea_parser_queue_clear();
}
/**
* \brief Analysis of buffer and put results to information structure
* @return Number of packets wos parsed
*/
int nmeaParser::nmea_parse(const char *buff, int buff_sz, nmeaInfo *info )
{
int ptype, nread = 0;
nmeaCode *pack = 0;
NMEA_ASSERT(this && buffer);
nmea_parser_push(buff, buff_sz);
while(GPNON != (ptype = nmea_parser_pop(&pack)))
{
nread++;
code_2info(pack, info);
free(pack);
}
return nread;
}
/*
* low level
*/
int nmeaParser::nmea_parser_real_push(const char *buff, int buff_sz)
{
int nparsed = 0, crc, sen_sz, ptype;
nmeaParserNODE *node = 0;
NMEA_ASSERT(this && buffer);
/* add */
if(buff_use + buff_sz >= buff_size)
nmea_parser_buff_clear(parser);
memcpy(buffer + buff_use, buff, buff_sz);
buff_use += buff_sz;
/* parse */
for(;;node = 0)
{
sen_sz = nmea_find_tail(
(const char *)buffer + nparsed,
(int)buff_use - nparsed, &crc);
if(!sen_sz)
{
if(nparsed)
memcpy(
buffer,
buffer + nparsed,
buff_use -= nparsed);
break;
}
else if(crc >= 0)
{
if(!node->pack->code_parse( (const char *)buffer + nparsed,
sen_sz)
{
free(node);
node = 0;
}
if(node)
{
if(end_node)
((nmeaParserNODE *)end_node)->next_node = node;
end_node = node;
if(!top_node)
top_node = node;
node->next_node = 0;
}
}
nparsed += sen_sz;
}
return nparsed;
}
/**
* \brief Analysis of buffer and keep results into parser
* @return Number of bytes wos parsed from buffer
*/
int nmeaParser::nmea_parser_push(const char *buff, int buff_sz)
{
int nparse, nparsed = 0;
do
{
if(buff_sz > buff_size)
nparse = buff_size;
else
nparse = buff_sz;
nparsed += nmea_parser_real_push(
buff, nparse);
buff_sz -= nparse;
} while(buff_sz);
return nparsed;
}
/**
* \brief Get type of top packet keeped into parser
* @return Type of packet
* @see nmeaPACKTYPE
*/
int nmeaParser::nmea_parser_top()
{
int retval = GPNON;
nmeaParserNODE *node = (nmeaParserNODE *)top_node;
NMEA_ASSERT(this && buffer);
if(node)
retval = node->pack->type;
return retval;
}
/**
* \brief Withdraw top packet from parser
* @return Received packet type
* @see nmeaPACKTYPE
*/
int nmeaParser::nmea_parser_pop( nmeaCode **pack_ptr)
{
int retval = GPNON;
nmeaParserNODE *node = (nmeaParserNODE *)top_node;
NMEA_ASSERT(this && buffer);
if(node)
{
*pack_ptr = node->pack;
retval = node->pack->type;
top_node = node->next_node;
if(!top_node)
end_node = 0;
free(node);
}
return retval;
}
/**
* \brief Get top packet from parser without withdraw
* @return Received packet type
* @see nmeaPACKTYPE
*/
int nmeaParser::nmea_parser_peek(nmeaCode **pack_ptr)
{
int retval = GPNON;
nmeaParserNODE *node = (nmeaParserNODE *)top_node;
NMEA_ASSERT(this && buffer);
if(node)
{
*pack_ptr = node->pack;
retval = node->pack->type;
}
return retval;
}
/**
* \brief Delete top packet from parser
* @return Deleted packet type
* @see nmeaPACKTYPE
*/
int nmeaParser::nmea_parser_drop()
{
int retval = GPNON;
nmeaParserNODE *node = (nmeaParserNODE *)top_node;
NMEA_ASSERT(this && buffer);
if(node)
{
if(node->pack)
free(node->pack);
retval = node->pack->type;
top_node = node->next_node;
if(!top_node)
end_node = 0;
free(node);
}
return retval;
}
/**
* \brief Clear cache of parser
* @return true (1) - success
*/
int nmeaParser::nmea_parser_buff_clear()
{
NMEA_ASSERT(this && buffer);
buff_use = 0;
return 1;
}
/**
* \brief Clear packets queue into parser
* @return true (1) - success
*/
int nmeaParser::nmea_parser_queue_clear()
{
NMEA_ASSERT(this);
while(top_node)
nmea_parser_drop();
return 1;
}
virtual void nmeaParser::code_2info(nmeaCode* code, nmeaInfo* info) {
}
int nmeaParser::nmea_find_tail(const char *buff, int buff_sz, int *res_crc)
{
static const int tail_sz = 3 /* *[CRC] */ + 2 /* \r\n */;
const char *end_buff = buff + buff_sz;
int nread = 0;
int crc = 0;
NMEA_ASSERT(buff && res_crc);
*res_crc = -1;
for(;buff < end_buff; ++buff, ++nread)
{
if(('$' == *buff) && nread)
{
buff = 0;
break;
}
else if('*' == *buff)
{
if(buff + tail_sz <= end_buff && '\r' == buff[3] && '\n' == buff[4])
{
*res_crc = code_atoi(buff + 1, 2, 16);
nread = buff_sz - (int)(end_buff - (buff + tail_sz));
if(*res_crc != crc)
{
*res_crc = -1;
buff = 0;
}
}
break;
}
else if(nread)
crc ^= (int)*buff;
}
if(*res_crc < 0 && buff)
nread = 0;
return nread;
}