parse.c

00001 /*
00002  *
00003  * NMEA library
00004  * URL: http://nmea.sourceforge.net
00005  * Author: Tim (xtimor@gmail.com)
00006  * Licence: http://www.gnu.org/licenses/lgpl.html
00007  * $Id: parse.c 17 2008-03-11 11:56:11Z xtimor $
00008  *
00009  */
00010 
00011 /**
00012  * \file parse.h
00013  * \brief Functions of a low level for analysis of
00014  * packages of NMEA stream.
00015  *
00016  * \code
00017  * ...
00018  * ptype = nmea_pack_type(
00019  *     (const char *)parser->buffer + nparsed + 1,
00020  *     parser->buff_use - nparsed - 1);
00021  * 
00022  * if(0 == (node = malloc(sizeof(nmeaParserNODE))))
00023  *     goto mem_fail;
00024  * 
00025  * node->pack = 0;
00026  * 
00027  * switch(ptype)
00028  * {
00029  * case GPGGA:
00030  *     if(0 == (node->pack = malloc(sizeof(nmeaGPGGA))))
00031  *         goto mem_fail;
00032  *     node->packType = GPGGA;
00033  *     if(!nmea_parse_GPGGA(
00034  *         (const char *)parser->buffer + nparsed,
00035  *         sen_sz, (nmeaGPGGA *)node->pack))
00036  *     {
00037  *         free(node);
00038  *         node = 0;
00039  *     }
00040  *     break;
00041  * case GPGSA:
00042  *     if(0 == (node->pack = malloc(sizeof(nmeaGPGSA))))
00043  *         goto mem_fail;
00044  *     node->packType = GPGSA;
00045  *     if(!nmea_parse_GPGSA(
00046  *         (const char *)parser->buffer + nparsed,
00047  *         sen_sz, (nmeaGPGSA *)node->pack))
00048  *     {
00049  *         free(node);
00050  *         node = 0;
00051  *     }
00052  *     break;
00053  * ...
00054  * \endcode
00055  */
00056 
00057 #include "nmea/tok.h "
00058 #include "nmea/parse.h"
00059 #include "nmea/context.h"
00060 #include "nmea/gmath.h "
00061 #include "nmea/units.h"
00062 
00063 #include <string.h>
00064 #include <stdio.h>
00065 
00066 int _nmea_parse_time(const char *buff, int buff_sz, nmeaTIME *res)
00067 {
00068     int success = 0;
00069 
00070     switch(buff_sz)
00071     {
00072     case sizeof("hhmmss") - 1:
00073         success = (3 == nmea_scanf(buff, buff_sz,
00074             "%2d%2d%2d", &(res->hour), &(res->min), &(res->sec)
00075             ));
00076         break;
00077     case sizeof("hhmmss.s") - 1:
00078     case sizeof("hhmmss.ss") - 1:
00079     case sizeof("hhmmss.sss") - 1:
00080         success = (4 == nmea_scanf(buff, buff_sz,
00081             "%2d%2d%2d.%d", &(res->hour), &(res->min), &(res->sec), &(res->hsec)
00082             ));
00083         break;
00084     default:
00085         nmea_error("Parse of time error (format error)!");
00086         success = 0;
00087         break;
00088     }
00089 
00090     return (success?0:-1);        
00091 }
00092 
00093 /**
00094  * \brief Define packet type by header (nmeaPACKTYPE).
00095  * @param buff a constant character pointer of packet buffer.
00096  * @param buff_sz buffer size.
00097  * @return The defined packet type
00098  * @see nmeaPACKTYPE
00099  */
00100 int nmea_pack_type(const char *buff, int buff_sz)
00101 {
00102     static const char *pheads[] = {
00103         "GPGGA",
00104         "GPGSA",
00105         "GPGSV",
00106         "GPRMC",
00107         "GPVTG",
00108     };
00109 
00110     NMEA_ASSERT(buff);
00111 
00112     if(buff_sz < 5)
00113         return GPNON;
00114     else if(0 == memcmp(buff, pheads[0], 5))
00115         return GPGGA;
00116     else if(0 == memcmp(buff, pheads[1], 5))
00117         return GPGSA;
00118     else if(0 == memcmp(buff, pheads[2], 5))
00119         return GPGSV;
00120     else if(0 == memcmp(buff, pheads[3], 5))
00121         return GPRMC;
00122     else if(0 == memcmp(buff, pheads[4], 5))
00123         return GPVTG;
00124 
00125     return GPNON;
00126 }
00127 
00128 /**
00129  * \brief Find tail of packet ("\r\n") in buffer and check control sum (CRC).
00130  * @param buff a constant character pointer of packets buffer.
00131  * @param buff_sz buffer size.
00132  * @param res_crc a integer pointer for return CRC of packet (must be defined).
00133  * @return Number of bytes to packet tail.
00134  */
00135 int nmea_find_tail(const char *buff, int buff_sz, int *res_crc)
00136 {
00137     static const int tail_sz = 3 /* *[CRC] */ + 2 /* \r\n */;
00138 
00139     const char *end_buff = buff + buff_sz;
00140     int nread = 0;
00141     int crc = 0;
00142 
00143     NMEA_ASSERT(buff && res_crc);
00144 
00145     *res_crc = -1;
00146 
00147     for(;buff < end_buff; ++buff, ++nread)
00148     {
00149         if(('$' == *buff) && nread)
00150         {
00151             buff = 0;
00152             break;
00153         }
00154         else if('*' == *buff)
00155         {
00156             if(buff + tail_sz <= end_buff && '\r' == buff[3] && '\n' == buff[4])
00157             {
00158                 *res_crc = nmea_atoi(buff + 1, 2, 16);
00159                 nread = buff_sz - (int)(end_buff - (buff + tail_sz));
00160                 if(*res_crc != crc)
00161                 {
00162                     *res_crc = -1;
00163                     buff = 0;
00164                 }
00165             }
00166 
00167             break;
00168         }
00169         else if(nread)
00170             crc ^= (int)*buff;
00171     }
00172 
00173     if(*res_crc < 0 && buff)
00174         nread = 0;
00175 
00176     return nread;
00177 }
00178 
00179 /**
00180  * \brief Parse GGA packet from buffer.
00181  * @param buff a constant character pointer of packet buffer.
00182  * @param buff_sz buffer size.
00183  * @param pack a pointer of packet which will filled by function.
00184  * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
00185  */
00186 int nmea_parse_GPGGA(const char *buff, int buff_sz, nmeaGPGGA *pack)
00187 {
00188     char time_buff[NMEA_TIMEPARSE_BUF];
00189 
00190     NMEA_ASSERT(buff && pack);
00191 
00192     memset(pack, 0, sizeof(nmeaGPGGA));
00193 
00194     nmea_trace_buff(buff, buff_sz);
00195 
00196     if(14 != nmea_scanf(buff, buff_sz,
00197         "$GPGGA,%s,%f,%C,%f,%C,%d,%d,%f,%f,%C,%f,%C,%f,%d*",
00198         &(time_buff[0]),
00199         &(pack->lat), &(pack->ns), &(pack->lon), &(pack->ew),
00200         &(pack->sig), &(pack->satinuse), &(pack->HDOP), &(pack->elv), &(pack->elv_units),
00201         &(pack->diff), &(pack->diff_units), &(pack->dgps_age), &(pack->dgps_sid)))
00202     {
00203         nmea_error("GPGGA parse error!");
00204         return 0;
00205     }
00206 
00207     if(0 != _nmea_parse_time(&time_buff[0], (int)strlen(&time_buff[0]), &(pack->utc)))
00208     {
00209         nmea_error("GPGGA time parse error!");
00210         return 0;
00211     }
00212 
00213     return 1;
00214 }
00215 
00216 /**
00217  * \brief Parse GSA packet from buffer.
00218  * @param buff a constant character pointer of packet buffer.
00219  * @param buff_sz buffer size.
00220  * @param pack a pointer of packet which will filled by function.
00221  * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
00222  */
00223 int nmea_parse_GPGSA(const char *buff, int buff_sz, nmeaGPGSA *pack)
00224 {
00225     NMEA_ASSERT(buff && pack);
00226 
00227     memset(pack, 0, sizeof(nmeaGPGSA));
00228 
00229     nmea_trace_buff(buff, buff_sz);
00230 
00231     if(17 != nmea_scanf(buff, buff_sz,
00232         "$GPGSA,%C,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f,%f*",
00233         &(pack->fix_mode), &(pack->fix_type),
00234         &(pack->sat_prn[0]), &(pack->sat_prn[1]), &(pack->sat_prn[2]), &(pack->sat_prn[3]), &(pack->sat_prn[4]), &(pack->sat_prn[5]),
00235         &(pack->sat_prn[6]), &(pack->sat_prn[7]), &(pack->sat_prn[8]), &(pack->sat_prn[9]), &(pack->sat_prn[10]), &(pack->sat_prn[11]),
00236         &(pack->PDOP), &(pack->HDOP), &(pack->VDOP)))
00237     {
00238         nmea_error("GPGSA parse error!");
00239         return 0;
00240     }
00241 
00242     return 1;
00243 }
00244 
00245 /**
00246  * \brief Parse GSV packet from buffer.
00247  * @param buff a constant character pointer of packet buffer.
00248  * @param buff_sz buffer size.
00249  * @param pack a pointer of packet which will filled by function.
00250  * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
00251  */
00252 int nmea_parse_GPGSV(const char *buff, int buff_sz, nmeaGPGSV *pack)
00253 {
00254     int nsen, nsat;
00255 
00256     NMEA_ASSERT(buff && pack);
00257 
00258     memset(pack, 0, sizeof(nmeaGPGSV));
00259 
00260     nmea_trace_buff(buff, buff_sz);
00261 
00262     nsen = nmea_scanf(buff, buff_sz,
00263         "$GPGSV,%d,%d,%d,"
00264         "%d,%d,%d,%d,"
00265         "%d,%d,%d,%d,"
00266         "%d,%d,%d,%d,"
00267         "%d,%d,%d,%d*",
00268         &(pack->pack_count), &(pack->pack_index), &(pack->sat_count),
00269         &(pack->sat_data[0].id), &(pack->sat_data[0].elv), &(pack->sat_data[0].azimuth), &(pack->sat_data[0].sig),
00270         &(pack->sat_data[1].id), &(pack->sat_data[1].elv), &(pack->sat_data[1].azimuth), &(pack->sat_data[1].sig),
00271         &(pack->sat_data[2].id), &(pack->sat_data[2].elv), &(pack->sat_data[2].azimuth), &(pack->sat_data[2].sig),
00272         &(pack->sat_data[3].id), &(pack->sat_data[3].elv), &(pack->sat_data[3].azimuth), &(pack->sat_data[3].sig));
00273 
00274     nsat = (pack->pack_index - 1) * NMEA_SATINPACK;
00275     nsat = (nsat + NMEA_SATINPACK > pack->sat_count)?pack->sat_count - nsat:NMEA_SATINPACK;
00276     nsat = nsat * 4 + 3 /* first three sentence`s */;
00277 
00278     if(nsen < nsat || nsen > (NMEA_SATINPACK * 4 + 3))
00279     {
00280         nmea_error("GPGSV parse error!");
00281         return 0;
00282     }
00283 
00284     return 1;
00285 }
00286 
00287 /**
00288  * \brief Parse RMC packet from buffer.
00289  * @param buff a constant character pointer of packet buffer.
00290  * @param buff_sz buffer size.
00291  * @param pack a pointer of packet which will filled by function.
00292  * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
00293  */
00294 int nmea_parse_GPRMC(const char *buff, int buff_sz, nmeaGPRMC *pack)
00295 {
00296     int nsen;
00297     
00298     char time_buff[NMEA_TIMEPARSE_BUF];
00299 
00300     NMEA_ASSERT(buff && pack);
00301 
00302     memset(pack, 0, sizeof(nmeaGPRMC));
00303 
00304     nmea_trace_buff(buff, buff_sz);
00305 
00306     nsen = nmea_scanf(buff, buff_sz,
00307         "$GPRMC,%s,%C,%f,%C,%f,%C,%f,%f,%2d%2d%2d,%f,%C,%C*",
00308         &(time_buff[0]),
00309         &(pack->status), &(pack->lat), &(pack->ns), &(pack->lon), &(pack->ew),
00310         &(pack->speed), &(pack->direction),
00311         &(pack->utc.day), &(pack->utc.mon), &(pack->utc.year),
00312         &(pack->declination), &(pack->declin_ew), &(pack->mode));
00313 
00314     if(nsen != 13 && nsen != 14)
00315     {
00316         nmea_error("GPRMC parse error!");
00317         return 0;
00318     }
00319 
00320     if(0 != _nmea_parse_time(&time_buff[0], (int)strlen(&time_buff[0]), &(pack->utc)))
00321     {
00322         nmea_error("GPRMC time parse error!");
00323         return 0;
00324     }
00325 
00326     if(pack->utc.year < 90)
00327         pack->utc.year += 100;
00328     pack->utc.mon -= 1;
00329 
00330     return 1;
00331 }
00332 
00333 /**
00334  * \brief Parse VTG packet from buffer.
00335  * @param buff a constant character pointer of packet buffer.
00336  * @param buff_sz buffer size.
00337  * @param pack a pointer of packet which will filled by function.
00338  * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
00339  */
00340 int nmea_parse_GPVTG(const char *buff, int buff_sz, nmeaGPVTG *pack)
00341 {
00342     NMEA_ASSERT(buff && pack);
00343 
00344     memset(pack, 0, sizeof(nmeaGPVTG));
00345 
00346     nmea_trace_buff(buff, buff_sz);
00347 
00348     if(8 != nmea_scanf(buff, buff_sz,
00349         "$GPVTG,%f,%C,%f,%C,%f,%C,%f,%C*",
00350         &(pack->dir), &(pack->dir_t),
00351         &(pack->dec), &(pack->dec_m),
00352         &(pack->spn), &(pack->spn_n),
00353         &(pack->spk), &(pack->spk_k)))
00354     {
00355         nmea_error("GPVTG parse error!");
00356         return 0;
00357     }
00358 
00359     if( pack->dir_t != 'T' ||
00360         pack->dec_m != 'M' ||
00361         pack->spn_n != 'N' ||
00362         pack->spk_k != 'K')
00363     {
00364         nmea_error("GPVTG parse error (format error)!");
00365         return 0;
00366     }
00367 
00368     return 1;
00369 }
00370 
00371 /**
00372  * \brief Fill nmeaINFO structure by GGA packet data.
00373  * @param pack a pointer of packet structure.
00374  * @param info a pointer of summary information structure.
00375  */
00376 void nmea_GPGGA2info(nmeaGPGGA *pack, nmeaINFO *info)
00377 {
00378     NMEA_ASSERT(pack && info);
00379 
00380     info->utc.hour = pack->utc.hour;
00381     info->utc.min = pack->utc.min;
00382     info->utc.sec = pack->utc.sec;
00383     info->utc.hsec = pack->utc.hsec;
00384     info->sig = pack->sig;
00385     info->HDOP = pack->HDOP;
00386     info->elv = pack->elv;
00387     info->lat = ((pack->ns == 'N')?pack->lat:-(pack->lat));
00388     info->lon = ((pack->ew == 'E')?pack->lon:-(pack->lon));
00389     info->smask |= GPGGA;
00390 }
00391 
00392 /**
00393  * \brief Fill nmeaINFO structure by GSA packet data.
00394  * @param pack a pointer of packet structure.
00395  * @param info a pointer of summary information structure.
00396  */
00397 void nmea_GPGSA2info(nmeaGPGSA *pack, nmeaINFO *info)
00398 {
00399     int i, j, nuse = 0;
00400 
00401     NMEA_ASSERT(pack && info);
00402 
00403     info->fix = pack->fix_type;
00404     info->PDOP = pack->PDOP;
00405     info->HDOP = pack->HDOP;
00406     info->VDOP = pack->VDOP;
00407 
00408     for(i = 0; i < NMEA_MAXSAT; ++i)
00409     {
00410         for(j = 0; j < info->satinfo.inview; ++j)
00411         {
00412             if(pack->sat_prn[i] && pack->sat_prn[i] == info->satinfo.sat[j].id)
00413             {
00414                 info->satinfo.sat[j].in_use = 1;
00415                 nuse++;
00416             }
00417         }
00418     }
00419 
00420     info->satinfo.inuse = nuse;
00421     info->smask |= GPGSA;
00422 }
00423 
00424 /**
00425  * \brief Fill nmeaINFO structure by GSV packet data.
00426  * @param pack a pointer of packet structure.
00427  * @param info a pointer of summary information structure.
00428  */
00429 void nmea_GPGSV2info(nmeaGPGSV *pack, nmeaINFO *info)
00430 {
00431     int isat, isi, nsat;
00432 
00433     NMEA_ASSERT(pack && info);
00434 
00435     if(pack->pack_index > pack->pack_count ||
00436         pack->pack_index * NMEA_SATINPACK > NMEA_MAXSAT)
00437         return;
00438 
00439     if(pack->pack_index < 1)
00440         pack->pack_index = 1;
00441 
00442     info->satinfo.inview = pack->sat_count;
00443 
00444     nsat = (pack->pack_index - 1) * NMEA_SATINPACK;
00445     nsat = (nsat + NMEA_SATINPACK > pack->sat_count)?pack->sat_count - nsat:NMEA_SATINPACK;
00446 
00447     for(isat = 0; isat < nsat; ++isat)
00448     {
00449         isi = (pack->pack_index - 1) * NMEA_SATINPACK + isat;
00450         info->satinfo.sat[isi].id = pack->sat_data[isat].id;
00451         info->satinfo.sat[isi].elv = pack->sat_data[isat].elv;
00452         info->satinfo.sat[isi].azimuth = pack->sat_data[isat].azimuth;
00453         info->satinfo.sat[isi].sig = pack->sat_data[isat].sig;
00454     }
00455 
00456     info->smask |= GPGSV;
00457 }
00458 
00459 /**
00460  * \brief Fill nmeaINFO structure by RMC packet data.
00461  * @param pack a pointer of packet structure.
00462  * @param info a pointer of summary information structure.
00463  */
00464 void nmea_GPRMC2info(nmeaGPRMC *pack, nmeaINFO *info)
00465 {
00466     NMEA_ASSERT(pack && info);
00467 
00468     if('A' == pack->status)
00469     {
00470         if(NMEA_SIG_BAD == info->sig)
00471             info->sig = NMEA_SIG_MID;
00472         if(NMEA_FIX_BAD == info->fix)
00473             info->fix = NMEA_FIX_2D;
00474     }
00475     else if('V' == pack->status)
00476     {
00477         info->sig = NMEA_SIG_BAD;
00478         info->fix = NMEA_FIX_BAD;
00479     }
00480 
00481     info->utc = pack->utc;
00482     info->lat = ((pack->ns == 'N')?pack->lat:-(pack->lat));
00483     info->lon = ((pack->ew == 'E')?pack->lon:-(pack->lon));
00484     info->speed = pack->speed * NMEA_TUD_KNOTS;
00485     info->direction = pack->direction;
00486     info->smask |= GPRMC;
00487 }
00488 
00489 /**
00490  * \brief Fill nmeaINFO structure by VTG packet data.
00491  * @param pack a pointer of packet structure.
00492  * @param info a pointer of summary information structure.
00493  */
00494 void nmea_GPVTG2info(nmeaGPVTG *pack, nmeaINFO *info)
00495 {
00496     NMEA_ASSERT(pack && info);
00497 
00498     info->direction = pack->dir;
00499     info->declination = pack->dec;
00500     info->speed = pack->spk;
00501     info->smask |= GPVTG;
00502 }