/** * \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; }
/** * \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 *P_HEADS[] = { "GPGGA", "GPGSA", "GPGSV", "GPRMC", "GPVTG", "GNRMC", }; NMEA_ASSERT( buff ); if ( buff_sz < 5 ) return GPNON; else if ( 0 == memcmp( buff, P_HEADS[0], 5 ) ) return GPGGA; else if ( 0 == memcmp( buff, P_HEADS[1], 5 ) ) return GPGSA; else if ( 0 == memcmp( buff, P_HEADS[2], 5 ) ) return GPGSV; else if ( 0 == memcmp( buff, P_HEADS[3], 5 ) ) return GPRMC; else if ( 0 == memcmp( buff, P_HEADS[4], 5 ) ) return GPVTG; else if ( 0 == memcmp( buff, P_HEADS[5], 5 ) ) return GPRMC; return GPNON; }
/** * \brief Fill nmeaINFO structure by XDR packet data. * @param pack a pointer of packet structure. * @param info a pointer of summary information structure. */ void nmea_YXXDR2info(nmeaYXXDR *pack, nmeaINFO *info) { NMEA_ASSERT(pack && info); info->pitch_osc = pack->pitch; info->roll_osc = pack->roll; }
/** * \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 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; }
/** * \brief Destroy parser object */ void nmea_parser_destroy(nmeaPARSER *parser) { NMEA_ASSERT(parser && parser->buffer); free(parser->buffer); nmea_parser_queue_clear(parser); memset(parser, 0, sizeof(nmeaPARSER)); }
/** * \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; NMEA_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) { if(pack->sat_prn[i]) { nuse++; for(j = 0; j < info->satinfo.inview; ++j) { if(pack->sat_prn[i] == info->satinfo.sat[j].id) { info->satinfo.sat[j].in_use = 1; break; } } } } info->satinfo.inuse = nuse; info->smask |= GPGSA; }
/** * \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", }; NMEA_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 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; NMEA_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; // This field contain data only in the first instance if(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 Clear packets queue into parser * @return true (1) - success */ int nmea_parser_queue_clear(nmeaPARSER *parser) { NMEA_ASSERT(parser); while(parser->top_node) nmea_parser_drop(parser); 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 Fill nmeaINFO structure by DBT packet data. * @param pack a pointer of packet structure. * @param info a pointer of summary information structure. */ void nmea_SDDBT2info(nmeaSDDBT *pack, nmeaINFO *info) { NMEA_ASSERT(pack && info); info->depthinfo.depth_f = pack->depth_f; info->depthinfo.depth_F = pack->depth_F; info->depthinfo.depth_M = pack->depth_M; info->smask |= SDDBT; }
/** * \brief Fill nmeaINFO structure by HCHDG packet data. * @param pack a pointer of packet structure. * @param info a pointer of summary information structure. */ void nmea_HCHDG2info(nmeaHCHDG *pack, nmeaINFO *info) { NMEA_ASSERT(pack && info); info->magnetic_sensor_heading = pack->mag_heading; info->magnetic_sensor_deviation = ((pack->ew_deviation == 'E')?pack->mag_deviation:-(pack->mag_deviation)); info->magnetic_sensor_variation = ((pack->ew_variation == 'E')?pack->mag_variation:-(pack->mag_variation)); }
/** * \brief Fill nmeaINFO structure by HCHDT packet data. * @param pack a pointer of packet structure. * @param info a pointer of summary information structure. */ void nmea_HCHDT2info(nmeaHCHDT *pack, nmeaINFO *info) { NMEA_ASSERT(pack && info); if('T' == pack->t_flag) info->magnetic_sensor_heading_true = pack->direction; info->smask |= HCHDT; }
/** * \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) { NMEA_ASSERT(pack && info); info->direction = pack->dir; info->declination = pack->dec; info->speed = pack->spk; info->smask |= GPVTG; }
/** * \brief Destroy parser object */ void nmea_parser_destroy(nmeaPARSER *parser) { NMEA_ASSERT(parser); if (parser->buffer) { free(parser->buffer); parser->buffer = NULL; } nmea_parser_queue_clear(parser); memset(parser, 0, sizeof(nmeaPARSER)); }
/** * \brief Fill nmeaINFO structure by ROT packet data. * @param pack a pointer of packet structure. * @param info a pointer of summary information structure. */ void nmea_TIROT2info(nmeaTIROT *pack, nmeaINFO *info) { NMEA_ASSERT(pack && info); if('A' == pack->status) { info->rate_turn = pack->rate; } }
/** * \brief Analysis of buffer and put results to information structure * @return Number of packets wos parsed */ int nmea_parse( nmeaPARSER *parser, const char *buff, int buff_sz, nmeaINFO *info ) { int ptype, nread = 0; void *pack = 0; NMEA_ASSERT(parser && parser->buffer); // Analyze buffer and take info into parser nmea_parser_push(parser, buff, buff_sz); while(GPNON != (ptype = nmea_parser_pop(parser, &pack))) { nread++; switch(ptype) // ADD: call function to add info parsed here { case GPGGA: nmea_GPGGA2info((nmeaGPGGA *)pack, info); break; case GPGSA: nmea_GPGSA2info((nmeaGPGSA *)pack, info); break; case GPGSV: nmea_GPGSV2info((nmeaGPGSV *)pack, info); break; case GPRMC: nmea_GPRMC2info((nmeaGPRMC *)pack, info); break; case GPVTG: nmea_GPVTG2info((nmeaGPVTG *)pack, info); break; case HCHDG: nmea_HCHDG2info((nmeaHCHDG *)pack, info); break; case HCHDT: nmea_HCHDT2info((nmeaHCHDT *)pack, info); break; case TIROT: nmea_TIROT2info((nmeaTIROT *)pack, info); break; case YXXDR: nmea_YXXDR2info((nmeaYXXDR *)pack, info); break; }; free(pack); } return nread; }
/** * \brief Get type of top packet keeped into parser * @return Type of packet * @see nmeaPACKTYPE */ int nmea_parser_top(nmeaPARSER *parser) { int retval = GPNON; nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node; NMEA_ASSERT(parser && parser->buffer); if(node) retval = node->packType; return retval; }
/** * \brief Analysis of buffer and put results to information structure * @return Number of packets wos parsed */ int nmea_parse( nmeaPARSER *parser, const char *buff, int buff_sz, nmeaINFO *info ) { int ptype, nread = 0; void *pack = 0; //char c; //debugga NMEA_ASSERT(parser && parser->buffer); nmea_parser_push(parser, buff, buff_sz); while(GPNON != (ptype = nmea_parser_pop(parser, &pack))) { nread++; //KAY #ifndef BRIDGE_USB_GSM //USBWriteStr("POP: "); #endif switch(ptype) { case GPGGA: nmea_GPGGA2info((nmeaGPGGA *)pack, info); break; case GPGSA: nmea_GPGSA2info((nmeaGPGSA *)pack, info); break; case GPGSV: nmea_GPGSV2info((nmeaGPGSV *)pack, info); break; case GPRMC: nmea_GPRMC2info((nmeaGPRMC *)pack, info); break; case GPVTG: nmea_GPVTG2info((nmeaGPVTG *)pack, info); break; }; free(pack); } //KAY #ifndef BRIDGE_USB_GSM //USBWriteStr("nread: "); //int2char(&c,nread,1,10); //USBWriteChar(c); //USBWriteStr("\r\n"); #endif return nread; }
/** * \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) { int i; NMEA_ASSERT(buff); if(buff_sz < 5) return GPNON; for (i = 0;i < g_TypeTotal; i++) { if (0 == memcmp(buff, &(g_nmeaheads[i]), 5)) return g_nmeaheads[i].typevalue; } return GPNON; }
/** * \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) { NMEA_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 Get top packet from parser without withdraw * @return Received packet type * @see nmeaPACKTYPE */ int nmea_parser_peek(nmeaPARSER *parser, void **pack_ptr) { int retval = GPNON; nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node; NMEA_ASSERT(parser && parser->buffer); if(node) { *pack_ptr = node->pack; retval = node->packType; } return retval; }
/** * \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 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 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; }
/** * \brief Analysis of buffer and put results to information structure * @return Number of packets wos parsed */ int nmea_parse( nmeaPARSER *parser, const char *buff, int buff_sz, nmeaINFO *info ) { int ptype, nread = 0; void *pack = 0; NMEA_ASSERT(parser && parser->buffer); nmea_parser_push(parser, buff, buff_sz); while(GPNON != (ptype = nmea_parser_pop(parser, &pack))) { nread++; switch(ptype) { case GPGGA: nmea_GPGGA2info((nmeaGPGGA *)pack, info); break; case GPGSA: nmea_GPGSA2info((nmeaGPGSA *)pack, info); break; case GPGSV: nmea_GPGSV2info((nmeaGPGSV *)pack, info); break; case GPRMC: nmea_GPRMC2info((nmeaGPRMC *)pack, info); break; case GPVTG: nmea_GPVTG2info((nmeaGPVTG *)pack, info); break; default: break; }; free(pack); } return 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; // EDIT: the end_buff can be a char instead a pointer to the last location of buffer 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 = 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 Withdraw top packet from parser * @return Received packet type * @see nmeaPACKTYPE */ int nmea_parser_pop(nmeaPARSER *parser, void **pack_ptr) { int retval = GPNON; nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node; NMEA_ASSERT(parser && parser->buffer); if(node) { *pack_ptr = node->pack; retval = node->packType; parser->top_node = node->next_node; if(!parser->top_node) parser->end_node = 0; free(node); } return retval; }
/** * \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; }