/**
* \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];
NMEA_ASSERT(buff && pack);
memset(pack, 0, sizeof(nmeaGPGGA));
nmea_trace_buff(buff, buff_sz);
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)))
{
nmea_error("GPGGA parse error!");
return 0;
}
if(0 != _nmea_parse_time(&time_buff[0], (int)strlen(&time_buff[0]), &(pack->utc)))
{
nmea_error("GPGGA time parse error!");
return 0;
}
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)
{
NMEA_ASSERT(buff && pack);
memset(pack, 0, sizeof(nmeaGPVTG));
nmea_trace_buff(buff, buff_sz);
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)))
{
nmea_error("GPVTG parse error!");
return 0;
}
if( pack->dir_t != 'T' ||
pack->dec_m != 'M' ||
pack->spn_n != 'N' ||
pack->spk_k != 'K')
{
nmea_error("GPVTG parse error (format error)!");
return 0;
}
return 1;
}
/**
* \brief Parse XDR 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_YXXDR(const char *buff, int buff_sz, nmeaYXXDR *pack)
{
NMEA_ASSERT(buff && pack);
memset(pack, 0, sizeof(nmeaYXXDR));
nmea_trace_buff(buff, buff_sz);
if(8 != nmea_scanf(buff, buff_sz,
"$YXXDR,%C,%f,%C,%s,%C,%f,%C,%s*",
&(pack->angular_pitch), &(pack->pitch),
&(pack->degrees_pitch), &(pack->pitch_id),
&(pack->angular_roll), &(pack->roll),
&(pack->degrees_roll), &(pack->roll_id)))
{
nmea_error("YXXDR parse error!");
return 0;
}
if( pack->angular_pitch != 'A' ||
pack->degrees_pitch != 'D' ||
memcmp(pack->pitch_id, "PTCH", 4) != 0 ||
pack->angular_roll != 'A' ||
pack->degrees_roll != 'D' ||
memcmp(pack->roll_id, "ROLL", 4) != 0
)
{
nmea_error("YXXDR parse error (format error)!");
return 0;
}
return 1;
}
nmeaGENERATOR * __nmea_create_generator(int type, nmeaINFO *info)
{
nmeaGENERATOR *gen = 0;
switch(type)
{
case NMEA_GEN_NOISE:
if(0 == (gen = malloc(sizeof(nmeaGENERATOR))))
nmea_error("Insufficient memory!");
else
{
memset(gen, 0, sizeof(nmeaGENERATOR));
gen->init_call = &nmea_igen_noise_init;
gen->loop_call = &nmea_igen_noise_loop;
gen->reset_call = &nmea_igen_noise_reset;
}
break;
case NMEA_GEN_STATIC:
case NMEA_GEN_SAT_STATIC:
if(0 == (gen = malloc(sizeof(nmeaGENERATOR))))
nmea_error("Insufficient memory!");
else
{
memset(gen, 0, sizeof(nmeaGENERATOR));
gen->init_call = &nmea_igen_static_init;
gen->loop_call = &nmea_igen_static_loop;
gen->reset_call = &nmea_igen_static_reset;
}
break;
case NMEA_GEN_SAT_ROTATE:
if(0 == (gen = malloc(sizeof(nmeaGENERATOR))))
nmea_error("Insufficient memory!");
else
{
memset(gen, 0, sizeof(nmeaGENERATOR));
gen->init_call = &nmea_igen_rotate_init;
gen->loop_call = &nmea_igen_rotate_loop;
gen->reset_call = &nmea_igen_rotate_reset;
}
break;
case NMEA_GEN_POS_RANDMOVE:
if(0 == (gen = malloc(sizeof(nmeaGENERATOR))))
nmea_error("Insufficient memory!");
else
{
memset(gen, 0, sizeof(nmeaGENERATOR));
gen->init_call = &nmea_igen_pos_rmove_init;
gen->loop_call = &nmea_igen_pos_rmove_loop;
gen->destroy_call = &nmea_igen_pos_rmove_destroy;
}
break;
case NMEA_GEN_ROTATE:
gen = __nmea_create_generator(NMEA_GEN_SAT_ROTATE, info);
nmea_gen_add(gen, __nmea_create_generator(NMEA_GEN_POS_RANDMOVE, info));
break;
};
return gen;
}
/**
* \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;
NMEA_ASSERT(buff && pack);
memset(pack, 0, sizeof(nmeaGPGSV));
nmea_trace_buff(buff, buff_sz);
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))
{
nmea_error("GPGSV parse error!");
return 0;
}
return 1;
}
/**
* Parse nmeaTIME (time only, no date) from a string.
* The format that is used (hhmmss, hhmmss.s, hhmmss.ss or hhmmss.sss) is
* determined by the length of the string.
*
* @param s the string
* @param len the length of the string
* @param t a pointer to the nmeaTIME structure in which to store the parsed time
* @return true on success, false otherwise
*/
static bool _nmea_parse_time(const char *s, const int len, nmeaTIME *t) {
assert(s);
assert(t);
if (len == (sizeof("hhmmss") - 1)) {
t->hsec = 0;
return (3 == nmea_scanf(s, len, "%2d%2d%2d", &t->hour, &t->min, &t->sec));
}
if (len == (sizeof("hhmmss.s") - 1)) {
if (4 == nmea_scanf(s, len, "%2d%2d%2d.%d", &t->hour, &t->min, &t->sec, &t->hsec)) {
t->hsec *= 10;
return true;
}
return false;
}
if (len == (sizeof("hhmmss.ss") - 1)) {
return (4 == nmea_scanf(s, len, "%2d%2d%2d.%d", &t->hour, &t->min, &t->sec, &t->hsec));
}
if (len == (sizeof("hhmmss.sss") - 1)) {
if ((4 == nmea_scanf(s, len, "%2d%2d%2d.%d", &t->hour, &t->min, &t->sec, &t->hsec))) {
t->hsec = (t->hsec + 9) / 10;
return true;
}
return false;
}
nmea_error("Parse error: invalid time format in %s", s);
return false;
}
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:
nmea_error("Parse of time error (format error)!");
success = 0;
break;
}
return (success?0:-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 type;
char time_buff[NMEA_TIMEPARSE_BUF];
NMEA_ASSERT( buff && pack );
memset( pack, 0, sizeof( nmeaGPRMC ) );
nmea_trace_buff( buff, buff_sz );
nsen = nmea_scanf( buff, buff_sz,
"$G%CRMC,%s,%C,%f,%C,%f,%C,%f,%f,%2d%2d%2d,%f,%C,%C*",
&( type ), &( 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 != 14 && nsen != 15 )
{
nmea_error( "GPRMC parse error!" );
return 0;
}
if ( type != 'P' && type != 'N' )
{
nmea_error( "G?RMC invalid type " );
return 0;
}
if ( 0 != _nmea_parse_time( &time_buff[0], ( int )strlen( &time_buff[0] ), &( pack->utc ) ) )
{
nmea_error( "GPRMC time parse error!" );
return 0;
}
if ( pack->utc.year < 90 )
pack->utc.year += 100;
pack->utc.mon -= 1;
return 1;
}
/**
* Validate north/south or east/west and uppercase it.
* Expects:
* <pre>
* c in { n, N, s, S } (for north/south)
* c in { e, E, w, W } (for east/west)
* </pre>
*
* @param c a pointer to the character. The character will be converted to uppercase.
* @param ns true: evaluate north/south, false: evaluate east/west
* @return true when valid, false otherwise
*/
static bool validateNSEW(char * c, const bool ns) {
if (!c) {
return false;
}
*c = toupper(*c);
if (ns) {
if (!((*c == 'N') || (*c == 'S'))) {
nmea_error("Parse error: invalid north/south (%c)", *c);
return false;
}
} else {
if (!((*c == 'E') || (*c == 'W'))) {
nmea_error("Parse error: invalid east/west (%c)", *c);
return false;
}
}
return true;
}
/**
* Validate the date fields in an nmeaTIME structure.
* Expects:
* <pre>
* year [90, 189]
* month [ 0, 11]
* day [ 1, 31]
* </pre>
*
* @param t a pointer to the structure
* @return true when valid, false otherwise
*/
static bool validateDate(const nmeaTIME * t) {
if (!t) {
return false;
}
if (!((t->year >= 90) && (t->year <= 189) && (t->mon >= 0) && (t->mon <= 11) && (t->day >= 1) && (t->day <= 31))) {
nmea_error("Parse error: invalid date (%d-%d-%d - D-M-Y)", t->day, t->mon, t->year);
return false;
}
return true;
}
/**
* Validate the time fields in an nmeaTIME structure.
* Expects:
* <pre>
* 0 <= hour < 24
* 0 <= min < 60
* 0 <= sec <= 60
* 0 <= hsec < 100
* </pre>
*
* @param t a pointer to the structure
* @return true when valid, false otherwise
*/
static bool validateTime(const nmeaTIME * t) {
if (!t) {
return false;
}
if (!((t->hour >= 0) && (t->hour < 24) && (t->min >= 0) && (t->min < 60) && (t->sec >= 0) && (t->sec <= 60)
&& (t->hsec >= 0) && (t->hsec < 100))) {
nmea_error("Parse error: invalid time (%d:%d:%d.%d)", t->hour, t->min, t->sec, t->hsec);
return false;
}
return true;
}
/**
* Uppercase mode and validate it.
* Expects:
* <pre>
* c in { A, D, E, F, M, N, P, R, S }
*
* A = Autonomous. Satellite system used in non-differential mode in position fix
* D = Differential. Satellite system used in differential mode in position fix
* E = Estimated (dead reckoning) mode
* F = Float RTK. Satellite system used in real time kinematic mode with floating integers
* M = Manual input mode
* N = No fix. Satellite system not used in position fix, or fix not valid
* P = Precise. Satellite system used in precision mode. Precision mode is defined
* as no deliberate degradation (such as Selective Availability) and higher
* resolution code (P-code) is used to compute position fix.
* R = Real Time Kinematic. Satellite system used in RTK mode with fixed integers
* S = Simulator mode
* </pre>
*
* @param c a pointer to the character. The character will be converted to uppercase.
* @return true when valid, false otherwise
*/
static bool validateMode(char * c) {
if (!c) {
return false;
}
*c = toupper(*c);
if (!((*c == 'A') || (*c == 'D') || (*c == 'E') || (*c == 'F') || (*c == 'M') || (*c == 'N') || (*c == 'P')
|| (*c == 'R') || (*c == 'S'))) {
nmea_error("Parse error: invalid mode (%c)", *c);
return false;
}
return true;
}
/**
* Parse nmeaTIME (date only, no time) from a string.
* The month is adjusted -1 to comply with the nmeaTIME month range of [0, 11].
* The year is adjusted +100 for years before 90 to comply with the nmeaTIME
* year range of [90, 189].
*
* @param date the date
* @param t a pointer to the nmeaTIME structure in which to store the parsed date
* @return true on success, false otherwise
*/
static bool _nmea_parse_date(const int date, nmeaTIME *t) {
assert(t);
if ((date < 0) || (date > 999999)) {
nmea_error("Parse error: invalid time format in %d", date);
return false;
}
t->day = date / 10000;
t->mon = (date / 100) % 100;
t->mon--;
t->year = date % 100;
if (t->year < 90) {
t->year += 100;
}
return true;
}
/**
* \brief Parse ROT 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_TIROT(const char *buff, int buff_sz, nmeaTIROT *pack)
{
NMEA_ASSERT(buff && pack);
memset(pack, 0, sizeof(nmeaTIROT));
nmea_trace_buff(buff, buff_sz);
if(2 != nmea_scanf(buff, buff_sz,
"$TIROT,%f,%C*",
&(pack->rate), &(pack->status)))
{
nmea_error("TIROT parse error!");
return 0;
}
return 1;
}
/**
* \brief Parse HDT 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_HCHDT(const char *buff, int buff_sz, nmeaHCHDT *pack)
{
NMEA_ASSERT(buff && pack);
memset(pack, 0, sizeof(nmeaHCHDT));
nmea_trace_buff(buff, buff_sz);
if(2 != nmea_scanf(buff, buff_sz,
"$HCHDT,%f,%C*",
&(pack->direction), &(pack->t_flag)))
{
nmea_error("HCHDT parse error!");
return 0;
}
return 1;
}
/**
* \brief Parse HDG 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_HCHDG(const char *buff, int buff_sz, nmeaHCHDG *pack)
{
NMEA_ASSERT(buff && pack);
memset(pack, 0, sizeof(nmeaHCHDG));
nmea_trace_buff(buff, buff_sz);
if(5 != nmea_scanf(buff, buff_sz,
"$HCHDG,%f,%f,%C,%f,%C*",
&(pack->mag_heading), &(pack->mag_deviation),
&(pack->ew_deviation), &(pack->mag_variation),
&(pack->ew_variation)))
{
nmea_error("HCHDG parse error!");
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)
{
NMEA_ASSERT(buff && pack);
memset(pack, 0, sizeof(nmeaGPGSA));
nmea_trace_buff(buff, buff_sz);
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)))
{
nmea_error("GPGSA parse error!");
return 0;
}
return 1;
}
/**
* \brief Initialization of parser object
* @return true (1) - success or false (0) - fail
*/
int nmea_parser_init(nmeaPARSER *parser)
{
int resv = 0;
int buff_size = nmea_property()->parse_buff_size;
NMEA_ASSERT(parser);
if(buff_size < NMEA_MIN_PARSEBUFF)
buff_size = NMEA_MIN_PARSEBUFF;
memset(parser, 0, sizeof(nmeaPARSER));
if(0 == (parser->buffer = malloc(buff_size)))
nmea_error("Insufficient memory!");
else
{
parser->buff_size = buff_size;
resv = 1;
}
return resv;
}
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;
}
/**
* Parse a GPGGA sentence from a string
*
* @param s the string
* @param len the length of the string
* @param pack a pointer to the result structure
* @return 1 (true) - if parsed successfully or 0 (false) otherwise.
*/
int nmea_parse_GPGGA(const char *s, const int len, nmeaGPGGA *pack) {
int token_count;
char time_buff[NMEA_TIMEPARSE_BUF];
size_t time_buff_len = 0;
assert(s);
assert(pack);
nmea_trace_buff(s, len);
/*
* Clear before parsing, to be able to detect absent fields
*/
time_buff[0] = '\0';
pack->present = 0;
pack->utc.hour = -1;
pack->utc.min = -1;
pack->utc.sec = -1;
pack->utc.hsec = -1;
pack->lat = NAN;
pack->ns = 0;
pack->lon = NAN;
pack->ew = 0;
pack->sig = -1;
pack->satinuse = -1;
pack->HDOP = NAN;
pack->elv = NAN;
pack->elv_units = 0;
pack->diff = 0; /* ignored */
pack->diff_units = 0; /* ignored */
pack->dgps_age = 0; /* ignored */
pack->dgps_sid = 0; /* ignored */
/* parse */
token_count = nmea_scanf(s, len, "$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);
/* see that we have enough tokens */
if (token_count != 14) {
nmea_error("GPGGA parse error: need 14 tokens, got %d in %s", token_count, s);
return 0;
}
/* determine which fields are present and validate them */
time_buff_len = strlen(&time_buff[0]);
if (time_buff_len > (NMEA_TIMEPARSE_BUF - 1))
time_buff_len = NMEA_TIMEPARSE_BUF - 1;
if (time_buff_len) {
if (!_nmea_parse_time(&time_buff[0], time_buff_len, &pack->utc)) {
return 0;
}
if (!validateTime(&pack->utc)) {
return 0;
}
nmea_INFO_set_present(&pack->present, UTCTIME);
}
if (!isnan(pack->lat) && (pack->ns)) {
if (!validateNSEW(&pack->ns, true)) {
return 0;
}
nmea_INFO_set_present(&pack->present, LAT);
}
if (!isnan(pack->lon) && (pack->ew)) {
if (!validateNSEW(&pack->ew, false)) {
return 0;
}
nmea_INFO_set_present(&pack->present, LON);
}
if (pack->sig != -1) {
if (!((pack->sig >= NMEA_SIG_FIRST) && (pack->sig <= NMEA_SIG_LAST))) {
nmea_error("GPGGA parse error: invalid signal %d, expected [%d, %d]", pack->sig, NMEA_SIG_FIRST, NMEA_SIG_LAST);
return 0;
}
nmea_INFO_set_present(&pack->present, SIG);
}
if (pack->satinuse != -1) {
nmea_INFO_set_present(&pack->present, SATINUSECOUNT);
}
if (!isnan(pack->HDOP)) {
nmea_INFO_set_present(&pack->present, HDOP);
}
if (!isnan(pack->elv) && (pack->elv_units)) {
if (pack->elv_units != 'M') {
nmea_error("GPGGA parse error: invalid elevation unit (%c)", pack->elv_units);
return 0;
}
nmea_INFO_set_present(&pack->present, ELV);
}
/* ignore diff and diff_units */
/* ignore dgps_age and dgps_sid */
return 1;
}
/**
* Parse a GPGSA sentence from a string
*
* @param s the string
* @param len the length of the string
* @param pack a pointer to the result structure
* @return 1 (true) - if parsed successfully or 0 (false) otherwise.
*/
int nmea_parse_GPGSA(const char *s, const int len, nmeaGPGSA *pack) {
int token_count;
int i;
assert(s);
assert(pack);
nmea_trace_buff(s, len);
/*
* Clear before parsing, to be able to detect absent fields
*/
pack->present = 0;
pack->fix_mode = 0;
pack->fix_type = -1;
for (i = 0; i < NMEA_MAXSAT; i++) {
pack->sat_prn[i] = 0;
}
pack->PDOP = NAN;
pack->HDOP = NAN;
pack->VDOP = NAN;
/* parse */
token_count = nmea_scanf(s, len, "$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);
/* see that we have enough tokens */
if (token_count != 17) {
nmea_error("GPGSA parse error: need 17 tokens, got %d in %s", token_count, s);
return 0;
}
/* determine which fields are present and validate them */
pack->fix_mode = toupper(pack->fix_mode);
if (!((pack->fix_mode == 'A') || (pack->fix_mode == 'M'))) {
nmea_error("GPGSA parse error: invalid fix mode (%c)", pack->fix_mode);
return 0;
}
if (pack->fix_type != -1) {
if (!((pack->fix_type >= NMEA_FIX_FIRST) && (pack->fix_type <= NMEA_FIX_LAST))) {
nmea_error("GPGSA parse error: invalid fix type %d, expected [%d, %d]", pack->fix_type, NMEA_FIX_FIRST, NMEA_FIX_LAST);
return 0;
}
nmea_INFO_set_present(&pack->present, FIX);
}
for (i = 0; i < NMEA_MAXSAT; i++) {
if (pack->sat_prn[i] != 0) {
nmea_INFO_set_present(&pack->present, SATINUSE);
break;
}
}
if (!isnan(pack->PDOP)) {
nmea_INFO_set_present(&pack->present, PDOP);
}
if (!isnan(pack->HDOP)) {
nmea_INFO_set_present(&pack->present, HDOP);
}
if (!isnan(pack->VDOP)) {
nmea_INFO_set_present(&pack->present, VDOP);
}
return 1;
}
/**
* Parse a GPGSV sentence from a string
*
* @param s the string
* @param len the length of the string
* @param pack a pointer to the result structure
* @return 1 (true) - if parsed successfully or 0 (false) otherwise.
*/
int nmea_parse_GPGSV(const char *s, const int len, nmeaGPGSV *pack) {
int token_count;
int token_count_expected;
int sat_count;
int sat_counted = 0;
assert(s);
assert(pack);
nmea_trace_buff(s, len);
/*
* Clear before parsing, to be able to detect absent fields
*/
memset(pack, 0, sizeof(nmeaGPGSV));
/* parse */
token_count = nmea_scanf(s, len, "$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);
/* return if we have no sentences or sats */
if ((pack->pack_count < 1) || (pack->pack_count > NMEA_NSATPACKS) || (pack->pack_index < 1)
|| (pack->pack_index > pack->pack_count) || (pack->sat_count < 0) || (pack->sat_count > NMEA_MAXSAT)) {
nmea_error("GPGSV parse error: inconsistent pack (count/index/satcount = %d/%d/%d)", pack->pack_count,
pack->pack_index, pack->sat_count);
return 0;
}
/* validate all sat settings and count the number of sats in the sentence */
for (sat_count = 0; sat_count < NMEA_SATINPACK; sat_count++) {
if (pack->sat_data[sat_count].id != 0) {
if ((pack->sat_data[sat_count].id < 0)) {
nmea_error("GPGSV parse error: invalid sat %d id (%d)", sat_count + 1, pack->sat_data[sat_count].id);
return 0;
}
if ((pack->sat_data[sat_count].elv < -90) || (pack->sat_data[sat_count].elv > 90)) {
nmea_error("GPGSV parse error: invalid sat %d elevation (%d)", sat_count + 1, pack->sat_data[sat_count].elv);
return 0;
}
if ((pack->sat_data[sat_count].azimuth < 0) || (pack->sat_data[sat_count].azimuth >= 360)) {
nmea_error("GPGSV parse error: invalid sat %d azimuth (%d)", sat_count + 1, pack->sat_data[sat_count].azimuth);
return 0;
}
if ((pack->sat_data[sat_count].sig < 0) || (pack->sat_data[sat_count].sig > 99)) {
nmea_error("GPGSV parse error: invalid sat %d signal (%d)", sat_count + 1, pack->sat_data[sat_count].sig);
return 0;
}
sat_counted++;
}
}
/* see that we have enough tokens */
token_count_expected = (sat_counted * 4) + 3;
if ((token_count < token_count_expected) || (token_count > (NMEA_SATINPACK * 4 + 3))) {
nmea_error("GPGSV parse error: need %d tokens, got %d", token_count_expected, token_count);
return 0;
}
/* determine which fields are present and validate them */
if (pack->sat_count > 0) {
nmea_INFO_set_present(&pack->present, SATINVIEW);
}
return 1;
}
/**
* Parse a GPRMC sentence from a string
*
* @param s the string
* @param len the length of the string
* @param pack a pointer to the result structure
* @return 1 (true) - if parsed successfully or 0 (false) otherwise.
*/
int nmea_parse_GPRMC(const char *s, const int len, nmeaGPRMC *pack) {
int token_count;
char time_buff[NMEA_TIMEPARSE_BUF];
int date;
size_t time_buff_len = 0;
assert(s);
assert(pack);
nmea_trace_buff(s, len);
/*
* Clear before parsing, to be able to detect absent fields
*/
time_buff[0] = '\0';
date = -1;
pack->present = 0;
pack->utc.year = -1;
pack->utc.mon = -1;
pack->utc.day = -1;
pack->utc.hour = -1;
pack->utc.min = -1;
pack->utc.sec = -1;
pack->utc.hsec = -1;
pack->status = 0;
pack->lat = NAN;
pack->ns = 0;
pack->lon = NAN;
pack->ew = 0;
pack->speed = NAN;
pack->track = NAN;
pack->magvar = NAN;
pack->magvar_ew = 0;
pack->mode = 0;
/* parse */
token_count = nmea_scanf(s, len, "$GPRMC,%s,%c,%f,%c,%f,%c,%f,%f,%d,%f,%c,%c*", &time_buff[0], &pack->status,
&pack->lat, &pack->ns, &pack->lon, &pack->ew, &pack->speed, &pack->track, &date,
&pack->magvar, &pack->magvar_ew, &pack->mode);
/* see that we have enough tokens */
if ((token_count != 11) && (token_count != 12)) {
nmea_error("GPRMC parse error: need 11 or 12 tokens, got %d in %s", token_count, s);
return 0;
}
/* determine which fields are present and validate them */
time_buff_len = strlen(&time_buff[0]);
if (time_buff_len) {
if (!_nmea_parse_time(&time_buff[0], time_buff_len, &pack->utc)) {
return 0;
}
if (!validateTime(&pack->utc)) {
return 0;
}
nmea_INFO_set_present(&pack->present, UTCTIME);
}
if (!pack->status) {
pack->status = 'V';
} else {
pack->status = toupper(pack->status);
if (!((pack->status == 'A') || (pack->status == 'V'))) {
nmea_error("GPRMC parse error: invalid status (%c)", pack->status);
return 0;
}
}
if (!isnan(pack->lat) && (pack->ns)) {
if (!validateNSEW(&pack->ns, true)) {
return 0;
}
nmea_INFO_set_present(&pack->present, LAT);
}
if (!isnan(pack->lon) && (pack->ew)) {
if (!validateNSEW(&pack->ew, false)) {
return 0;
}
nmea_INFO_set_present(&pack->present, LON);
}
if (!isnan(pack->speed)) {
nmea_INFO_set_present(&pack->present, SPEED);
}
if (!isnan(pack->track)) {
nmea_INFO_set_present(&pack->present, TRACK);
}
if (date != -1) {
if (!_nmea_parse_date(date, &pack->utc)) {
return 0;
}
if (!validateDate(&pack->utc)) {
return 0;
}
nmea_INFO_set_present(&pack->present, UTCDATE);
}
if (!isnan(pack->magvar) && (pack->magvar_ew)) {
if (!validateNSEW(&pack->magvar_ew, false)) {
return 0;
}
nmea_INFO_set_present(&pack->present, MAGVAR);
}
if (token_count == 11) {
pack->mode = 'A';
} else {
if (!pack->mode) {
pack->mode = 'N';
} else {
if (!validateMode(&pack->mode)) {
return 0;
}
}
}
return 1;
}
/**
* Parse a GPVTG sentence from a string
*
* @param s the string
* @param len the length of the string
* @param pack a pointer to the result structure
* @return 1 (true) - if parsed successfully or 0 (false) otherwise.
*/
int nmea_parse_GPVTG(const char *s, const int len, nmeaGPVTG *pack) {
int token_count;
assert(s);
assert(pack);
nmea_trace_buff(s, len);
/*
* Clear before parsing, to be able to detect absent fields
*/
pack->present = 0;
pack->track = NAN;
pack->track_t = 0;
pack->mtrack = NAN;
pack->mtrack_m = 0;
pack->spn = NAN;
pack->spn_n = 0;
pack->spk = NAN;
pack->spk_k = 0;
/* parse */
token_count = nmea_scanf(s, len, "$GPVTG,%f,%c,%f,%c,%f,%c,%f,%c*", &pack->track, &pack->track_t, &pack->mtrack,
&pack->mtrack_m, &pack->spn, &pack->spn_n, &pack->spk, &pack->spk_k);
/* see that we have enough tokens */
if (token_count != 8) {
nmea_error("GPVTG parse error: need 8 tokens, got %d in %s", token_count, s);
return 0;
}
/* determine which fields are present and validate them */
if (!isnan(pack->track) && (pack->track_t)) {
pack->track_t = toupper(pack->track_t);
if (pack->track_t != 'T') {
nmea_error("GPVTG parse error: invalid track unit, got %c, expected T", pack->track_t);
return 0;
}
nmea_INFO_set_present(&pack->present, TRACK);
}
if (!isnan(pack->mtrack) && (pack->mtrack_m)) {
pack->mtrack_m = toupper(pack->mtrack_m);
if (pack->mtrack_m != 'M') {
nmea_error("GPVTG parse error: invalid mtrack unit, got %c, expected M", pack->mtrack_m);
return 0;
}
nmea_INFO_set_present(&pack->present, MTRACK);
}
if (!isnan(pack->spn) && (pack->spn_n)) {
pack->spn_n = toupper(pack->spn_n);
if (pack->spn_n != 'N') {
nmea_error("GPVTG parse error: invalid knots speed unit, got %c, expected N", pack->spn_n);
return 0;
}
nmea_INFO_set_present(&pack->present, SPEED);
if (isnan(pack->spk)) {
pack->spk = pack->spn * NMEA_TUD_KNOTS;
pack->spk_k = 'K';
}
}
if (!isnan(pack->spk) && (pack->spk_k)) {
pack->spk_k = toupper(pack->spk_k);
if (pack->spk_k != 'K') {
nmea_error("GPVTG parse error: invalid kph speed unit, got %c, expected K", pack->spk_k);
return 0;
}
nmea_INFO_set_present(&pack->present, SPEED);
if (isnan(pack->spn)) {
pack->spn = pack->spk / NMEA_TUD_KNOTS;
pack->spn_n = 'N';
}
}
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;
}
