diff options
author | px4dev <px4@purgatory.org> | 2012-08-04 15:12:36 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-08-04 15:12:36 -0700 |
commit | 8a365179eafdf3aea98e60ab9f5882b200d4c759 (patch) | |
tree | 4f38d6d4cd80bd0b6e22e2bb534c3f117ce44e56 /apps/gps/nmealib/parse.c | |
download | px4-firmware-8a365179eafdf3aea98e60ab9f5882b200d4c759.tar.gz px4-firmware-8a365179eafdf3aea98e60ab9f5882b200d4c759.tar.bz2 px4-firmware-8a365179eafdf3aea98e60ab9f5882b200d4c759.zip |
Fresh import of the PX4 firmware sources.
Diffstat (limited to 'apps/gps/nmealib/parse.c')
-rw-r--r-- | apps/gps/nmealib/parse.c | 501 |
1 files changed, 501 insertions, 0 deletions
diff --git a/apps/gps/nmealib/parse.c b/apps/gps/nmealib/parse.c new file mode 100644 index 000000000..99bdf075b --- /dev/null +++ b/apps/gps/nmealib/parse.c @@ -0,0 +1,501 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: parse.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +/** + * \file parse.h + * \brief Functions of a low level for analysis of + * packages of NMEA stream. + * + * \code + * ... + * 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); + * 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; + * ... + * \endcode + */ +#include "nmea/tok.h" +#include "nmea/parse.h" +#include "nmea/context.h" +#include "nmea/gmath.h" +#include "nmea/units.h" + +#include <string.h> +#include <stdio.h> + +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 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 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 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 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 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 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; +} + +/** + * \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 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 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) + { + 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; + + //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; + + 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) +{ + //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 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; +} |