/*
|
*
|
* NMEA library
|
* URL: http://nmea.sourceforge.net
|
* Author: Tim (xtimor@gmail.com)
|
* Licence: http://www.gnu.org/licenses/lgpl.html
|
* $Id: context.c 17 2008-03-11 11:56:11Z xtimor $
|
*
|
*/
|
#include "nmea_parse.h"
|
#include "cp_logger.h"
|
|
#include <assert.h>
|
#include <stdarg.h>
|
#include <stdlib.h>
|
#include <stdio.h>
|
#include <ctype.h>
|
#include <string.h>
|
#include <limits.h>
|
|
#define NMEA_TOKS_COMPARE (1)
|
#define NMEA_TOKS_PERCENT (2)
|
#define NMEA_TOKS_WIDTH (3)
|
#define NMEA_TOKS_TYPE (4)
|
|
/**
|
* \brief Convert string to number
|
*/
|
int nmea_atoi(const char *str, int str_sz, int radix)
|
{
|
char *tmp_ptr;
|
char buff[NMEA_CONVSTR_BUF];
|
int res = 0;
|
|
if(str_sz < NMEA_CONVSTR_BUF)
|
{
|
memcpy(&buff[0], str, str_sz);
|
buff[str_sz] = '\0';
|
res = strtol(&buff[0], &tmp_ptr, radix);
|
}
|
|
return res;
|
}
|
|
/**
|
* \brief Convert string to fraction number
|
*/
|
double nmea_atof(const char *str, int str_sz)
|
{
|
char *tmp_ptr;
|
char buff[NMEA_CONVSTR_BUF];
|
double res = 0;
|
|
if(str_sz < NMEA_CONVSTR_BUF)
|
{
|
memcpy(&buff[0], str, str_sz);
|
buff[str_sz] = '\0';
|
res = strtod(&buff[0], &tmp_ptr);
|
}
|
|
return res;
|
}
|
|
/**
|
* \brief Analyse string (specificate for NMEA sentences)
|
*/
|
int nmea_scanf(const char *buff, int buff_sz, const char *format, ...)
|
{
|
const char *beg_tok;
|
const char *end_buf = buff + buff_sz;
|
|
va_list arg_ptr;
|
int tok_type = NMEA_TOKS_COMPARE;
|
int width = 0;
|
const char *beg_fmt = 0;
|
int snum = 0, unum = 0;
|
|
int tok_count = 0;
|
void *parg_target;
|
|
va_start(arg_ptr, format);
|
|
for(; *format && buff < end_buf; ++format)
|
{
|
switch(tok_type)
|
{
|
case NMEA_TOKS_COMPARE:
|
if('%' == *format)
|
tok_type = NMEA_TOKS_PERCENT;
|
else if(*buff++ != *format)
|
goto fail;
|
break;
|
case NMEA_TOKS_PERCENT:
|
width = 0;
|
beg_fmt = format;
|
tok_type = NMEA_TOKS_WIDTH;
|
case NMEA_TOKS_WIDTH:
|
if(isdigit(*format))
|
break;
|
{
|
tok_type = NMEA_TOKS_TYPE;
|
if(format > beg_fmt)
|
width = nmea_atoi(beg_fmt, (int)(format - beg_fmt), 10);
|
}
|
case NMEA_TOKS_TYPE:
|
beg_tok = buff;
|
|
if(!width && ('c' == *format || 'C' == *format) && *buff != format[1])
|
width = 1;
|
|
if(width)
|
{
|
if(buff + width <= end_buf)
|
buff += width;
|
else
|
goto fail;
|
}
|
else
|
{
|
if(!format[1] || (0 == (buff = (char *)memchr(buff, format[1], end_buf - buff))))
|
buff = end_buf;
|
}
|
|
if(buff > end_buf)
|
goto fail;
|
|
tok_type = NMEA_TOKS_COMPARE;
|
tok_count++;
|
|
parg_target = 0; width = (int)(buff - beg_tok);
|
|
switch(*format)
|
{
|
case 'c':
|
case 'C':
|
parg_target = (void *)va_arg(arg_ptr, char *);
|
if(width && 0 != (parg_target))
|
*((char *)parg_target) = *beg_tok;
|
break;
|
case 's':
|
case 'S':
|
parg_target = (void *)va_arg(arg_ptr, char *);
|
if(width && 0 != (parg_target))
|
{
|
memcpy(parg_target, beg_tok, width);
|
((char *)parg_target)[width] = '\0';
|
}
|
break;
|
case 'f':
|
case 'g':
|
case 'G':
|
case 'e':
|
case 'E':
|
parg_target = (void *)va_arg(arg_ptr, double *);
|
if(width && 0 != (parg_target))
|
*((double *)parg_target) = nmea_atof(beg_tok, width);
|
break;
|
};
|
|
if(parg_target)
|
break;
|
if(0 == (parg_target = (void *)va_arg(arg_ptr, int *)))
|
break;
|
if(!width)
|
break;
|
|
switch(*format)
|
{
|
case 'd':
|
case 'i':
|
snum = nmea_atoi(beg_tok, width, 10);
|
memcpy(parg_target, &snum, sizeof(int));
|
break;
|
case 'u':
|
unum = nmea_atoi(beg_tok, width, 10);
|
memcpy(parg_target, &unum, sizeof(unsigned int));
|
break;
|
case 'x':
|
case 'X':
|
unum = nmea_atoi(beg_tok, width, 16);
|
memcpy(parg_target, &unum, sizeof(unsigned int));
|
break;
|
case 'o':
|
unum = nmea_atoi(beg_tok, width, 8);
|
memcpy(parg_target, &unum, sizeof(unsigned int));
|
break;
|
default:
|
goto fail;
|
};
|
|
break;
|
};
|
}
|
|
fail:
|
|
va_end(arg_ptr);
|
|
return tok_count;
|
}
|
|
int nmea_parse_time(const char *buff, int buff_sz, nmeaTIME *res)
|
{
|
int success = 0;
|
|
switch(buff_sz)
|
{
|
case sizeof("hhmmss") - 1:
|
success = (3 == nmea_scanf(buff, buff_sz,
|
"%2d%2d%2d", &(res->hour), &(res->min), &(res->sec)
|
));
|
break;
|
case sizeof("hhmmss.s") - 1:
|
case sizeof("hhmmss.ss") - 1:
|
case sizeof("hhmmss.sss") - 1:
|
success = (4 == nmea_scanf(buff, buff_sz,
|
"%2d%2d%2d.%d", &(res->hour), &(res->min), &(res->sec), &(res->hsec)
|
));
|
break;
|
default:
|
log_err("Parse of time error (format error)!\n");
|
success = 0;
|
break;
|
}
|
|
return (success?0:-1);
|
}
|
|
/**
|
* \brief Define packet type by header (nmeaPACKTYPE).
|
* @param buff a constant character pointer of packet buffer.
|
* @param buff_sz buffer size.
|
* @return The defined packet type
|
* @see nmeaPACKTYPE
|
*/
|
int nmea_pack_type(const char *buff, int buff_sz)
|
{
|
static const char *pheads[] = {
|
"GPGGA",
|
"GPGSA",
|
"GPGSV",
|
"GPRMC",
|
"GPVTG",
|
};
|
|
assert(buff);
|
|
if(buff_sz < 5)
|
return GPNON;
|
else if(0 == memcmp(buff, pheads[0], 5))
|
return GPGGA;
|
else if(0 == memcmp(buff, pheads[1], 5))
|
return GPGSA;
|
else if(0 == memcmp(buff, pheads[2], 5))
|
return GPGSV;
|
else if(0 == memcmp(buff, pheads[3], 5))
|
return GPRMC;
|
else if(0 == memcmp(buff, pheads[4], 5))
|
return GPVTG;
|
|
return GPNON;
|
}
|
|
/**
|
* \brief Get the first line in buffer and check control sum (CRC).
|
* @param buff a constant character pointer of packets buffer.
|
* @param buff_sz buffer size.
|
* @param line pointer output for a line in the packet
|
* @param line_len a integer pointer for return the line length (must be defined).
|
* @return already read bytes to a line end from buff.
|
*/
|
int nmea_get_item_line(const char *buff, int buff_sz, char *line, int *line_len)
|
{
|
static const int tail_sz = 3 /* *[CRC] */ + 2 /* \r\n */;
|
char *start, *star;
|
int nread = 0;
|
|
*line_len = -1;
|
|
/* Data sample:
|
* $GPGGA,081803.000,3029.9100,N,11423.2012,E,1,05,1.6,38.7,M,-13.7,M,,0000*4C
|
* $GPGSA,A,3,18,24,21,22,15,,,,,,,,3.1,1.6,2.7*3A
|
* $GPRMC,081803.000,A,3029.9100,N,11423.2012,E,1.14,126.38,210113,,,A*62
|
*/
|
start = strchr(buff, '$');
|
if(!start)
|
{
|
/* Can not find the line start flag '$', means can not find an integrated line */
|
return buff_sz;
|
}
|
|
star = strchr(start, '*');
|
if(!star)
|
{
|
/* Can not find the line CRC start flag '*', means can not find an integrated line */
|
return buff_sz;
|
}
|
|
*line_len = star - start + 1; /* include '*' */
|
strncpy(line, start, *line_len);
|
|
nread = (star-buff) + tail_sz ;
|
|
return nread>buff_sz ? buff_sz : nread;
|
}
|
|
/**
|
* \brief Find tail of packet ("\r\n") in buffer and check control sum (CRC).
|
* @param buff a constant character pointer of packets buffer.
|
* @param buff_sz buffer size.
|
* @param res_crc a integer pointer for return CRC of packet (must be defined).
|
* @return Number of bytes to packet tail.
|
*/
|
int 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;
|
|
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 = nmea_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;
|
}
|
|
/**
|
* \brief Parse GGA packet from buffer.
|
* @param buff a constant character pointer of packet buffer.
|
* @param buff_sz buffer size.
|
* @param pack a pointer of packet which will filled by function.
|
* @return 1 (true) - if parsed successfully or 0 (false) - if fail.
|
*/
|
int nmea_parse_GPGGA(const char *buff, int buff_sz, nmeaGPGGA *pack)
|
{
|
char time_buff[NMEA_TIMEPARSE_BUF];
|
|
assert(buff && pack);
|
|
memset(pack, 0, sizeof(nmeaGPGGA));
|
|
log_dbg("parse_GPGGA: %s\n", buff);
|
|
if(14 != nmea_scanf(buff, buff_sz,
|
"$GPGGA,%s,%f,%C,%f,%C,%d,%d,%f,%f,%C,%f,%C,%f,%d*",
|
&(time_buff[0]),
|
&(pack->lat), &(pack->ns), &(pack->lon), &(pack->ew),
|
&(pack->sig), &(pack->satinuse), &(pack->HDOP), &(pack->elv), &(pack->elv_units),
|
&(pack->diff), &(pack->diff_units), &(pack->dgps_age), &(pack->dgps_sid)))
|
{
|
log_err("GPGGA parse error!\n");
|
return 0;
|
}
|
|
if(0 != nmea_parse_time(&time_buff[0], (int)strlen(&time_buff[0]), &(pack->utc)))
|
{
|
log_err("GPGGA time parse error!\n");
|
return 0;
|
}
|
|
return 1;
|
}
|
|
/**
|
* \brief Parse GSA packet from buffer.
|
* @param buff a constant character pointer of packet buffer.
|
* @param buff_sz buffer size.
|
* @param pack a pointer of packet which will filled by function.
|
* @return 1 (true) - if parsed successfully or 0 (false) - if fail.
|
*/
|
int nmea_parse_GPGSA(const char *buff, int buff_sz, nmeaGPGSA *pack)
|
{
|
assert(buff && pack);
|
|
memset(pack, 0, sizeof(nmeaGPGSA));
|
|
log_dbg("parse_GPGSA: %s\n", buff);
|
|
if(17 != nmea_scanf(buff, buff_sz,
|
"$GPGSA,%C,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f,%f*",
|
&(pack->fix_mode), &(pack->fix_type),
|
&(pack->sat_prn[0]), &(pack->sat_prn[1]), &(pack->sat_prn[2]), &(pack->sat_prn[3]), &(pack->sat_prn[4]), &(pack->sat_prn[5]),
|
&(pack->sat_prn[6]), &(pack->sat_prn[7]), &(pack->sat_prn[8]), &(pack->sat_prn[9]), &(pack->sat_prn[10]), &(pack->sat_prn[11]),
|
&(pack->PDOP), &(pack->HDOP), &(pack->VDOP)))
|
{
|
log_err("GPGSA parse error!\n");
|
return 0;
|
}
|
|
return 1;
|
}
|
|
/**
|
* \brief Parse GSV packet from buffer.
|
* @param buff a constant character pointer of packet buffer.
|
* @param buff_sz buffer size.
|
* @param pack a pointer of packet which will filled by function.
|
* @return 1 (true) - if parsed successfully or 0 (false) - if fail.
|
*/
|
int nmea_parse_GPGSV(const char *buff, int buff_sz, nmeaGPGSV *pack)
|
{
|
int nsen, nsat;
|
|
assert(buff && pack);
|
|
memset(pack, 0, sizeof(nmeaGPGSV));
|
|
log_dbg("parse_GPGSV: %s\n", buff);
|
|
nsen = nmea_scanf(buff, buff_sz,
|
"$GPGSV,%d,%d,%d,"
|
"%d,%d,%d,%d,"
|
"%d,%d,%d,%d,"
|
"%d,%d,%d,%d,"
|
"%d,%d,%d,%d*",
|
&(pack->pack_count), &(pack->pack_index), &(pack->sat_count),
|
&(pack->sat_data[0].id), &(pack->sat_data[0].elv), &(pack->sat_data[0].azimuth), &(pack->sat_data[0].sig),
|
&(pack->sat_data[1].id), &(pack->sat_data[1].elv), &(pack->sat_data[1].azimuth), &(pack->sat_data[1].sig),
|
&(pack->sat_data[2].id), &(pack->sat_data[2].elv), &(pack->sat_data[2].azimuth), &(pack->sat_data[2].sig),
|
&(pack->sat_data[3].id), &(pack->sat_data[3].elv), &(pack->sat_data[3].azimuth), &(pack->sat_data[3].sig));
|
|
nsat = (pack->pack_index - 1) * NMEA_SATINPACK;
|
nsat = (nsat + NMEA_SATINPACK > pack->sat_count)?pack->sat_count - nsat:NMEA_SATINPACK;
|
nsat = nsat * 4 + 3 /* first three sentence`s */;
|
|
if(nsen < nsat || nsen > (NMEA_SATINPACK * 4 + 3))
|
{
|
log_err("GPGSV parse error!\n");
|
return 0;
|
}
|
|
return 1;
|
}
|
|
/**
|
* \brief Parse RMC packet from buffer.
|
* @param buff a constant character pointer of packet buffer.
|
* @param buff_sz buffer size.
|
* @param pack a pointer of packet which will filled by function.
|
* @return 1 (true) - if parsed successfully or 0 (false) - if fail.
|
*/
|
int nmea_parse_GPRMC(const char *buff, int buff_sz, nmeaGPRMC *pack)
|
{
|
int nsen;
|
char time_buff[NMEA_TIMEPARSE_BUF];
|
|
assert(buff && pack);
|
|
memset(pack, 0, sizeof(nmeaGPRMC));
|
|
log_dbg("parse_GPGRMC: %s\n", buff);
|
|
nsen = nmea_scanf(buff, buff_sz,
|
"$GPRMC,%s,%C,%f,%C,%f,%C,%f,%f,%2d%2d%2d,%f,%C,%C*",
|
&(time_buff[0]),
|
&(pack->status), &(pack->lat), &(pack->ns), &(pack->lon), &(pack->ew),
|
&(pack->speed), &(pack->direction),
|
&(pack->utc.day), &(pack->utc.mon), &(pack->utc.year),
|
&(pack->declination), &(pack->declin_ew), &(pack->mode));
|
|
if(nsen != 13 && nsen != 14)
|
{
|
log_err("GPRMC parse error!\n");
|
return 0;
|
}
|
|
if(0 != nmea_parse_time(&time_buff[0], (int)strlen(&time_buff[0]), &(pack->utc)))
|
{
|
log_err("GPRMC time parse error!\n");
|
return 0;
|
}
|
|
if(pack->utc.year < 90)
|
pack->utc.year += 100;
|
pack->utc.mon -= 1;
|
|
return 1;
|
}
|
|
/**
|
* \brief Parse VTG packet from buffer.
|
* @param buff a constant character pointer of packet buffer.
|
* @param buff_sz buffer size.
|
* @param pack a pointer of packet which will filled by function.
|
* @return 1 (true) - if parsed successfully or 0 (false) - if fail.
|
*/
|
int nmea_parse_GPVTG(const char *buff, int buff_sz, nmeaGPVTG *pack)
|
{
|
assert(buff && pack);
|
|
memset(pack, 0, sizeof(nmeaGPVTG));
|
|
log_dbg("parse_GPVTG: %s\n", buff);
|
|
if(8 != nmea_scanf(buff, buff_sz,
|
"$GPVTG,%f,%C,%f,%C,%f,%C,%f,%C*",
|
&(pack->dir), &(pack->dir_t),
|
&(pack->dec), &(pack->dec_m),
|
&(pack->spn), &(pack->spn_n),
|
&(pack->spk), &(pack->spk_k)))
|
{
|
log_err("GPVTG parse error!\n");
|
return 0;
|
}
|
|
if( pack->dir_t != 'T' ||
|
pack->dec_m != 'M' ||
|
pack->spn_n != 'N' ||
|
pack->spk_k != 'K')
|
{
|
log_err("GPVTG parse error (format error)!\n");
|
return 0;
|
}
|
|
return 1;
|
}
|
|
/**
|
* \brief Fill nmeaINFO structure by GGA packet data.
|
* @param pack a pointer of packet structure.
|
* @param info a pointer of summary information structure.
|
*/
|
void nmea_GPGGA2info(nmeaGPGGA *pack, nmeaINFO *info)
|
{
|
assert(pack && info);
|
|
info->utc.hour = pack->utc.hour;
|
info->utc.min = pack->utc.min;
|
info->utc.sec = pack->utc.sec;
|
info->utc.hsec = pack->utc.hsec;
|
info->sig = pack->sig;
|
info->HDOP = pack->HDOP;
|
info->elv = pack->elv;
|
info->lat = ((pack->ns == 'N')?pack->lat:-(pack->lat));
|
info->lon = ((pack->ew == 'E')?pack->lon:-(pack->lon));
|
info->smask |= GPGGA;
|
}
|
|
/**
|
* \brief Fill nmeaINFO structure by GSA packet data.
|
* @param pack a pointer of packet structure.
|
* @param info a pointer of summary information structure.
|
*/
|
void nmea_GPGSA2info(nmeaGPGSA *pack, nmeaINFO *info)
|
{
|
int i, j, nuse = 0;
|
|
assert(pack && info);
|
|
info->fix = pack->fix_type;
|
info->PDOP = pack->PDOP;
|
info->HDOP = pack->HDOP;
|
info->VDOP = pack->VDOP;
|
|
for(i = 0; i < NMEA_MAXSAT; ++i)
|
{
|
for(j = 0; j < info->satinfo.inview; ++j)
|
{
|
if(pack->sat_prn[i] && pack->sat_prn[i] == info->satinfo.sat[j].id)
|
{
|
info->satinfo.sat[j].in_use = 1;
|
nuse++;
|
}
|
}
|
}
|
|
info->satinfo.inuse = nuse;
|
info->smask |= GPGSA;
|
}
|
|
/**
|
* \brief Fill nmeaINFO structure by GSV packet data.
|
* @param pack a pointer of packet structure.
|
* @param info a pointer of summary information structure.
|
*/
|
void nmea_GPGSV2info(nmeaGPGSV *pack, nmeaINFO *info)
|
{
|
int isat, isi, nsat;
|
|
assert(pack && info);
|
|
if(pack->pack_index > pack->pack_count ||
|
pack->pack_index * NMEA_SATINPACK > NMEA_MAXSAT)
|
return;
|
|
if(pack->pack_index < 1)
|
pack->pack_index = 1;
|
|
info->satinfo.inview = pack->sat_count;
|
|
nsat = (pack->pack_index - 1) * NMEA_SATINPACK;
|
nsat = (nsat + NMEA_SATINPACK > pack->sat_count)?pack->sat_count - nsat:NMEA_SATINPACK;
|
|
for(isat = 0; isat < nsat; ++isat)
|
{
|
isi = (pack->pack_index - 1) * NMEA_SATINPACK + isat;
|
info->satinfo.sat[isi].id = pack->sat_data[isat].id;
|
info->satinfo.sat[isi].elv = pack->sat_data[isat].elv;
|
info->satinfo.sat[isi].azimuth = pack->sat_data[isat].azimuth;
|
info->satinfo.sat[isi].sig = pack->sat_data[isat].sig;
|
}
|
|
info->smask |= GPGSV;
|
}
|
|
/**
|
* \brief Fill nmeaINFO structure by RMC packet data.
|
* @param pack a pointer of packet structure.
|
* @param info a pointer of summary information structure.
|
*/
|
void nmea_GPRMC2info(nmeaGPRMC *pack, nmeaINFO *info)
|
{
|
assert(pack && info);
|
|
if('A' == pack->status)
|
{
|
if(NMEA_SIG_BAD == info->sig)
|
info->sig = NMEA_SIG_MID;
|
if(NMEA_FIX_BAD == info->fix)
|
info->fix = NMEA_FIX_2D;
|
}
|
else if('V' == pack->status)
|
{
|
info->sig = NMEA_SIG_BAD;
|
info->fix = NMEA_FIX_BAD;
|
}
|
|
info->utc = pack->utc;
|
info->lat = ((pack->ns == 'N')?pack->lat:-(pack->lat));
|
info->lon = ((pack->ew == 'E')?pack->lon:-(pack->lon));
|
info->speed = pack->speed * NMEA_TUD_KNOTS;
|
info->direction = pack->direction;
|
info->smask |= GPRMC;
|
}
|
|
/**
|
* \brief Fill nmeaINFO structure by VTG packet data.
|
* @param pack a pointer of packet structure.
|
* @param info a pointer of summary information structure.
|
*/
|
void nmea_GPVTG2info(nmeaGPVTG *pack, nmeaINFO *info)
|
{
|
assert(pack && info);
|
|
info->direction = pack->dir;
|
info->declination = pack->dec;
|
info->speed = pack->spk;
|
info->smask |= GPVTG;
|
}
|
