nmealib是一个基于C语言的用于nmea协议的开源库。
在http://nmea.sourceforge.net/上下载的。
部分文件分析：
在头文件info.h中定义了NMEA解码需要的关键变量和结构体。
在头文件time.h中定义了NMEA日期和时间结构体。
在头文件sentence.h中定义了需要解析的NMEA数据格式结构体。
在sentence.c文件中，针对五种解析频率较高的GPS帧信息编写了各自的初始化函数。
在parse.c文件中定义了对NMEA数据流进行解码的底层函数。
在头文件parser.h中定义了NMEA解码使用的nmeaPARSER结构体，该结构体是组成对GPS NMEA信息进行解码的链表节点的基础。
在parser.c文件中定义了解析NMEA信息最底层的函数，这些函数在parser.c中继续进行封装，以供上层函数调用。
在头文件gmath.h中对一些数学常数进行了宏定义。
在gmath.c文件中定义了计算地理信息需要的数学计算函数。函数nmea_distance返回两坐标点之间的直线距离。函数nmea_distance_ellipsoid用于计算地球表面两个点之间的距离。
以下是nmealib解析GPRMC数据的整个代码流程：
/** * Date and time data * @see nmea_time_now */ typedef struct _nmeaTIME { int year; /**< Years since 1900 */ int mon; /**< Months since January - [0,11] */ int day; /**< Day of the month - [1,31] */ int hour; /**< Hours since midnight - [0,23] */ int min; /**< Minutes after the hour - [0,59] */ int sec; /**< Seconds after the minute - [0,59] */ int hsec; /**< Hundredth part of second - [0,99] */ } nmeaTIME; /** * RMC packet information structure (Recommended Minimum sentence C) */ typedef struct _nmeaGPRMC { nmeaTIME utc; /**< UTC of position */ char status; /**< Status (A = active or V = void) */ double lat; /**< Latitude in NDEG - [degree][min].[sec/60] */ char ns; /**< [N]orth or [S]outh */ double lon; /**< Longitude in NDEG - [degree][min].[sec/60] */ char ew; /**< [E]ast or [W]est */ double speed; /**< Speed over the ground in knots */ double direction; /**< Track angle in degrees True */ double declination; /**< Magnetic variation degrees (Easterly var. subtracts from true course) */ char declin_ew; /**< [E]ast or [W]est */ char mode; /**< Mode indicator of fix type (A = autonomous, D = differential, E = estimated, N = not valid, S = simulator) */ } nmeaGPRMC; #define NMEA_CONVSTR_BUF (256) #define NMEA_TIMEPARSE_BUF (256) #define NMEA_DEF_PARSEBUFF (1024) #define NMEA_MIN_PARSEBUFF (256) #define NMEA_ASSERT(x) RT_ASSERT(x) typedef void(*nmeaTraceFunc)(const char *str, int str_size); typedef void(*nmeaErrorFunc)(const char *str, int str_size); typedef struct _nmeaPROPERTY { nmeaTraceFunc trace_func; nmeaErrorFunc error_func; int parse_buff_size; } nmeaPROPERTY; nmeaPROPERTY * nmea_property(void); nmeaPROPERTY * nmea_property(void) { static nmeaPROPERTY prop = { 0, 0, NMEA_DEF_PARSEBUFF }; return ∝ } void nmea_trace_buff(const char *buff, int buff_size) { nmeaTraceFunc func = nmea_property()->trace_func; if(func && buff_size) (*func)(buff, buff_size); } #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; } #if defined(_MSC_VER) # define NMEA_POSIX(x) _##x # define NMEA_INLINE __inline #else # define NMEA_POSIX(x) x # define NMEA_INLINE inline #endif void nmea_error(const char *str, ...) { int size; va_list arg_list; char buff[NMEA_DEF_PARSEBUFF]; nmeaErrorFunc func = nmea_property()->error_func; if(func) { va_start(arg_list, str); size = NMEA_POSIX(vsnprintf)(&buff[0], NMEA_DEF_PARSEBUFF - 1, str, arg_list); va_end(arg_list); if(size > 0) (*func)(&buff[0], size); } } 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 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, "$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) { nmea_error("GPRMC parse error!"); 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; } #define NMEA_SIG_BAD (0) #define NMEA_SIG_LOW (1) #define NMEA_SIG_MID (2) #define NMEA_SIG_HIGH (3) #define NMEA_FIX_BAD (1) #define NMEA_FIX_2D (2) #define NMEA_FIX_3D (3) #define NMEA_MAXSAT (12) #define NMEA_SATINPACK (4) #define NMEA_NSATPACKS (NMEA_MAXSAT / NMEA_SATINPACK) #define NMEA_DEF_LAT (5001.2621) #define NMEA_DEF_LON (3613.0595) /** * Position data in fractional degrees or radians */ typedef struct _nmeaPOS { double lat; /**< Latitude */ double lon; /**< Longitude */ } nmeaPOS; /** * Information about satellite * @see nmeaSATINFO * @see nmeaGPGSV */ typedef struct _nmeaSATELLITE { int id; /**< Satellite PRN number */ int in_use; /**< Used in position fix */ int elv; /**< Elevation in degrees, 90 maximum */ int azimuth; /**< Azimuth, degrees from true north, 000 to 359 */ int sig; /**< Signal, 00-99 dB */ } nmeaSATELLITE; /** * Information about all satellites in view * @see nmeaINFO * @see nmeaGPGSV */ typedef struct _nmeaSATINFO { int inuse; /**< Number of satellites in use (not those in view) */ int inview; /**< Total number of satellites in view */ nmeaSATELLITE sat[NMEA_MAXSAT]; /**< Satellites information */ } nmeaSATINFO; /** * Summary GPS information from all parsed packets, * used also for generating NMEA stream * @see nmea_parse * @see nmea_GPGGA2info, nmea_...2info */ typedef struct _nmeaINFO { int smask; /**< Mask specifying types of packages from which data have been obtained */ nmeaTIME utc; /**< UTC of position */ int sig; /**< GPS quality indicator (0 = Invalid; 1 = Fix; 2 = Differential, 3 = Sensitive) */ int fix; /**< Operating mode, used for navigation (1 = Fix not available; 2 = 2D; 3 = 3D) */ double PDOP; /**< Position Dilution Of Precision */ double HDOP; /**< Horizontal Dilution Of Precision */ double VDOP; /**< Vertical Dilution Of Precision */ double lat; /**< Latitude in NDEG - +/-[degree][min].[sec/60] */ double lon; /**< Longitude in NDEG - +/-[degree][min].[sec/60] */ double elv; /**< Antenna altitude above/below mean sea level (geoid) in meters */ double speed; /**< Speed over the ground in kilometers/hour */ double direction; /**< Track angle in degrees True */ double declination; /**< Magnetic variation degrees (Easterly var. subtracts from true course) */ nmeaSATINFO satinfo; /**< Satellites information */ } nmeaINFO; /* * high level */ typedef struct _nmeaPARSER { void *top_node; void *end_node; unsigned char *buffer; int buff_size; int buff_use; } nmeaPARSER; void nmea_time_now(nmeaTIME *stm) { time_t t; struct tm st; t = time(NULL); localtime_r(&t, &st); stm->year = st.tm_year; stm->mon = st.tm_mon; stm->day = st.tm_mday; stm->hour = st.tm_hour; stm->min = st.tm_min; stm->sec = st.tm_sec; stm->hsec = 0; } void nmea_zero_INFO(nmeaINFO *info) { memset(info, 0, sizeof(nmeaINFO)); nmea_time_now(&info->utc); info->sig = NMEA_SIG_BAD; info->fix = NMEA_FIX_BAD; info->smask = 0; } /** * NMEA packets type which parsed and generated by library */ enum nmeaPACKTYPE { GPNON = 0x0000, /**< Unknown packet type. */ GPGGA = 0x0001, /**< GGA - Essential fix data which provide 3D location and accuracy data. */ GPGSA = 0x0002, /**< GSA - GPS receiver operating mode, SVs used for navigation, and DOP values. */ GPGSV = 0x0004, /**< GSV - Number of SVs in view, PRN numbers, elevation, azimuth & SNR values. */ GPRMC = 0x0008, /**< RMC - Recommended Minimum Specific GPS/TRANSIT Data. */ GPVTG = 0x0010 /**< VTG - Actual track made good and speed over ground. */ }; typedef struct _nmeaParserNODE { int packType; void *pack; struct _nmeaParserNODE *next_node; } nmeaParserNODE; /** * \brief Delete top packet from parser * @return Deleted packet type * @see nmeaPACKTYPE */ int nmea_parser_drop(nmeaPARSER *parser) { int retval = GPNON; nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node; NMEA_ASSERT(parser && parser->buffer); if(node) { if(node->pack) free(node->pack); retval = node->packType; parser->top_node = node->next_node; if(!parser->top_node) parser->end_node = 0; free(node); } return retval; } /** * \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; } /** * \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 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 Clear cache of parser * @return true (1) - success */ int nmea_parser_buff_clear(nmeaPARSER *parser) { NMEA_ASSERT(parser && parser->buffer); parser->buff_use = 0; return 1; } /** * \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; 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 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; } 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: free(node); node = 0; break; case GPGSA: free(node); node = 0; break; case GPGSV: 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: 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; } /** * \brief Analysis of buffer and keep results into parser * @return Number of bytes wos parsed from buffer */ int nmea_parser_push(nmeaPARSER *parser, const char *buff, int buff_sz) { int nparse, nparsed = 0; do { if(buff_sz > parser->buff_size) nparse = parser->buff_size; else nparse = buff_sz; nparsed += nmea_parser_real_push( parser, buff, nparse); buff_sz -= nparse; } while(buff_sz); return nparsed; } #define NMEA_TUD_YARDS (1.0936) /**< Yeards, meter * NMEA_TUD_YARDS = yard */ #define NMEA_TUD_KNOTS (1.852) /**< Knots, kilometer / NMEA_TUD_KNOTS = knot */ #define NMEA_TUD_MILES (1.609) /**< Miles, kilometer / NMEA_TUD_MILES = mile */ /* * Speed units */ #define NMEA_TUS_MS (3.6) /**< Meters per seconds, (k/h) / NMEA_TUS_MS= (m/s) */ /** * \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) { NMEA_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 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 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: break; case GPGSA: break; case GPGSV: break; case GPRMC: nmea_GPRMC2info((nmeaGPRMC *)pack, info); break; case GPVTG: break; }; free(pack); } return nread; } int main_gps() { const char *buff[] = { "$GPRMC,173843,A,3349.896,N,11808.521,W,000.0,360.0,230108,013.4,E*69\r\n", "$GPRMC,111609.14,A,5001.27,N,3613.06,E,11.2,0.0,261206,0.0,E*50\r\n", }; int it; nmeaINFO info; nmeaPARSER parser; nmea_zero_INFO(&info); nmea_parser_init(&parser); for(it = 0; it < 2; ++it) nmea_parse(&parser, buff[it], (int)strlen(buff[it]), &info); nmea_parser_destroy(&parser); return 0; }
扫一扫
4372
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
