void GPS_Parse_thread(void)
{
while(1){
memset((char *)Gps_str, 0, 512);
len = serial_read(&GpsDevice, Gps_str, 512);
if(len>0){
if(strstr(Gps_str,"$GPRMC")!=NULL){
nmea_parse_GPRMC(Gps_str, len, &vGPRMC);
pthread_rwlock_wrlock(&GPS_rwlock);
gps_tmp.status = vGPRMC.status;
gps_tmp.lat = (vGPRMC.status == 'A') ? vGPRMC.lat : gps_tmp.lat;
gps_tmp.ns = vGPRMC.ns;
gps_tmp.lon = (vGPRMC.status == 'A') ? vGPRMC.lon : gps_tmp.lon;
gps_tmp.ew = vGPRMC.ew;
gps_tmp.speed = (vGPRMC.status == 'A') ? vGPRMC.speed : gps_tmp.speed;
gps_tmp.direction = vGPRMC.direction;
pthread_rwlock_unlock(&GPS_rwlock);
}
}
sleep(2);
}
}
void GPS_Read(GpsInfo *pGPS)
{
while(1){
memset((char *)Gps_str, 0, 512);
len = serial_read(&GpsDevice, Gps_str, 512);
if(len>0){
if(strstr(Gps_str,"$GPRMC")!=NULL){
nmea_parse_GPRMC(Gps_str, len, &vGPRMC);
pGPS->status = vGPRMC.status;
pGPS->lat = vGPRMC.lat;
pGPS->ns = vGPRMC.ns;
pGPS->lon = vGPRMC.lon;
pGPS->ew = vGPRMC.ew;
pGPS->speed = (vGPRMC.speed * NMEA_TUD_KNOTS);
pGPS->direction = vGPRMC.direction;
break;
}
}
}
}
/**
* 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;
}
void QgsNMEAConnection::processRMCSentence( const char* data, int len )
{
nmeaGPRMC result;
if ( nmea_parse_GPRMC( data, len, &result ) )
{
double longitude = result.lon;
if ( result.ew == 'W' )
{
longitude = -longitude;
}
double latitude = result.lat;
if ( result.ns == 'S' )
{
latitude = -latitude;
}
mLastGPSInformation.longitude = nmea_ndeg2degree( longitude );
mLastGPSInformation.latitude = nmea_ndeg2degree( latitude );
mLastGPSInformation.speed = KNOTS_TO_KMH * result.speed;
mLastGPSInformation.direction = result.direction;
mLastGPSInformation.status = result.status; // A,V
//date and time
QDate date( result.utc.year + 1900, result.utc.mon + 1, result.utc.day );
QTime time( result.utc.hour, result.utc.min, result.utc.sec, result.utc.msec ); // added msec part
if ( date.isValid() && time.isValid() )
{
mLastGPSInformation.utcDateTime.setTimeSpec( Qt::UTC );
mLastGPSInformation.utcDateTime.setDate( date );
mLastGPSInformation.utcDateTime.setTime( time );
QgsDebugMsg( "utc time:" );
QgsDebugMsg( mLastGPSInformation.utcDateTime.toString() );
QgsDebugMsg( "local time:" );
QgsDebugMsg( mLastGPSInformation.utcDateTime.toLocalTime().toString() );
}
}
}
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;
}
