Guido Ottaviani
/
LeonardoMbos
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbos Watchdog TextLCD mbed ConfigFile
nmea/parse.c@0:d177c0087d1f, 2012-01-29 (annotated)
- Committer:
- guiott
- Date:
- Sun Jan 29 16:06:12 2012 +0000
- Revision:
- 0:d177c0087d1f
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| guiott | 0:d177c0087d1f | 1 | /* |
| guiott | 0:d177c0087d1f | 2 | * |
| guiott | 0:d177c0087d1f | 3 | * NMEA library |
| guiott | 0:d177c0087d1f | 4 | * URL: http://nmea.sourceforge.net |
| guiott | 0:d177c0087d1f | 5 | * Author: Tim (xtimor@gmail.com) |
| guiott | 0:d177c0087d1f | 6 | * Licence: http://www.gnu.org/licenses/lgpl.html |
| guiott | 0:d177c0087d1f | 7 | * $Id: parse.c 17 2008-03-11 11:56:11Z xtimor $ |
| guiott | 0:d177c0087d1f | 8 | * |
| guiott | 0:d177c0087d1f | 9 | */ |
| guiott | 0:d177c0087d1f | 10 | |
| guiott | 0:d177c0087d1f | 11 | /** |
| guiott | 0:d177c0087d1f | 12 | * \file parse.h |
| guiott | 0:d177c0087d1f | 13 | * \brief Functions of a low level for analysis of |
| guiott | 0:d177c0087d1f | 14 | * packages of NMEA stream. |
| guiott | 0:d177c0087d1f | 15 | * |
| guiott | 0:d177c0087d1f | 16 | * \code |
| guiott | 0:d177c0087d1f | 17 | * ... |
| guiott | 0:d177c0087d1f | 18 | * ptype = nmea_pack_type( |
| guiott | 0:d177c0087d1f | 19 | * (const char *)parser->buffer + nparsed + 1, |
| guiott | 0:d177c0087d1f | 20 | * parser->buff_use - nparsed - 1); |
| guiott | 0:d177c0087d1f | 21 | * |
| guiott | 0:d177c0087d1f | 22 | * if(0 == (node = malloc(sizeof(nmeaParserNODE)))) |
| guiott | 0:d177c0087d1f | 23 | * goto mem_fail; |
| guiott | 0:d177c0087d1f | 24 | * |
| guiott | 0:d177c0087d1f | 25 | * node->pack = 0; |
| guiott | 0:d177c0087d1f | 26 | * |
| guiott | 0:d177c0087d1f | 27 | * switch(ptype) |
| guiott | 0:d177c0087d1f | 28 | * { |
| guiott | 0:d177c0087d1f | 29 | * case GPGGA: |
| guiott | 0:d177c0087d1f | 30 | * if(0 == (node->pack = malloc(sizeof(nmeaGPGGA)))) |
| guiott | 0:d177c0087d1f | 31 | * goto mem_fail; |
| guiott | 0:d177c0087d1f | 32 | * node->packType = GPGGA; |
| guiott | 0:d177c0087d1f | 33 | * if(!nmea_parse_GPGGA( |
| guiott | 0:d177c0087d1f | 34 | * (const char *)parser->buffer + nparsed, |
| guiott | 0:d177c0087d1f | 35 | * sen_sz, (nmeaGPGGA *)node->pack)) |
| guiott | 0:d177c0087d1f | 36 | * { |
| guiott | 0:d177c0087d1f | 37 | * free(node); |
| guiott | 0:d177c0087d1f | 38 | * node = 0; |
| guiott | 0:d177c0087d1f | 39 | * } |
| guiott | 0:d177c0087d1f | 40 | * break; |
| guiott | 0:d177c0087d1f | 41 | * case GPGSA: |
| guiott | 0:d177c0087d1f | 42 | * if(0 == (node->pack = malloc(sizeof(nmeaGPGSA)))) |
| guiott | 0:d177c0087d1f | 43 | * goto mem_fail; |
| guiott | 0:d177c0087d1f | 44 | * node->packType = GPGSA; |
| guiott | 0:d177c0087d1f | 45 | * if(!nmea_parse_GPGSA( |
| guiott | 0:d177c0087d1f | 46 | * (const char *)parser->buffer + nparsed, |
| guiott | 0:d177c0087d1f | 47 | * sen_sz, (nmeaGPGSA *)node->pack)) |
| guiott | 0:d177c0087d1f | 48 | * { |
| guiott | 0:d177c0087d1f | 49 | * free(node); |
| guiott | 0:d177c0087d1f | 50 | * node = 0; |
| guiott | 0:d177c0087d1f | 51 | * } |
| guiott | 0:d177c0087d1f | 52 | * break; |
| guiott | 0:d177c0087d1f | 53 | * ... |
| guiott | 0:d177c0087d1f | 54 | * \endcode |
| guiott | 0:d177c0087d1f | 55 | */ |
| guiott | 0:d177c0087d1f | 56 | |
| guiott | 0:d177c0087d1f | 57 | #include "nmea/tok.h" |
| guiott | 0:d177c0087d1f | 58 | #include "nmea/parse.h" |
| guiott | 0:d177c0087d1f | 59 | #include "nmea/context.h" |
| guiott | 0:d177c0087d1f | 60 | #include "nmea/gmath.h" |
| guiott | 0:d177c0087d1f | 61 | #include "nmea/units.h" |
| guiott | 0:d177c0087d1f | 62 | |
| guiott | 0:d177c0087d1f | 63 | #include <string.h> |
| guiott | 0:d177c0087d1f | 64 | #include <stdio.h> |
| guiott | 0:d177c0087d1f | 65 | |
| guiott | 0:d177c0087d1f | 66 | int _nmea_parse_time(const char *buff, int buff_sz, nmeaTIME *res) |
| guiott | 0:d177c0087d1f | 67 | { |
| guiott | 0:d177c0087d1f | 68 | int success = 0; |
| guiott | 0:d177c0087d1f | 69 | |
| guiott | 0:d177c0087d1f | 70 | switch(buff_sz) |
| guiott | 0:d177c0087d1f | 71 | { |
| guiott | 0:d177c0087d1f | 72 | case sizeof("hhmmss") - 1: |
| guiott | 0:d177c0087d1f | 73 | success = (3 == nmea_scanf(buff, buff_sz, |
| guiott | 0:d177c0087d1f | 74 | "%2d%2d%2d", &(res->hour), &(res->min), &(res->sec) |
| guiott | 0:d177c0087d1f | 75 | )); |
| guiott | 0:d177c0087d1f | 76 | break; |
| guiott | 0:d177c0087d1f | 77 | case sizeof("hhmmss.s") - 1: |
| guiott | 0:d177c0087d1f | 78 | case sizeof("hhmmss.ss") - 1: |
| guiott | 0:d177c0087d1f | 79 | case sizeof("hhmmss.sss") - 1: |
| guiott | 0:d177c0087d1f | 80 | success = (4 == nmea_scanf(buff, buff_sz, |
| guiott | 0:d177c0087d1f | 81 | "%2d%2d%2d.%d", &(res->hour), &(res->min), &(res->sec), &(res->hsec) |
| guiott | 0:d177c0087d1f | 82 | )); |
| guiott | 0:d177c0087d1f | 83 | break; |
| guiott | 0:d177c0087d1f | 84 | default: |
| guiott | 0:d177c0087d1f | 85 | nmea_error("Parse of time error (format error)!"); |
| guiott | 0:d177c0087d1f | 86 | success = 0; |
| guiott | 0:d177c0087d1f | 87 | break; |
| guiott | 0:d177c0087d1f | 88 | } |
| guiott | 0:d177c0087d1f | 89 | |
| guiott | 0:d177c0087d1f | 90 | return (success?0:-1); |
| guiott | 0:d177c0087d1f | 91 | } |
| guiott | 0:d177c0087d1f | 92 | |
| guiott | 0:d177c0087d1f | 93 | /** |
| guiott | 0:d177c0087d1f | 94 | * \brief Define packet type by header (nmeaPACKTYPE). |
| guiott | 0:d177c0087d1f | 95 | * @param buff a constant character pointer of packet buffer. |
| guiott | 0:d177c0087d1f | 96 | * @param buff_sz buffer size. |
| guiott | 0:d177c0087d1f | 97 | * @return The defined packet type |
| guiott | 0:d177c0087d1f | 98 | * @see nmeaPACKTYPE |
| guiott | 0:d177c0087d1f | 99 | */ |
| guiott | 0:d177c0087d1f | 100 | int nmea_pack_type(const char *buff, int buff_sz) |
| guiott | 0:d177c0087d1f | 101 | { |
| guiott | 0:d177c0087d1f | 102 | static const char *pheads[] = { |
| guiott | 0:d177c0087d1f | 103 | "GPGGA", |
| guiott | 0:d177c0087d1f | 104 | "GPGSA", |
| guiott | 0:d177c0087d1f | 105 | "GPGSV", |
| guiott | 0:d177c0087d1f | 106 | "GPRMC", |
| guiott | 0:d177c0087d1f | 107 | "GPVTG", |
| guiott | 0:d177c0087d1f | 108 | }; |
| guiott | 0:d177c0087d1f | 109 | |
| guiott | 0:d177c0087d1f | 110 | NMEA_ASSERT(buff); |
| guiott | 0:d177c0087d1f | 111 | |
| guiott | 0:d177c0087d1f | 112 | if(buff_sz < 5) |
| guiott | 0:d177c0087d1f | 113 | return GPNON; |
| guiott | 0:d177c0087d1f | 114 | else if(0 == memcmp(buff, pheads[0], 5)) |
| guiott | 0:d177c0087d1f | 115 | return GPGGA; |
| guiott | 0:d177c0087d1f | 116 | else if(0 == memcmp(buff, pheads[1], 5)) |
| guiott | 0:d177c0087d1f | 117 | return GPGSA; |
| guiott | 0:d177c0087d1f | 118 | else if(0 == memcmp(buff, pheads[2], 5)) |
| guiott | 0:d177c0087d1f | 119 | return GPGSV; |
| guiott | 0:d177c0087d1f | 120 | else if(0 == memcmp(buff, pheads[3], 5)) |
| guiott | 0:d177c0087d1f | 121 | return GPRMC; |
| guiott | 0:d177c0087d1f | 122 | else if(0 == memcmp(buff, pheads[4], 5)) |
| guiott | 0:d177c0087d1f | 123 | return GPVTG; |
| guiott | 0:d177c0087d1f | 124 | |
| guiott | 0:d177c0087d1f | 125 | return GPNON; |
| guiott | 0:d177c0087d1f | 126 | } |
| guiott | 0:d177c0087d1f | 127 | |
| guiott | 0:d177c0087d1f | 128 | /** |
| guiott | 0:d177c0087d1f | 129 | * \brief Find tail of packet ("\r\n") in buffer and check control sum (CRC). |
| guiott | 0:d177c0087d1f | 130 | * @param buff a constant character pointer of packets buffer. |
| guiott | 0:d177c0087d1f | 131 | * @param buff_sz buffer size. |
| guiott | 0:d177c0087d1f | 132 | * @param res_crc a integer pointer for return CRC of packet (must be defined). |
| guiott | 0:d177c0087d1f | 133 | * @return Number of bytes to packet tail. |
| guiott | 0:d177c0087d1f | 134 | */ |
| guiott | 0:d177c0087d1f | 135 | int nmea_find_tail(const char *buff, int buff_sz, int *res_crc) |
| guiott | 0:d177c0087d1f | 136 | { |
| guiott | 0:d177c0087d1f | 137 | static const int tail_sz = 3 /* *[CRC] */ + 2 /* \r\n */; |
| guiott | 0:d177c0087d1f | 138 | |
| guiott | 0:d177c0087d1f | 139 | const char *end_buff = buff + buff_sz; |
| guiott | 0:d177c0087d1f | 140 | int nread = 0; |
| guiott | 0:d177c0087d1f | 141 | int crc = 0; |
| guiott | 0:d177c0087d1f | 142 | |
| guiott | 0:d177c0087d1f | 143 | NMEA_ASSERT(buff && res_crc); |
| guiott | 0:d177c0087d1f | 144 | |
| guiott | 0:d177c0087d1f | 145 | *res_crc = -1; |
| guiott | 0:d177c0087d1f | 146 | |
| guiott | 0:d177c0087d1f | 147 | for(;buff < end_buff; ++buff, ++nread) |
| guiott | 0:d177c0087d1f | 148 | { |
| guiott | 0:d177c0087d1f | 149 | if(('$' == *buff) && nread) |
| guiott | 0:d177c0087d1f | 150 | { |
| guiott | 0:d177c0087d1f | 151 | buff = 0; |
| guiott | 0:d177c0087d1f | 152 | break; |
| guiott | 0:d177c0087d1f | 153 | } |
| guiott | 0:d177c0087d1f | 154 | else if('*' == *buff) |
| guiott | 0:d177c0087d1f | 155 | { |
| guiott | 0:d177c0087d1f | 156 | if(buff + tail_sz <= end_buff && '\r' == buff[3] && '\n' == buff[4]) |
| guiott | 0:d177c0087d1f | 157 | { |
| guiott | 0:d177c0087d1f | 158 | *res_crc = nmea_atoi(buff + 1, 2, 16); |
| guiott | 0:d177c0087d1f | 159 | nread = buff_sz - (int)(end_buff - (buff + tail_sz)); |
| guiott | 0:d177c0087d1f | 160 | if(*res_crc != crc) |
| guiott | 0:d177c0087d1f | 161 | { |
| guiott | 0:d177c0087d1f | 162 | *res_crc = -1; |
| guiott | 0:d177c0087d1f | 163 | buff = 0; |
| guiott | 0:d177c0087d1f | 164 | } |
| guiott | 0:d177c0087d1f | 165 | } |
| guiott | 0:d177c0087d1f | 166 | |
| guiott | 0:d177c0087d1f | 167 | break; |
| guiott | 0:d177c0087d1f | 168 | } |
| guiott | 0:d177c0087d1f | 169 | else if(nread) |
| guiott | 0:d177c0087d1f | 170 | crc ^= (int)*buff; |
| guiott | 0:d177c0087d1f | 171 | } |
| guiott | 0:d177c0087d1f | 172 | |
| guiott | 0:d177c0087d1f | 173 | if(*res_crc < 0 && buff) |
| guiott | 0:d177c0087d1f | 174 | nread = 0; |
| guiott | 0:d177c0087d1f | 175 | |
| guiott | 0:d177c0087d1f | 176 | return nread; |
| guiott | 0:d177c0087d1f | 177 | } |
| guiott | 0:d177c0087d1f | 178 | |
| guiott | 0:d177c0087d1f | 179 | /** |
| guiott | 0:d177c0087d1f | 180 | * \brief Parse GGA packet from buffer. |
| guiott | 0:d177c0087d1f | 181 | * @param buff a constant character pointer of packet buffer. |
| guiott | 0:d177c0087d1f | 182 | * @param buff_sz buffer size. |
| guiott | 0:d177c0087d1f | 183 | * @param pack a pointer of packet which will filled by function. |
| guiott | 0:d177c0087d1f | 184 | * @return 1 (true) - if parsed successfully or 0 (false) - if fail. |
| guiott | 0:d177c0087d1f | 185 | */ |
| guiott | 0:d177c0087d1f | 186 | int nmea_parse_GPGGA(const char *buff, int buff_sz, nmeaGPGGA *pack) |
| guiott | 0:d177c0087d1f | 187 | { |
| guiott | 0:d177c0087d1f | 188 | char time_buff[NMEA_TIMEPARSE_BUF]; |
| guiott | 0:d177c0087d1f | 189 | |
| guiott | 0:d177c0087d1f | 190 | NMEA_ASSERT(buff && pack); |
| guiott | 0:d177c0087d1f | 191 | |
| guiott | 0:d177c0087d1f | 192 | memset(pack, 0, sizeof(nmeaGPGGA)); |
| guiott | 0:d177c0087d1f | 193 | |
| guiott | 0:d177c0087d1f | 194 | nmea_trace_buff(buff, buff_sz); |
| guiott | 0:d177c0087d1f | 195 | |
| guiott | 0:d177c0087d1f | 196 | if(14 != nmea_scanf(buff, buff_sz, |
| guiott | 0:d177c0087d1f | 197 | "$GPGGA,%s,%f,%C,%f,%C,%d,%d,%f,%f,%C,%f,%C,%f,%d*", |
| guiott | 0:d177c0087d1f | 198 | &(time_buff[0]), |
| guiott | 0:d177c0087d1f | 199 | &(pack->lat), &(pack->ns), &(pack->lon), &(pack->ew), |
| guiott | 0:d177c0087d1f | 200 | &(pack->sig), &(pack->satinuse), &(pack->HDOP), &(pack->elv), &(pack->elv_units), |
| guiott | 0:d177c0087d1f | 201 | &(pack->diff), &(pack->diff_units), &(pack->dgps_age), &(pack->dgps_sid))) |
| guiott | 0:d177c0087d1f | 202 | { |
| guiott | 0:d177c0087d1f | 203 | nmea_error("GPGGA parse error!"); |
| guiott | 0:d177c0087d1f | 204 | return 0; |
| guiott | 0:d177c0087d1f | 205 | } |
| guiott | 0:d177c0087d1f | 206 | |
| guiott | 0:d177c0087d1f | 207 | if(0 != _nmea_parse_time(&time_buff[0], (int)strlen(&time_buff[0]), &(pack->utc))) |
| guiott | 0:d177c0087d1f | 208 | { |
| guiott | 0:d177c0087d1f | 209 | nmea_error("GPGGA time parse error!"); |
| guiott | 0:d177c0087d1f | 210 | return 0; |
| guiott | 0:d177c0087d1f | 211 | } |
| guiott | 0:d177c0087d1f | 212 | |
| guiott | 0:d177c0087d1f | 213 | return 1; |
| guiott | 0:d177c0087d1f | 214 | } |
| guiott | 0:d177c0087d1f | 215 | |
| guiott | 0:d177c0087d1f | 216 | /** |
| guiott | 0:d177c0087d1f | 217 | * \brief Parse GSA packet from buffer. |
| guiott | 0:d177c0087d1f | 218 | * @param buff a constant character pointer of packet buffer. |
| guiott | 0:d177c0087d1f | 219 | * @param buff_sz buffer size. |
| guiott | 0:d177c0087d1f | 220 | * @param pack a pointer of packet which will filled by function. |
| guiott | 0:d177c0087d1f | 221 | * @return 1 (true) - if parsed successfully or 0 (false) - if fail. |
| guiott | 0:d177c0087d1f | 222 | */ |
| guiott | 0:d177c0087d1f | 223 | int nmea_parse_GPGSA(const char *buff, int buff_sz, nmeaGPGSA *pack) |
| guiott | 0:d177c0087d1f | 224 | { |
| guiott | 0:d177c0087d1f | 225 | NMEA_ASSERT(buff && pack); |
| guiott | 0:d177c0087d1f | 226 | |
| guiott | 0:d177c0087d1f | 227 | memset(pack, 0, sizeof(nmeaGPGSA)); |
| guiott | 0:d177c0087d1f | 228 | |
| guiott | 0:d177c0087d1f | 229 | nmea_trace_buff(buff, buff_sz); |
| guiott | 0:d177c0087d1f | 230 | |
| guiott | 0:d177c0087d1f | 231 | if(17 != nmea_scanf(buff, buff_sz, |
| guiott | 0:d177c0087d1f | 232 | "$GPGSA,%C,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f,%f*", |
| guiott | 0:d177c0087d1f | 233 | &(pack->fix_mode), &(pack->fix_type), |
| guiott | 0:d177c0087d1f | 234 | &(pack->sat_prn[0]), &(pack->sat_prn[1]), &(pack->sat_prn[2]), &(pack->sat_prn[3]), &(pack->sat_prn[4]), &(pack->sat_prn[5]), |
| guiott | 0:d177c0087d1f | 235 | &(pack->sat_prn[6]), &(pack->sat_prn[7]), &(pack->sat_prn[8]), &(pack->sat_prn[9]), &(pack->sat_prn[10]), &(pack->sat_prn[11]), |
| guiott | 0:d177c0087d1f | 236 | &(pack->PDOP), &(pack->HDOP), &(pack->VDOP))) |
| guiott | 0:d177c0087d1f | 237 | { |
| guiott | 0:d177c0087d1f | 238 | nmea_error("GPGSA parse error!"); |
| guiott | 0:d177c0087d1f | 239 | return 0; |
| guiott | 0:d177c0087d1f | 240 | } |
| guiott | 0:d177c0087d1f | 241 | |
| guiott | 0:d177c0087d1f | 242 | return 1; |
| guiott | 0:d177c0087d1f | 243 | } |
| guiott | 0:d177c0087d1f | 244 | |
| guiott | 0:d177c0087d1f | 245 | /** |
| guiott | 0:d177c0087d1f | 246 | * \brief Parse GSV packet from buffer. |
| guiott | 0:d177c0087d1f | 247 | * @param buff a constant character pointer of packet buffer. |
| guiott | 0:d177c0087d1f | 248 | * @param buff_sz buffer size. |
| guiott | 0:d177c0087d1f | 249 | * @param pack a pointer of packet which will filled by function. |
| guiott | 0:d177c0087d1f | 250 | * @return 1 (true) - if parsed successfully or 0 (false) - if fail. |
| guiott | 0:d177c0087d1f | 251 | */ |
| guiott | 0:d177c0087d1f | 252 | int nmea_parse_GPGSV(const char *buff, int buff_sz, nmeaGPGSV *pack) |
| guiott | 0:d177c0087d1f | 253 | { |
| guiott | 0:d177c0087d1f | 254 | int nsen, nsat; |
| guiott | 0:d177c0087d1f | 255 | |
| guiott | 0:d177c0087d1f | 256 | NMEA_ASSERT(buff && pack); |
| guiott | 0:d177c0087d1f | 257 | |
| guiott | 0:d177c0087d1f | 258 | memset(pack, 0, sizeof(nmeaGPGSV)); |
| guiott | 0:d177c0087d1f | 259 | |
| guiott | 0:d177c0087d1f | 260 | nmea_trace_buff(buff, buff_sz); |
| guiott | 0:d177c0087d1f | 261 | |
| guiott | 0:d177c0087d1f | 262 | nsen = nmea_scanf(buff, buff_sz, |
| guiott | 0:d177c0087d1f | 263 | "$GPGSV,%d,%d,%d," |
| guiott | 0:d177c0087d1f | 264 | "%d,%d,%d,%d," |
| guiott | 0:d177c0087d1f | 265 | "%d,%d,%d,%d," |
| guiott | 0:d177c0087d1f | 266 | "%d,%d,%d,%d," |
| guiott | 0:d177c0087d1f | 267 | "%d,%d,%d,%d*", |
| guiott | 0:d177c0087d1f | 268 | &(pack->pack_count), &(pack->pack_index), &(pack->sat_count), |
| guiott | 0:d177c0087d1f | 269 | &(pack->sat_data[0].id), &(pack->sat_data[0].elv), &(pack->sat_data[0].azimuth), &(pack->sat_data[0].sig), |
| guiott | 0:d177c0087d1f | 270 | &(pack->sat_data[1].id), &(pack->sat_data[1].elv), &(pack->sat_data[1].azimuth), &(pack->sat_data[1].sig), |
| guiott | 0:d177c0087d1f | 271 | &(pack->sat_data[2].id), &(pack->sat_data[2].elv), &(pack->sat_data[2].azimuth), &(pack->sat_data[2].sig), |
| guiott | 0:d177c0087d1f | 272 | &(pack->sat_data[3].id), &(pack->sat_data[3].elv), &(pack->sat_data[3].azimuth), &(pack->sat_data[3].sig)); |
| guiott | 0:d177c0087d1f | 273 | |
| guiott | 0:d177c0087d1f | 274 | nsat = (pack->pack_index - 1) * NMEA_SATINPACK; |
| guiott | 0:d177c0087d1f | 275 | nsat = (nsat + NMEA_SATINPACK > pack->sat_count)?pack->sat_count - nsat:NMEA_SATINPACK; |
| guiott | 0:d177c0087d1f | 276 | nsat = nsat * 4 + 3 /* first three sentence`s */; |
| guiott | 0:d177c0087d1f | 277 | |
| guiott | 0:d177c0087d1f | 278 | if(nsen < nsat || nsen > (NMEA_SATINPACK * 4 + 3)) |
| guiott | 0:d177c0087d1f | 279 | { |
| guiott | 0:d177c0087d1f | 280 | nmea_error("GPGSV parse error!"); |
| guiott | 0:d177c0087d1f | 281 | return 0; |
| guiott | 0:d177c0087d1f | 282 | } |
| guiott | 0:d177c0087d1f | 283 | |
| guiott | 0:d177c0087d1f | 284 | return 1; |
| guiott | 0:d177c0087d1f | 285 | } |
| guiott | 0:d177c0087d1f | 286 | |
| guiott | 0:d177c0087d1f | 287 | /** |
| guiott | 0:d177c0087d1f | 288 | * \brief Parse RMC packet from buffer. |
| guiott | 0:d177c0087d1f | 289 | * @param buff a constant character pointer of packet buffer. |
| guiott | 0:d177c0087d1f | 290 | * @param buff_sz buffer size. |
| guiott | 0:d177c0087d1f | 291 | * @param pack a pointer of packet which will filled by function. |
| guiott | 0:d177c0087d1f | 292 | * @return 1 (true) - if parsed successfully or 0 (false) - if fail. |
| guiott | 0:d177c0087d1f | 293 | */ |
| guiott | 0:d177c0087d1f | 294 | int nmea_parse_GPRMC(const char *buff, int buff_sz, nmeaGPRMC *pack) |
| guiott | 0:d177c0087d1f | 295 | { |
| guiott | 0:d177c0087d1f | 296 | int nsen; |
| guiott | 0:d177c0087d1f | 297 | |
| guiott | 0:d177c0087d1f | 298 | char time_buff[NMEA_TIMEPARSE_BUF]; |
| guiott | 0:d177c0087d1f | 299 | |
| guiott | 0:d177c0087d1f | 300 | NMEA_ASSERT(buff && pack); |
| guiott | 0:d177c0087d1f | 301 | |
| guiott | 0:d177c0087d1f | 302 | memset(pack, 0, sizeof(nmeaGPRMC)); |
| guiott | 0:d177c0087d1f | 303 | |
| guiott | 0:d177c0087d1f | 304 | nmea_trace_buff(buff, buff_sz); |
| guiott | 0:d177c0087d1f | 305 | |
| guiott | 0:d177c0087d1f | 306 | nsen = nmea_scanf(buff, buff_sz, |
| guiott | 0:d177c0087d1f | 307 | "$GPRMC,%s,%C,%f,%C,%f,%C,%f,%f,%2d%2d%2d,%f,%C,%C*", |
| guiott | 0:d177c0087d1f | 308 | &(time_buff[0]), |
| guiott | 0:d177c0087d1f | 309 | &(pack->status), &(pack->lat), &(pack->ns), &(pack->lon), &(pack->ew), |
| guiott | 0:d177c0087d1f | 310 | &(pack->speed), &(pack->direction), |
| guiott | 0:d177c0087d1f | 311 | &(pack->utc.day), &(pack->utc.mon), &(pack->utc.year), |
| guiott | 0:d177c0087d1f | 312 | &(pack->declination), &(pack->declin_ew), &(pack->mode)); |
| guiott | 0:d177c0087d1f | 313 | |
| guiott | 0:d177c0087d1f | 314 | if(nsen != 13 && nsen != 14) |
| guiott | 0:d177c0087d1f | 315 | { |
| guiott | 0:d177c0087d1f | 316 | nmea_error("GPRMC parse error!"); |
| guiott | 0:d177c0087d1f | 317 | return 0; |
| guiott | 0:d177c0087d1f | 318 | } |
| guiott | 0:d177c0087d1f | 319 | |
| guiott | 0:d177c0087d1f | 320 | if(0 != _nmea_parse_time(&time_buff[0], (int)strlen(&time_buff[0]), &(pack->utc))) |
| guiott | 0:d177c0087d1f | 321 | { |
| guiott | 0:d177c0087d1f | 322 | nmea_error("GPRMC time parse error!"); |
| guiott | 0:d177c0087d1f | 323 | return 0; |
| guiott | 0:d177c0087d1f | 324 | } |
| guiott | 0:d177c0087d1f | 325 | |
| guiott | 0:d177c0087d1f | 326 | if(pack->utc.year < 90) |
| guiott | 0:d177c0087d1f | 327 | pack->utc.year += 100; |
| guiott | 0:d177c0087d1f | 328 | pack->utc.mon -= 1; |
| guiott | 0:d177c0087d1f | 329 | |
| guiott | 0:d177c0087d1f | 330 | return 1; |
| guiott | 0:d177c0087d1f | 331 | } |
| guiott | 0:d177c0087d1f | 332 | |
| guiott | 0:d177c0087d1f | 333 | /** |
| guiott | 0:d177c0087d1f | 334 | * \brief Parse VTG packet from buffer. |
| guiott | 0:d177c0087d1f | 335 | * @param buff a constant character pointer of packet buffer. |
| guiott | 0:d177c0087d1f | 336 | * @param buff_sz buffer size. |
| guiott | 0:d177c0087d1f | 337 | * @param pack a pointer of packet which will filled by function. |
| guiott | 0:d177c0087d1f | 338 | * @return 1 (true) - if parsed successfully or 0 (false) - if fail. |
| guiott | 0:d177c0087d1f | 339 | */ |
| guiott | 0:d177c0087d1f | 340 | int nmea_parse_GPVTG(const char *buff, int buff_sz, nmeaGPVTG *pack) |
| guiott | 0:d177c0087d1f | 341 | { |
| guiott | 0:d177c0087d1f | 342 | NMEA_ASSERT(buff && pack); |
| guiott | 0:d177c0087d1f | 343 | |
| guiott | 0:d177c0087d1f | 344 | memset(pack, 0, sizeof(nmeaGPVTG)); |
| guiott | 0:d177c0087d1f | 345 | |
| guiott | 0:d177c0087d1f | 346 | nmea_trace_buff(buff, buff_sz); |
| guiott | 0:d177c0087d1f | 347 | |
| guiott | 0:d177c0087d1f | 348 | if(8 != nmea_scanf(buff, buff_sz, |
| guiott | 0:d177c0087d1f | 349 | "$GPVTG,%f,%C,%f,%C,%f,%C,%f,%C*", |
| guiott | 0:d177c0087d1f | 350 | &(pack->dir), &(pack->dir_t), |
| guiott | 0:d177c0087d1f | 351 | &(pack->dec), &(pack->dec_m), |
| guiott | 0:d177c0087d1f | 352 | &(pack->spn), &(pack->spn_n), |
| guiott | 0:d177c0087d1f | 353 | &(pack->spk), &(pack->spk_k))) |
| guiott | 0:d177c0087d1f | 354 | { |
| guiott | 0:d177c0087d1f | 355 | nmea_error("GPVTG parse error!"); |
| guiott | 0:d177c0087d1f | 356 | return 0; |
| guiott | 0:d177c0087d1f | 357 | } |
| guiott | 0:d177c0087d1f | 358 | |
| guiott | 0:d177c0087d1f | 359 | if( pack->dir_t != 'T' || |
| guiott | 0:d177c0087d1f | 360 | pack->dec_m != 'M' || |
| guiott | 0:d177c0087d1f | 361 | pack->spn_n != 'N' || |
| guiott | 0:d177c0087d1f | 362 | pack->spk_k != 'K') |
| guiott | 0:d177c0087d1f | 363 | { |
| guiott | 0:d177c0087d1f | 364 | nmea_error("GPVTG parse error (format error)!"); |
| guiott | 0:d177c0087d1f | 365 | return 0; |
| guiott | 0:d177c0087d1f | 366 | } |
| guiott | 0:d177c0087d1f | 367 | |
| guiott | 0:d177c0087d1f | 368 | return 1; |
| guiott | 0:d177c0087d1f | 369 | } |
| guiott | 0:d177c0087d1f | 370 | |
| guiott | 0:d177c0087d1f | 371 | /** |
| guiott | 0:d177c0087d1f | 372 | * \brief Fill nmeaINFO structure by GGA packet data. |
| guiott | 0:d177c0087d1f | 373 | * @param pack a pointer of packet structure. |
| guiott | 0:d177c0087d1f | 374 | * @param info a pointer of summary information structure. |
| guiott | 0:d177c0087d1f | 375 | */ |
| guiott | 0:d177c0087d1f | 376 | void nmea_GPGGA2info(nmeaGPGGA *pack, nmeaINFO *info) |
| guiott | 0:d177c0087d1f | 377 | { |
| guiott | 0:d177c0087d1f | 378 | NMEA_ASSERT(pack && info); |
| guiott | 0:d177c0087d1f | 379 | |
| guiott | 0:d177c0087d1f | 380 | info->utc.hour = pack->utc.hour; |
| guiott | 0:d177c0087d1f | 381 | info->utc.min = pack->utc.min; |
| guiott | 0:d177c0087d1f | 382 | info->utc.sec = pack->utc.sec; |
| guiott | 0:d177c0087d1f | 383 | info->utc.hsec = pack->utc.hsec; |
| guiott | 0:d177c0087d1f | 384 | info->sig = pack->sig; |
| guiott | 0:d177c0087d1f | 385 | info->HDOP = pack->HDOP; |
| guiott | 0:d177c0087d1f | 386 | info->elv = pack->elv; |
| guiott | 0:d177c0087d1f | 387 | info->lat = ((pack->ns == 'N')?pack->lat:-(pack->lat)); |
| guiott | 0:d177c0087d1f | 388 | info->lon = ((pack->ew == 'E')?pack->lon:-(pack->lon)); |
| guiott | 0:d177c0087d1f | 389 | info->smask |= GPGGA; |
| guiott | 0:d177c0087d1f | 390 | } |
| guiott | 0:d177c0087d1f | 391 | |
| guiott | 0:d177c0087d1f | 392 | /** |
| guiott | 0:d177c0087d1f | 393 | * \brief Fill nmeaINFO structure by GSA packet data. |
| guiott | 0:d177c0087d1f | 394 | * @param pack a pointer of packet structure. |
| guiott | 0:d177c0087d1f | 395 | * @param info a pointer of summary information structure. |
| guiott | 0:d177c0087d1f | 396 | */ |
| guiott | 0:d177c0087d1f | 397 | void nmea_GPGSA2info(nmeaGPGSA *pack, nmeaINFO *info) |
| guiott | 0:d177c0087d1f | 398 | { |
| guiott | 0:d177c0087d1f | 399 | int i, j, nuse = 0; |
| guiott | 0:d177c0087d1f | 400 | |
| guiott | 0:d177c0087d1f | 401 | NMEA_ASSERT(pack && info); |
| guiott | 0:d177c0087d1f | 402 | |
| guiott | 0:d177c0087d1f | 403 | info->fix = pack->fix_type; |
| guiott | 0:d177c0087d1f | 404 | info->PDOP = pack->PDOP; |
| guiott | 0:d177c0087d1f | 405 | info->HDOP = pack->HDOP; |
| guiott | 0:d177c0087d1f | 406 | info->VDOP = pack->VDOP; |
| guiott | 0:d177c0087d1f | 407 | |
| guiott | 0:d177c0087d1f | 408 | for(i = 0; i < NMEA_MAXSAT; ++i) |
| guiott | 0:d177c0087d1f | 409 | { |
| guiott | 0:d177c0087d1f | 410 | for(j = 0; j < info->satinfo.inview; ++j) |
| guiott | 0:d177c0087d1f | 411 | { |
| guiott | 0:d177c0087d1f | 412 | if(pack->sat_prn[i] && pack->sat_prn[i] == info->satinfo.sat[j].id) |
| guiott | 0:d177c0087d1f | 413 | { |
| guiott | 0:d177c0087d1f | 414 | info->satinfo.sat[j].in_use = 1; |
| guiott | 0:d177c0087d1f | 415 | nuse++; |
| guiott | 0:d177c0087d1f | 416 | } |
| guiott | 0:d177c0087d1f | 417 | } |
| guiott | 0:d177c0087d1f | 418 | } |
| guiott | 0:d177c0087d1f | 419 | |
| guiott | 0:d177c0087d1f | 420 | info->satinfo.inuse = nuse; |
| guiott | 0:d177c0087d1f | 421 | info->smask |= GPGSA; |
| guiott | 0:d177c0087d1f | 422 | } |
| guiott | 0:d177c0087d1f | 423 | |
| guiott | 0:d177c0087d1f | 424 | /** |
| guiott | 0:d177c0087d1f | 425 | * \brief Fill nmeaINFO structure by GSV packet data. |
| guiott | 0:d177c0087d1f | 426 | * @param pack a pointer of packet structure. |
| guiott | 0:d177c0087d1f | 427 | * @param info a pointer of summary information structure. |
| guiott | 0:d177c0087d1f | 428 | */ |
| guiott | 0:d177c0087d1f | 429 | void nmea_GPGSV2info(nmeaGPGSV *pack, nmeaINFO *info) |
| guiott | 0:d177c0087d1f | 430 | { |
| guiott | 0:d177c0087d1f | 431 | int isat, isi, nsat; |
| guiott | 0:d177c0087d1f | 432 | |
| guiott | 0:d177c0087d1f | 433 | NMEA_ASSERT(pack && info); |
| guiott | 0:d177c0087d1f | 434 | |
| guiott | 0:d177c0087d1f | 435 | if(pack->pack_index > pack->pack_count || |
| guiott | 0:d177c0087d1f | 436 | pack->pack_index * NMEA_SATINPACK > NMEA_MAXSAT) |
| guiott | 0:d177c0087d1f | 437 | return; |
| guiott | 0:d177c0087d1f | 438 | |
| guiott | 0:d177c0087d1f | 439 | if(pack->pack_index < 1) |
| guiott | 0:d177c0087d1f | 440 | pack->pack_index = 1; |
| guiott | 0:d177c0087d1f | 441 | |
| guiott | 0:d177c0087d1f | 442 | info->satinfo.inview = pack->sat_count; |
| guiott | 0:d177c0087d1f | 443 | |
| guiott | 0:d177c0087d1f | 444 | nsat = (pack->pack_index - 1) * NMEA_SATINPACK; |
| guiott | 0:d177c0087d1f | 445 | nsat = (nsat + NMEA_SATINPACK > pack->sat_count)?pack->sat_count - nsat:NMEA_SATINPACK; |
| guiott | 0:d177c0087d1f | 446 | |
| guiott | 0:d177c0087d1f | 447 | for(isat = 0; isat < nsat; ++isat) |
| guiott | 0:d177c0087d1f | 448 | { |
| guiott | 0:d177c0087d1f | 449 | isi = (pack->pack_index - 1) * NMEA_SATINPACK + isat; |
| guiott | 0:d177c0087d1f | 450 | info->satinfo.sat[isi].id = pack->sat_data[isat].id; |
| guiott | 0:d177c0087d1f | 451 | info->satinfo.sat[isi].elv = pack->sat_data[isat].elv; |
| guiott | 0:d177c0087d1f | 452 | info->satinfo.sat[isi].azimuth = pack->sat_data[isat].azimuth; |
| guiott | 0:d177c0087d1f | 453 | info->satinfo.sat[isi].sig = pack->sat_data[isat].sig; |
| guiott | 0:d177c0087d1f | 454 | } |
| guiott | 0:d177c0087d1f | 455 | |
| guiott | 0:d177c0087d1f | 456 | info->smask |= GPGSV; |
| guiott | 0:d177c0087d1f | 457 | } |
| guiott | 0:d177c0087d1f | 458 | |
| guiott | 0:d177c0087d1f | 459 | /** |
| guiott | 0:d177c0087d1f | 460 | * \brief Fill nmeaINFO structure by RMC packet data. |
| guiott | 0:d177c0087d1f | 461 | * @param pack a pointer of packet structure. |
| guiott | 0:d177c0087d1f | 462 | * @param info a pointer of summary information structure. |
| guiott | 0:d177c0087d1f | 463 | */ |
| guiott | 0:d177c0087d1f | 464 | void nmea_GPRMC2info(nmeaGPRMC *pack, nmeaINFO *info) |
| guiott | 0:d177c0087d1f | 465 | { |
| guiott | 0:d177c0087d1f | 466 | NMEA_ASSERT(pack && info); |
| guiott | 0:d177c0087d1f | 467 | |
| guiott | 0:d177c0087d1f | 468 | if('A' == pack->status) |
| guiott | 0:d177c0087d1f | 469 | { |
| guiott | 0:d177c0087d1f | 470 | if(NMEA_SIG_BAD == info->sig) |
| guiott | 0:d177c0087d1f | 471 | info->sig = NMEA_SIG_MID; |
| guiott | 0:d177c0087d1f | 472 | if(NMEA_FIX_BAD == info->fix) |
| guiott | 0:d177c0087d1f | 473 | info->fix = NMEA_FIX_2D; |
| guiott | 0:d177c0087d1f | 474 | } |
| guiott | 0:d177c0087d1f | 475 | else if('V' == pack->status) |
| guiott | 0:d177c0087d1f | 476 | { |
| guiott | 0:d177c0087d1f | 477 | info->sig = NMEA_SIG_BAD; |
| guiott | 0:d177c0087d1f | 478 | info->fix = NMEA_FIX_BAD; |
| guiott | 0:d177c0087d1f | 479 | } |
| guiott | 0:d177c0087d1f | 480 | |
| guiott | 0:d177c0087d1f | 481 | info->utc = pack->utc; |
| guiott | 0:d177c0087d1f | 482 | info->lat = ((pack->ns == 'N')?pack->lat:-(pack->lat)); |
| guiott | 0:d177c0087d1f | 483 | info->lon = ((pack->ew == 'E')?pack->lon:-(pack->lon)); |
| guiott | 0:d177c0087d1f | 484 | info->speed = pack->speed * NMEA_TUD_KNOTS; |
| guiott | 0:d177c0087d1f | 485 | info->direction = pack->direction; |
| guiott | 0:d177c0087d1f | 486 | info->smask |= GPRMC; |
| guiott | 0:d177c0087d1f | 487 | } |
| guiott | 0:d177c0087d1f | 488 | |
| guiott | 0:d177c0087d1f | 489 | /** |
| guiott | 0:d177c0087d1f | 490 | * \brief Fill nmeaINFO structure by VTG packet data. |
| guiott | 0:d177c0087d1f | 491 | * @param pack a pointer of packet structure. |
| guiott | 0:d177c0087d1f | 492 | * @param info a pointer of summary information structure. |
| guiott | 0:d177c0087d1f | 493 | */ |
| guiott | 0:d177c0087d1f | 494 | void nmea_GPVTG2info(nmeaGPVTG *pack, nmeaINFO *info) |
| guiott | 0:d177c0087d1f | 495 | { |
| guiott | 0:d177c0087d1f | 496 | NMEA_ASSERT(pack && info); |
| guiott | 0:d177c0087d1f | 497 | |
| guiott | 0:d177c0087d1f | 498 | info->direction = pack->dir; |
| guiott | 0:d177c0087d1f | 499 | info->declination = pack->dec; |
| guiott | 0:d177c0087d1f | 500 | info->speed = pack->spk; |
| guiott | 0:d177c0087d1f | 501 | info->smask |= GPVTG; |
| guiott | 0:d177c0087d1f | 502 | } |