29 |
30 | void nmea_zero_INFO(nmeaINFO *info)
31 | {
32 | if (!info) {
33 | return;
34 | }
35 |
36 | memset(info, 0, sizeof(nmeaINFO));
37 | nmea_time_now(&info->utc);
38 | info->sig = NMEA_SIG_BAD;
39 | info->fix = NMEA_FIX_BAD;
40 | }
41 |
42 | /**
43 | * Determine whether a given nmeaINFO structure has a certain field.
44 | *
45 | * nmeaINFO dependencies:
46 |
47 | field/sentence GPGGA GPGSA GPGSV GPRMC GPVTG
48 | smask: x x x x x
49 | utc: x x
50 | sig: x x
51 | fix: x x
52 | PDOP: x
53 | HDOP: x x
54 | VDOP: x
55 | lat: x x
56 | lon: x x
57 | elv: x
58 | speed: x x
59 | direction: x x
60 | declination: x
61 | satinfo: x x
62 |
63 | *
64 | * @param smask
65 | * the smask of a nmeaINFO structure
66 | * @param fieldName
67 | * the field name
68 | *
69 | * @return
70 | * - true when the nmeaINFO structure has the field
71 | * - false otherwise
72 | */
73 | bool nmea_INFO_has_field(int smask, nmeaINFO_FIELD fieldName) {
74 | switch (fieldName) {
75 | case SMASK:
76 | return true;
77 |
78 | case UTC:
79 | case SIG:
80 | case LAT:
81 | case LON:
82 | return ((smask & (GPGGA | GPRMC)) != 0);
83 |
84 | case FIX:
85 | return ((smask & (GPGSA | GPRMC)) != 0);
86 |
87 | case PDOP:
88 | case VDOP:
89 | return ((smask & GPGSA) != 0);
90 |
91 | case HDOP:
92 | return ((smask & (GPGGA | GPGSA)) != 0);
93 |
94 | case ELV:
95 | return ((smask & GPGGA) != 0);
96 |
97 | case SPEED:
98 | case DIRECTION:
99 | return ((smask & (GPRMC | GPVTG)) != 0);
100 |
101 | case DECLINATION:
102 | return ((smask & GPVTG) != 0);
103 |
104 | case SATINFO:
105 | return ((smask & (GPGSA | GPGSV)) != 0);
106 |
107 | default:
108 | return false;
109 | }
110 | }
111 |
112 | /**
113 | * Sanitise the NMEA info, make sure that:
114 | * - latitude is in the range [-9000, 9000],
115 | * - longitude is in the range [-18000, 18000],
116 | * - DOPs are positive,
117 | * - speed is positive,
118 | * - direction is in the range [0, 360>.
119 | *
120 | * Time is set to the current time when not present.
121 | *
122 | * When a field is not present then it is reset to its default (NMEA_SIG_BAD,
123 | * NMEA_FIX_BAD, 0).
124 | *
125 | * Satinfo is not touched.
126 | *
127 | * @param nmeaInfo
128 | * the NMEA info structure to sanitise
129 | */
130 | void nmea_INFO_sanitise(nmeaINFO *nmeaInfo) {
131 | double lat = 0;
132 | double lon = 0;
133 | double speed = 0;
134 | double direction = 0;
135 | bool latAdjusted = false;
136 | bool lonAdjusted = false;
137 | bool speedAdjusted = false;
138 | bool directionAdjusted = false;
139 |
140 | if (!nmeaInfo) {
141 | return;
142 | }
143 |
144 | if (!nmea_INFO_has_field(nmeaInfo->smask, UTC)) {
145 | nmea_time_now(&nmeaInfo->utc);
146 | }
147 |
148 | if (!nmea_INFO_has_field(nmeaInfo->smask, SIG)) {
149 | nmeaInfo->sig = NMEA_SIG_BAD;
150 | }
151 |
152 | if (!nmea_INFO_has_field(nmeaInfo->smask, FIX)) {
153 | nmeaInfo->fix = NMEA_FIX_BAD;
154 | }
155 |
156 | if (!nmea_INFO_has_field(nmeaInfo->smask, PDOP)) {
157 | nmeaInfo->PDOP = 0;
158 | } else {
159 | nmeaInfo->PDOP = fabs(nmeaInfo->PDOP);
160 | }
161 |
162 | if (!nmea_INFO_has_field(nmeaInfo->smask, HDOP)) {
163 | nmeaInfo->HDOP = 0;
164 | } else {
165 | nmeaInfo->HDOP = fabs(nmeaInfo->HDOP);
166 | }
167 |
168 | if (!nmea_INFO_has_field(nmeaInfo->smask, VDOP)) {
169 | nmeaInfo->VDOP = 0;
170 | } else {
171 | nmeaInfo->VDOP = fabs(nmeaInfo->VDOP);
172 | }
173 |
174 | if (!nmea_INFO_has_field(nmeaInfo->smask, LAT)) {
175 | nmeaInfo->lat = 0;
176 | }
177 |
178 | if (!nmea_INFO_has_field(nmeaInfo->smask, LON)) {
179 | nmeaInfo->lon = 0;
180 | }
181 |
182 | if (!nmea_INFO_has_field(nmeaInfo->smask, ELV)) {
183 | nmeaInfo->elv = 0;
184 | }
185 |
186 | if (!nmea_INFO_has_field(nmeaInfo->smask, SPEED)) {
187 | nmeaInfo->speed = 0;
188 | }
189 |
190 | if (!nmea_INFO_has_field(nmeaInfo->smask, DIRECTION)) {
191 | nmeaInfo->direction = 0;
192 | }
193 |
194 | if (!nmea_INFO_has_field(nmeaInfo->smask, DECLINATION)) {
195 | nmeaInfo->declination = 0;
196 | }
197 |
198 | /* satinfo is not used */
199 |
200 | /*
201 | * lat
202 | */
203 |
204 | lat = nmeaInfo->lat;
205 | lon = nmeaInfo->lon;
206 |
207 | /* force lat in [-18000, 18000] */
208 | while (lat < -18000.0) {
209 | lat += 36000.0;
210 | latAdjusted = true;
211 | }
212 | while (lat > 18000.0) {
213 | lat -= 36000.0;
214 | latAdjusted = true;
215 | }
216 |
217 | /* lat is now in [-18000, 18000] */
218 |
219 | /* force lat from <9000, 18000] in [9000, 0] */
220 | if (lat > 9000.0) {
221 | lat = 18000.0 - lat;
222 | lon += 18000.0;
223 | latAdjusted = true;
224 | lonAdjusted = true;
225 | }
226 |
227 | /* force lat from [-18000, -9000> in [0, -9000] */
228 | if (lat < -9000.0) {
229 | lat = -18000.0 - lat;
230 | lon += 18000.0;
231 | latAdjusted = true;
232 | lonAdjusted = true;
233 | }
234 |
235 | /* lat is now in [-9000, 9000] */
236 |
237 | if (latAdjusted) {
238 | nmeaInfo->lat = lat;
239 | }
240 |
241 | /*
242 | * lon
243 | */
244 |
245 | /* force lon in [-18000, 18000] */
246 | while (lon < -18000.0) {
247 | lon += 36000.0;
248 | lonAdjusted = true;
249 | }
250 | while (lon > 18000.0) {
251 | lon -= 36000.0;
252 | lonAdjusted = true;
253 | }
254 |
255 | /* lon is now in [-18000, 18000] */
256 |
257 | if (lonAdjusted) {
258 | nmeaInfo->lon = lon;
259 | }
260 |
261 | /*
262 | * speed
263 | */
264 |
265 | speed = nmeaInfo->speed;
266 | direction = nmeaInfo->direction;
267 |
268 | if (speed < 0.0) {
269 | speed = -speed;
270 | direction += 180.0;
271 | speedAdjusted = true;
272 | directionAdjusted = true;
273 | }
274 |
275 | /* speed is now in [0, max> */
276 |
277 | if (speedAdjusted) {
278 | nmeaInfo->speed = speed;
279 | }
280 |
281 | /*
282 | * direction
283 | */
284 |
285 | /* force direction in [0, 360> */
286 | while (direction < 0.0) {
287 | direction += 360.0;
288 | directionAdjusted = true;
289 | }
290 | while (direction >= 360.0) {
291 | direction -= 360.0;
292 | directionAdjusted = true;
293 | }
294 |
295 | /* direction is now in [0, 360> */
296 |
297 | if (directionAdjusted) {
298 | nmeaInfo->direction = direction;
299 | }
300 | }
301 |
302 | /**
303 | * Converts the position fields to degrees and DOP fields to meters so that
304 | * all fields use normal metric units.
305 | *
306 | * @param nmeaInfo
307 | * the nmeaINFO
308 | */
309 | void nmea_INFO_unit_conversion(nmeaINFO * nmeaInfo) {
310 | if (!nmeaInfo) {
311 | return;
312 | }
313 |
314 | /* smask (already in correct format) */
315 |
316 | /* utc (already in correct format) */
317 |
318 | /* sig (already in correct format) */
319 | /* fix (already in correct format) */
320 |
321 | if (nmea_INFO_has_field(nmeaInfo->smask, PDOP)) {
322 | nmeaInfo->PDOP = nmea_dop2meters(nmeaInfo->PDOP);
323 | }
324 |
325 | if (nmea_INFO_has_field(nmeaInfo->smask, HDOP)) {
326 | nmeaInfo->HDOP = nmea_dop2meters(nmeaInfo->HDOP);
327 | }
328 |
329 | if (nmea_INFO_has_field(nmeaInfo->smask, VDOP)) {
330 | nmeaInfo->VDOP = nmea_dop2meters(nmeaInfo->VDOP);
331 | }
332 |
333 | if (nmea_INFO_has_field(nmeaInfo->smask, LAT)) {
334 | nmeaInfo->lat = nmea_ndeg2degree(nmeaInfo->lat);
335 | }
336 |
337 | if (nmea_INFO_has_field(nmeaInfo->smask, LON)) {
338 | nmeaInfo->lon = nmea_ndeg2degree(nmeaInfo->lon);
339 | }
340 |
341 | /* elv (already in correct format) */
342 | /* speed (already in correct format) */
343 | /* direction (already in correct format) */
344 | /* declination (already in correct format) */
345 |
346 | /* satinfo (not used) */
347 | }
348 |
--------------------------------------------------------------------------------
/src/parse.c:
--------------------------------------------------------------------------------
1 | /*
2 | * This file is part of nmealib.
3 | *
4 | * Copyright (c) 2008 Timur Sinitsyn
5 | * Copyright (c) 2011 Ferry Huberts
6 | *
7 | * This library is free software; you can redistribute it and/or
8 | * modify it under the terms of the GNU Lesser General Public
9 | * License as published by the Free Software Foundation; either
10 | * version 2.1 of the License, or (at your option) any later version.
11 | *
12 | * This library is distributed in the hope that it will be useful,
13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 | * Lesser General Public License for more details.
16 | *
17 | * You should have received a copy of the GNU General Public License
18 | * along with this program. If not, see .
19 | */
20 |
21 | /**
22 | * \file parse.h
23 | * \brief Functions of a low level for analysis of
24 | * packages of NMEA stream.
25 | *
26 | * \code
27 | * ...
28 | * ptype = nmea_pack_type(
29 | * (const char *)parser->buffer + nparsed + 1,
30 | * parser->buff_use - nparsed - 1);
31 | *
32 | * if(0 == (node = malloc(sizeof(nmeaParserNODE))))
33 | * goto mem_fail;
34 | *
35 | * node->pack = 0;
36 | *
37 | * switch(ptype)
38 | * {
39 | * case GPGGA:
40 | * if(0 == (node->pack = malloc(sizeof(nmeaGPGGA))))
41 | * goto mem_fail;
42 | * node->packType = GPGGA;
43 | * if(!nmea_parse_GPGGA(
44 | * (const char *)parser->buffer + nparsed,
45 | * sen_sz, (nmeaGPGGA *)node->pack))
46 | * {
47 | * free(node);
48 | * node = 0;
49 | * }
50 | * break;
51 | * case GPGSA:
52 | * if(0 == (node->pack = malloc(sizeof(nmeaGPGSA))))
53 | * goto mem_fail;
54 | * node->packType = GPGSA;
55 | * if(!nmea_parse_GPGSA(
56 | * (const char *)parser->buffer + nparsed,
57 | * sen_sz, (nmeaGPGSA *)node->pack))
58 | * {
59 | * free(node);
60 | * node = 0;
61 | * }
62 | * break;
63 | * ...
64 | * \endcode
65 | */
66 |
67 | #include
68 |
69 | #include
70 |
71 | #include
72 | #include
73 | #include
74 | #include
75 |
76 | #define NMEA_TIMEPARSE_BUF (256)
77 |
78 | int _nmea_parse_time(const char *buff, int buff_sz, nmeaTIME *res)
79 | {
80 | int success = 0;
81 |
82 | switch(buff_sz)
83 | {
84 | case sizeof("hhmmss") - 1:
85 | success = (3 == nmea_scanf(buff, buff_sz,
86 | "%2d%2d%2d", &(res->hour), &(res->min), &(res->sec)
87 | ));
88 | break;
89 | case sizeof("hhmmss.s") - 1:
90 | case sizeof("hhmmss.ss") - 1:
91 | case sizeof("hhmmss.sss") - 1:
92 | success = (4 == nmea_scanf(buff, buff_sz,
93 | "%2d%2d%2d.%d", &(res->hour), &(res->min), &(res->sec), &(res->hsec)
94 | ));
95 | break;
96 | default:
97 | nmea_error("Parse of time error (format error)!");
98 | success = 0;
99 | break;
100 | }
101 |
102 | return (success?0:-1);
103 | }
104 |
105 | /**
106 | * \brief Define packet type by header (nmeaPACKTYPE).
107 | * @param buff a constant character pointer of packet buffer.
108 | * @param buff_sz buffer size.
109 | * @return The defined packet type
110 | * @see nmeaPACKTYPE
111 | */
112 | int nmea_pack_type(const char *buff, int buff_sz)
113 | {
114 | static const char *pheads[] = {
115 | "GPGGA",
116 | "GPGSA",
117 | "GPGSV",
118 | "GPRMC",
119 | "GPVTG",
120 | };
121 |
122 | NMEA_ASSERT(buff);
123 |
124 | if(buff_sz < 5)
125 | return GPNON;
126 | else if(0 == memcmp(buff, pheads[0], 5))
127 | return GPGGA;
128 | else if(0 == memcmp(buff, pheads[1], 5))
129 | return GPGSA;
130 | else if(0 == memcmp(buff, pheads[2], 5))
131 | return GPGSV;
132 | else if(0 == memcmp(buff, pheads[3], 5))
133 | return GPRMC;
134 | else if(0 == memcmp(buff, pheads[4], 5))
135 | return GPVTG;
136 |
137 | return GPNON;
138 | }
139 |
140 | /**
141 | * \brief Find tail of packet ("\r\n") in buffer and check control sum (CRC).
142 | * @param buff a constant character pointer of packets buffer.
143 | * @param buff_sz buffer size.
144 | * @param res_crc a integer pointer for return CRC of packet (must be defined).
145 | * @return Number of bytes to packet tail.
146 | */
147 | int nmea_find_tail(const char *buff, int buff_sz, int *res_crc)
148 | {
149 | static const int tail_sz = 3 /* *[CRC] */ + 2 /* \r\n */;
150 |
151 | const char *end_buff = buff + buff_sz;
152 | int nread = 0;
153 | int crc = 0;
154 |
155 | NMEA_ASSERT(buff && res_crc);
156 |
157 | *res_crc = -1;
158 |
159 | for(;buff < end_buff; ++buff, ++nread)
160 | {
161 | if(('$' == *buff) && nread)
162 | {
163 | buff = 0;
164 | break;
165 | }
166 | else if('*' == *buff)
167 | {
168 | if(buff + tail_sz <= end_buff && '\r' == buff[3] && '\n' == buff[4])
169 | {
170 | *res_crc = nmea_atoi(buff + 1, 2, 16);
171 | nread = buff_sz - (int)(end_buff - (buff + tail_sz));
172 | if(*res_crc != crc)
173 | {
174 | *res_crc = -1;
175 | buff = 0;
176 | }
177 | }
178 |
179 | break;
180 | }
181 | else if(nread)
182 | crc ^= (int)*buff;
183 | }
184 |
185 | if(*res_crc < 0 && buff)
186 | nread = 0;
187 |
188 | return nread;
189 | }
190 |
191 | /**
192 | * \brief Parse GGA packet from buffer.
193 | * @param buff a constant character pointer of packet buffer.
194 | * @param buff_sz buffer size.
195 | * @param pack a pointer of packet which will filled by function.
196 | * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
197 | */
198 | int nmea_parse_GPGGA(const char *buff, int buff_sz, nmeaGPGGA *pack)
199 | {
200 | char time_buff[NMEA_TIMEPARSE_BUF];
201 |
202 | NMEA_ASSERT(buff && pack);
203 |
204 | memset(pack, 0, sizeof(nmeaGPGGA));
205 |
206 | nmea_trace_buff(buff, buff_sz);
207 |
208 | if(14 != nmea_scanf(buff, buff_sz,
209 | "$GPGGA,%s,%f,%C,%f,%C,%d,%d,%f,%f,%C,%f,%C,%f,%d*",
210 | &(time_buff[0]),
211 | &(pack->lat), &(pack->ns), &(pack->lon), &(pack->ew),
212 | &(pack->sig), &(pack->satinuse), &(pack->HDOP), &(pack->elv), &(pack->elv_units),
213 | &(pack->diff), &(pack->diff_units), &(pack->dgps_age), &(pack->dgps_sid)))
214 | {
215 | nmea_error("GPGGA parse error!");
216 | return 0;
217 | }
218 |
219 | if(0 != _nmea_parse_time(&time_buff[0], (int)strlen(&time_buff[0]), &(pack->utc)))
220 | {
221 | nmea_error("GPGGA time parse error!");
222 | return 0;
223 | }
224 |
225 | return 1;
226 | }
227 |
228 | /**
229 | * \brief Parse GSA packet from buffer.
230 | * @param buff a constant character pointer of packet buffer.
231 | * @param buff_sz buffer size.
232 | * @param pack a pointer of packet which will filled by function.
233 | * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
234 | */
235 | int nmea_parse_GPGSA(const char *buff, int buff_sz, nmeaGPGSA *pack)
236 | {
237 | NMEA_ASSERT(buff && pack);
238 |
239 | memset(pack, 0, sizeof(nmeaGPGSA));
240 |
241 | nmea_trace_buff(buff, buff_sz);
242 |
243 | if(17 != nmea_scanf(buff, buff_sz,
244 | "$GPGSA,%C,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f,%f*",
245 | &(pack->fix_mode), &(pack->fix_type),
246 | &(pack->sat_prn[0]), &(pack->sat_prn[1]), &(pack->sat_prn[2]), &(pack->sat_prn[3]), &(pack->sat_prn[4]), &(pack->sat_prn[5]),
247 | &(pack->sat_prn[6]), &(pack->sat_prn[7]), &(pack->sat_prn[8]), &(pack->sat_prn[9]), &(pack->sat_prn[10]), &(pack->sat_prn[11]),
248 | &(pack->PDOP), &(pack->HDOP), &(pack->VDOP)))
249 | {
250 | nmea_error("GPGSA parse error!");
251 | return 0;
252 | }
253 |
254 | return 1;
255 | }
256 |
257 | /**
258 | * \brief Parse GSV packet from buffer.
259 | * @param buff a constant character pointer of packet buffer.
260 | * @param buff_sz buffer size.
261 | * @param pack a pointer of packet which will filled by function.
262 | * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
263 | */
264 | int nmea_parse_GPGSV(const char *buff, int buff_sz, nmeaGPGSV *pack)
265 | {
266 | int nsen, nsat;
267 |
268 | NMEA_ASSERT(buff && pack);
269 |
270 | memset(pack, 0, sizeof(nmeaGPGSV));
271 |
272 | nmea_trace_buff(buff, buff_sz);
273 |
274 | nsen = nmea_scanf(buff, buff_sz,
275 | "$GPGSV,%d,%d,%d,"
276 | "%d,%d,%d,%d,"
277 | "%d,%d,%d,%d,"
278 | "%d,%d,%d,%d,"
279 | "%d,%d,%d,%d*",
280 | &(pack->pack_count), &(pack->pack_index), &(pack->sat_count),
281 | &(pack->sat_data[0].id), &(pack->sat_data[0].elv), &(pack->sat_data[0].azimuth), &(pack->sat_data[0].sig),
282 | &(pack->sat_data[1].id), &(pack->sat_data[1].elv), &(pack->sat_data[1].azimuth), &(pack->sat_data[1].sig),
283 | &(pack->sat_data[2].id), &(pack->sat_data[2].elv), &(pack->sat_data[2].azimuth), &(pack->sat_data[2].sig),
284 | &(pack->sat_data[3].id), &(pack->sat_data[3].elv), &(pack->sat_data[3].azimuth), &(pack->sat_data[3].sig));
285 |
286 | nsat = (pack->pack_index - 1) * NMEA_SATINPACK;
287 | nsat = (nsat + NMEA_SATINPACK > pack->sat_count)?pack->sat_count - nsat:NMEA_SATINPACK;
288 | nsat = nsat * 4 + 3 /* first three sentence`s */;
289 |
290 | if(nsen < nsat || nsen > (NMEA_SATINPACK * 4 + 3))
291 | {
292 | nmea_error("GPGSV parse error!");
293 | return 0;
294 | }
295 |
296 | return 1;
297 | }
298 |
299 | /**
300 | * \brief Parse RMC packet from buffer.
301 | * @param buff a constant character pointer of packet buffer.
302 | * @param buff_sz buffer size.
303 | * @param pack a pointer of packet which will filled by function.
304 | * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
305 | */
306 | int nmea_parse_GPRMC(const char *buff, int buff_sz, nmeaGPRMC *pack)
307 | {
308 | int nsen;
309 | char time_buff[NMEA_TIMEPARSE_BUF];
310 |
311 | NMEA_ASSERT(buff && pack);
312 |
313 | memset(pack, 0, sizeof(nmeaGPRMC));
314 |
315 | nmea_trace_buff(buff, buff_sz);
316 |
317 | nsen = nmea_scanf(buff, buff_sz,
318 | "$GPRMC,%s,%C,%f,%C,%f,%C,%f,%f,%2d%2d%2d,%f,%C,%C*",
319 | &(time_buff[0]),
320 | &(pack->status), &(pack->lat), &(pack->ns), &(pack->lon), &(pack->ew),
321 | &(pack->speed), &(pack->direction),
322 | &(pack->utc.day), &(pack->utc.mon), &(pack->utc.year),
323 | &(pack->declination), &(pack->declin_ew), &(pack->mode));
324 |
325 | if(nsen != 13 && nsen != 14)
326 | {
327 | nmea_error("GPRMC parse error!");
328 | return 0;
329 | }
330 |
331 | if(0 != _nmea_parse_time(&time_buff[0], (int)strlen(&time_buff[0]), &(pack->utc)))
332 | {
333 | nmea_error("GPRMC time parse error!");
334 | return 0;
335 | }
336 |
337 | if(pack->utc.year < 90)
338 | pack->utc.year += 100;
339 | pack->utc.mon -= 1;
340 |
341 | return 1;
342 | }
343 |
344 | /**
345 | * \brief Parse VTG packet from buffer.
346 | * @param buff a constant character pointer of packet buffer.
347 | * @param buff_sz buffer size.
348 | * @param pack a pointer of packet which will filled by function.
349 | * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
350 | */
351 | int nmea_parse_GPVTG(const char *buff, int buff_sz, nmeaGPVTG *pack)
352 | {
353 | NMEA_ASSERT(buff && pack);
354 |
355 | memset(pack, 0, sizeof(nmeaGPVTG));
356 |
357 | nmea_trace_buff(buff, buff_sz);
358 |
359 | if(8 != nmea_scanf(buff, buff_sz,
360 | "$GPVTG,%f,%C,%f,%C,%f,%C,%f,%C*",
361 | &(pack->dir), &(pack->dir_t),
362 | &(pack->dec), &(pack->dec_m),
363 | &(pack->spn), &(pack->spn_n),
364 | &(pack->spk), &(pack->spk_k)))
365 | {
366 | nmea_error("GPVTG parse error!");
367 | return 0;
368 | }
369 |
370 | if( pack->dir_t != 'T' ||
371 | pack->dec_m != 'M' ||
372 | pack->spn_n != 'N' ||
373 | pack->spk_k != 'K')
374 | {
375 | nmea_error("GPVTG parse error (format error)!");
376 | return 0;
377 | }
378 |
379 | return 1;
380 | }
381 |
382 | /**
383 | * \brief Fill nmeaINFO structure by GGA packet data.
384 | * @param pack a pointer of packet structure.
385 | * @param info a pointer of summary information structure.
386 | */
387 | void nmea_GPGGA2info(nmeaGPGGA *pack, nmeaINFO *info)
388 | {
389 | NMEA_ASSERT(pack && info);
390 |
391 | info->utc.hour = pack->utc.hour;
392 | info->utc.min = pack->utc.min;
393 | info->utc.sec = pack->utc.sec;
394 | info->utc.hsec = pack->utc.hsec;
395 | info->sig = pack->sig;
396 | info->HDOP = pack->HDOP;
397 | info->elv = pack->elv;
398 | info->lat = ((pack->ns == 'N')?pack->lat:-(pack->lat));
399 | info->lon = ((pack->ew == 'E')?pack->lon:-(pack->lon));
400 | info->smask |= GPGGA;
401 | }
402 |
403 | /**
404 | * \brief Fill nmeaINFO structure by GSA packet data.
405 | * @param pack a pointer of packet structure.
406 | * @param info a pointer of summary information structure.
407 | */
408 | void nmea_GPGSA2info(nmeaGPGSA *pack, nmeaINFO *info)
409 | {
410 | int i, j, nuse = 0;
411 |
412 | NMEA_ASSERT(pack && info);
413 |
414 | info->fix = pack->fix_type;
415 | info->PDOP = pack->PDOP;
416 | info->HDOP = pack->HDOP;
417 | info->VDOP = pack->VDOP;
418 |
419 | for(i = 0; i < NMEA_MAXSAT; ++i)
420 | {
421 | for(j = 0; j < info->satinfo.inview; ++j)
422 | {
423 | if(pack->sat_prn[i] && pack->sat_prn[i] == info->satinfo.sat[j].id)
424 | {
425 | info->satinfo.sat[j].in_use = 1;
426 | nuse++;
427 | }
428 | }
429 | }
430 |
431 | info->satinfo.inuse = nuse;
432 | info->smask |= GPGSA;
433 | }
434 |
435 | /**
436 | * \brief Fill nmeaINFO structure by GSV packet data.
437 | * @param pack a pointer of packet structure.
438 | * @param info a pointer of summary information structure.
439 | */
440 | void nmea_GPGSV2info(nmeaGPGSV *pack, nmeaINFO *info)
441 | {
442 | int isat, isi, nsat;
443 |
444 | NMEA_ASSERT(pack && info);
445 |
446 | if(pack->pack_index > pack->pack_count ||
447 | pack->pack_index * NMEA_SATINPACK > NMEA_MAXSAT)
448 | return;
449 |
450 | if(pack->pack_index < 1)
451 | pack->pack_index = 1;
452 |
453 | info->satinfo.inview = pack->sat_count;
454 |
455 | nsat = (pack->pack_index - 1) * NMEA_SATINPACK;
456 | nsat = (nsat + NMEA_SATINPACK > pack->sat_count)?pack->sat_count - nsat:NMEA_SATINPACK;
457 |
458 | for(isat = 0; isat < nsat; ++isat)
459 | {
460 | isi = (pack->pack_index - 1) * NMEA_SATINPACK + isat;
461 | info->satinfo.sat[isi].id = pack->sat_data[isat].id;
462 | info->satinfo.sat[isi].elv = pack->sat_data[isat].elv;
463 | info->satinfo.sat[isi].azimuth = pack->sat_data[isat].azimuth;
464 | info->satinfo.sat[isi].sig = pack->sat_data[isat].sig;
465 | }
466 |
467 | info->smask |= GPGSV;
468 | }
469 |
470 | /**
471 | * \brief Fill nmeaINFO structure by RMC packet data.
472 | * @param pack a pointer of packet structure.
473 | * @param info a pointer of summary information structure.
474 | */
475 | void nmea_GPRMC2info(nmeaGPRMC *pack, nmeaINFO *info)
476 | {
477 | NMEA_ASSERT(pack && info);
478 |
479 | if('A' == pack->status)
480 | {
481 | if(NMEA_SIG_BAD == info->sig)
482 | info->sig = NMEA_SIG_MID;
483 | if(NMEA_FIX_BAD == info->fix)
484 | info->fix = NMEA_FIX_2D;
485 | }
486 | else if('V' == pack->status)
487 | {
488 | info->sig = NMEA_SIG_BAD;
489 | info->fix = NMEA_FIX_BAD;
490 | }
491 |
492 | info->utc = pack->utc;
493 | info->lat = ((pack->ns == 'N')?pack->lat:-(pack->lat));
494 | info->lon = ((pack->ew == 'E')?pack->lon:-(pack->lon));
495 | info->speed = pack->speed * NMEA_TUD_KNOTS;
496 | info->direction = pack->direction;
497 | info->smask |= GPRMC;
498 | }
499 |
500 | /**
501 | * \brief Fill nmeaINFO structure by VTG packet data.
502 | * @param pack a pointer of packet structure.
503 | * @param info a pointer of summary information structure.
504 | */
505 | void nmea_GPVTG2info(nmeaGPVTG *pack, nmeaINFO *info)
506 | {
507 | NMEA_ASSERT(pack && info);
508 |
509 | info->direction = pack->dir;
510 | info->declination = pack->dec;
511 | info->speed = pack->spk;
512 | info->smask |= GPVTG;
513 | }
514 |
--------------------------------------------------------------------------------
/src/parser.c:
--------------------------------------------------------------------------------
1 | /*
2 | * This file is part of nmealib.
3 | *
4 | * Copyright (c) 2008 Timur Sinitsyn
5 | * Copyright (c) 2011 Ferry Huberts
6 | *
7 | * This library is free software; you can redistribute it and/or
8 | * modify it under the terms of the GNU Lesser General Public
9 | * License as published by the Free Software Foundation; either
10 | * version 2.1 of the License, or (at your option) any later version.
11 | *
12 | * This library is distributed in the hope that it will be useful,
13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 | * Lesser General Public License for more details.
16 | *
17 | * You should have received a copy of the GNU General Public License
18 | * along with this program. If not, see .
19 | */
20 |
21 | /**
22 | * \file parser.h
23 | */
24 |
25 | #include
26 |
27 | #include
28 | #include
29 | #include
30 |
31 | #include
32 | #include
33 | #include
34 | #include
35 |
36 | typedef struct _nmeaParserNODE
37 | {
38 | int packType;
39 | void *pack;
40 | struct _nmeaParserNODE *next_node;
41 |
42 | } nmeaParserNODE;
43 |
44 | /*
45 | * high level
46 | */
47 |
48 | /**
49 | * \brief Initialization of parser object
50 | * @return true (1) - success or false (0) - fail
51 | */
52 | int nmea_parser_init(nmeaPARSER *parser)
53 | {
54 | int resv = 0;
55 | int buff_size = nmea_property()->parse_buff_size;
56 |
57 | NMEA_ASSERT(parser);
58 |
59 | if(buff_size < NMEA_MIN_PARSEBUFF)
60 | buff_size = NMEA_MIN_PARSEBUFF;
61 |
62 | memset(parser, 0, sizeof(nmeaPARSER));
63 |
64 | if(0 == (parser->buffer = malloc(buff_size)))
65 | nmea_error("Insufficient memory!");
66 | else
67 | {
68 | parser->buff_size = buff_size;
69 | resv = 1;
70 | }
71 |
72 | return resv;
73 | }
74 |
75 | /**
76 | * \brief Destroy parser object
77 | */
78 | void nmea_parser_destroy(nmeaPARSER *parser)
79 | {
80 | NMEA_ASSERT(parser);
81 | if (parser->buffer) {
82 | free(parser->buffer);
83 | parser->buffer = NULL;
84 | }
85 | nmea_parser_queue_clear(parser);
86 | memset(parser, 0, sizeof(nmeaPARSER));
87 | }
88 |
89 | /**
90 | * \brief Analysis of buffer and put results to information structure
91 | * @return Number of packets wos parsed
92 | */
93 | int nmea_parse(
94 | nmeaPARSER *parser,
95 | const char *buff, int buff_sz,
96 | nmeaINFO *info
97 | )
98 | {
99 | int ptype, nread = 0;
100 | void *pack = 0;
101 |
102 | NMEA_ASSERT(parser && parser->buffer);
103 |
104 | nmea_parser_push(parser, buff, buff_sz);
105 |
106 | while(GPNON != (ptype = nmea_parser_pop(parser, &pack)))
107 | {
108 | nread++;
109 |
110 | switch(ptype)
111 | {
112 | case GPGGA:
113 | nmea_GPGGA2info((nmeaGPGGA *)pack, info);
114 | break;
115 | case GPGSA:
116 | nmea_GPGSA2info((nmeaGPGSA *)pack, info);
117 | break;
118 | case GPGSV:
119 | nmea_GPGSV2info((nmeaGPGSV *)pack, info);
120 | break;
121 | case GPRMC:
122 | nmea_GPRMC2info((nmeaGPRMC *)pack, info);
123 | break;
124 | case GPVTG:
125 | nmea_GPVTG2info((nmeaGPVTG *)pack, info);
126 | break;
127 | default:
128 | break;
129 | };
130 |
131 | free(pack);
132 | }
133 |
134 | return nread;
135 | }
136 |
137 | /*
138 | * low level
139 | */
140 |
141 | int nmea_parser_real_push(nmeaPARSER *parser, const char *buff, int buff_sz)
142 | {
143 | int nparsed = 0, crc, sen_sz, ptype;
144 | nmeaParserNODE *node = 0;
145 |
146 | NMEA_ASSERT(parser && parser->buffer);
147 |
148 | /* clear unuse buffer (for debug) */
149 | /*
150 | memset(
151 | parser->buffer + parser->buff_use, 0,
152 | parser->buff_size - parser->buff_use
153 | );
154 | */
155 |
156 | /* add */
157 | if(parser->buff_use + buff_sz >= parser->buff_size)
158 | nmea_parser_buff_clear(parser);
159 |
160 | memcpy(parser->buffer + parser->buff_use, buff, buff_sz);
161 | parser->buff_use += buff_sz;
162 |
163 | /* parse */
164 | for(;;node = 0)
165 | {
166 | sen_sz = nmea_find_tail(
167 | (const char *)parser->buffer + nparsed,
168 | (int)parser->buff_use - nparsed, &crc);
169 |
170 | if(!sen_sz)
171 | {
172 | if(nparsed)
173 | memcpy(
174 | parser->buffer,
175 | parser->buffer + nparsed,
176 | parser->buff_use -= nparsed);
177 | break;
178 | }
179 | else if(crc >= 0)
180 | {
181 | ptype = nmea_pack_type(
182 | (const char *)parser->buffer + nparsed + 1,
183 | parser->buff_use - nparsed - 1);
184 |
185 | if(0 == (node = malloc(sizeof(nmeaParserNODE))))
186 | goto mem_fail;
187 |
188 | node->pack = 0;
189 |
190 | switch(ptype)
191 | {
192 | case GPGGA:
193 | if(0 == (node->pack = malloc(sizeof(nmeaGPGGA))))
194 | goto mem_fail;
195 | node->packType = GPGGA;
196 | if(!nmea_parse_GPGGA(
197 | (const char *)parser->buffer + nparsed,
198 | sen_sz, (nmeaGPGGA *)node->pack))
199 | {
200 | free(node);
201 | node = 0;
202 | }
203 | break;
204 | case GPGSA:
205 | if(0 == (node->pack = malloc(sizeof(nmeaGPGSA))))
206 | goto mem_fail;
207 | node->packType = GPGSA;
208 | if(!nmea_parse_GPGSA(
209 | (const char *)parser->buffer + nparsed,
210 | sen_sz, (nmeaGPGSA *)node->pack))
211 | {
212 | free(node);
213 | node = 0;
214 | }
215 | break;
216 | case GPGSV:
217 | if(0 == (node->pack = malloc(sizeof(nmeaGPGSV))))
218 | goto mem_fail;
219 | node->packType = GPGSV;
220 | if(!nmea_parse_GPGSV(
221 | (const char *)parser->buffer + nparsed,
222 | sen_sz, (nmeaGPGSV *)node->pack))
223 | {
224 | free(node);
225 | node = 0;
226 | }
227 | break;
228 | case GPRMC:
229 | if(0 == (node->pack = malloc(sizeof(nmeaGPRMC))))
230 | goto mem_fail;
231 | node->packType = GPRMC;
232 | if(!nmea_parse_GPRMC(
233 | (const char *)parser->buffer + nparsed,
234 | sen_sz, (nmeaGPRMC *)node->pack))
235 | {
236 | free(node);
237 | node = 0;
238 | }
239 | break;
240 | case GPVTG:
241 | if(0 == (node->pack = malloc(sizeof(nmeaGPVTG))))
242 | goto mem_fail;
243 | node->packType = GPVTG;
244 | if(!nmea_parse_GPVTG(
245 | (const char *)parser->buffer + nparsed,
246 | sen_sz, (nmeaGPVTG *)node->pack))
247 | {
248 | free(node);
249 | node = 0;
250 | }
251 | break;
252 | default:
253 | free(node);
254 | node = 0;
255 | break;
256 | };
257 |
258 | if(node)
259 | {
260 | if(parser->end_node)
261 | ((nmeaParserNODE *)parser->end_node)->next_node = node;
262 | parser->end_node = node;
263 | if(!parser->top_node)
264 | parser->top_node = node;
265 | node->next_node = 0;
266 | }
267 | }
268 |
269 | nparsed += sen_sz;
270 | }
271 |
272 | return nparsed;
273 |
274 | mem_fail:
275 | if(node)
276 | free(node);
277 |
278 | nmea_error("Insufficient memory!");
279 |
280 | return -1;
281 | }
282 |
283 | /**
284 | * \brief Analysis of buffer and keep results into parser
285 | * @return Number of bytes wos parsed from buffer
286 | */
287 | int nmea_parser_push(nmeaPARSER *parser, const char *buff, int buff_sz)
288 | {
289 | int nparse, nparsed = 0;
290 |
291 | do
292 | {
293 | if(buff_sz > parser->buff_size)
294 | nparse = parser->buff_size;
295 | else
296 | nparse = buff_sz;
297 |
298 | nparsed += nmea_parser_real_push(
299 | parser, buff, nparse);
300 |
301 | buff_sz -= nparse;
302 |
303 | } while(buff_sz);
304 |
305 | return nparsed;
306 | }
307 |
308 | /**
309 | * \brief Get type of top packet keeped into parser
310 | * @return Type of packet
311 | * @see nmeaPACKTYPE
312 | */
313 | int nmea_parser_top(nmeaPARSER *parser)
314 | {
315 | int retval = GPNON;
316 | nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node;
317 |
318 | NMEA_ASSERT(parser && parser->buffer);
319 |
320 | if(node)
321 | retval = node->packType;
322 |
323 | return retval;
324 | }
325 |
326 | /**
327 | * \brief Withdraw top packet from parser
328 | * @return Received packet type
329 | * @see nmeaPACKTYPE
330 | */
331 | int nmea_parser_pop(nmeaPARSER *parser, void **pack_ptr)
332 | {
333 | int retval = GPNON;
334 | nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node;
335 |
336 | NMEA_ASSERT(parser && parser->buffer);
337 |
338 | if(node)
339 | {
340 | *pack_ptr = node->pack;
341 | retval = node->packType;
342 | parser->top_node = node->next_node;
343 | if(!parser->top_node)
344 | parser->end_node = 0;
345 | free(node);
346 | }
347 |
348 | return retval;
349 | }
350 |
351 | /**
352 | * \brief Get top packet from parser without withdraw
353 | * @return Received packet type
354 | * @see nmeaPACKTYPE
355 | */
356 | int nmea_parser_peek(nmeaPARSER *parser, void **pack_ptr)
357 | {
358 | int retval = GPNON;
359 | nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node;
360 |
361 | NMEA_ASSERT(parser && parser->buffer);
362 |
363 | if(node)
364 | {
365 | *pack_ptr = node->pack;
366 | retval = node->packType;
367 | }
368 |
369 | return retval;
370 | }
371 |
372 | /**
373 | * \brief Delete top packet from parser
374 | * @return Deleted packet type
375 | * @see nmeaPACKTYPE
376 | */
377 | int nmea_parser_drop(nmeaPARSER *parser)
378 | {
379 | int retval = GPNON;
380 | nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node;
381 |
382 | NMEA_ASSERT(parser && parser->buffer);
383 |
384 | if(node)
385 | {
386 | if(node->pack)
387 | free(node->pack);
388 | retval = node->packType;
389 | parser->top_node = node->next_node;
390 | if(!parser->top_node)
391 | parser->end_node = 0;
392 | free(node);
393 | }
394 |
395 | return retval;
396 | }
397 |
398 | /**
399 | * \brief Clear cache of parser
400 | * @return true (1) - success
401 | */
402 | int nmea_parser_buff_clear(nmeaPARSER *parser)
403 | {
404 | NMEA_ASSERT(parser && parser->buffer);
405 | parser->buff_use = 0;
406 | return 1;
407 | }
408 |
409 | /**
410 | * \brief Clear packets queue into parser
411 | * @return true (1) - success
412 | */
413 | int nmea_parser_queue_clear(nmeaPARSER *parser)
414 | {
415 | NMEA_ASSERT(parser);
416 | while(parser->top_node)
417 | nmea_parser_drop(parser);
418 | return 1;
419 | }
420 |
--------------------------------------------------------------------------------
/src/sentence.c:
--------------------------------------------------------------------------------
1 | /*
2 | * This file is part of nmealib.
3 | *
4 | * Copyright (c) 2008 Timur Sinitsyn
5 | * Copyright (c) 2011 Ferry Huberts
6 | *
7 | * This library is free software; you can redistribute it and/or
8 | * modify it under the terms of the GNU Lesser General Public
9 | * License as published by the Free Software Foundation; either
10 | * version 2.1 of the License, or (at your option) any later version.
11 | *
12 | * This library is distributed in the hope that it will be useful,
13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 | * Lesser General Public License for more details.
16 | *
17 | * You should have received a copy of the GNU General Public License
18 | * along with this program. If not, see .
19 | */
20 |
21 | #include
22 |
23 | #include
24 |
25 | void nmea_zero_GPGGA(nmeaGPGGA *pack)
26 | {
27 | memset(pack, 0, sizeof(nmeaGPGGA));
28 | nmea_time_now(&pack->utc);
29 | pack->ns = 'N';
30 | pack->ew = 'E';
31 | pack->elv_units = 'M';
32 | pack->diff_units = 'M';
33 | }
34 |
35 | void nmea_zero_GPGSA(nmeaGPGSA *pack)
36 | {
37 | memset(pack, 0, sizeof(nmeaGPGSA));
38 | pack->fix_mode = 'A';
39 | pack->fix_type = NMEA_FIX_BAD;
40 | }
41 |
42 | void nmea_zero_GPGSV(nmeaGPGSV *pack)
43 | {
44 | memset(pack, 0, sizeof(nmeaGPGSV));
45 | }
46 |
47 | void nmea_zero_GPRMC(nmeaGPRMC *pack)
48 | {
49 | memset(pack, 0, sizeof(nmeaGPRMC));
50 | nmea_time_now(&pack->utc);
51 | pack->status = 'V';
52 | pack->ns = 'N';
53 | pack->ew = 'E';
54 | pack->declin_ew = 'E';
55 | }
56 |
57 | void nmea_zero_GPVTG(nmeaGPVTG *pack)
58 | {
59 | memset(pack, 0, sizeof(nmeaGPVTG));
60 | pack->dir_t = 'T';
61 | pack->dec_m = 'M';
62 | pack->spn_n = 'N';
63 | pack->spk_k = 'K';
64 | }
65 |
--------------------------------------------------------------------------------
/src/time.c:
--------------------------------------------------------------------------------
1 | /*
2 | * This file is part of nmealib.
3 | *
4 | * Copyright (c) 2008 Timur Sinitsyn
5 | * Copyright (c) 2011 Ferry Huberts
6 | *
7 | * This library is free software; you can redistribute it and/or
8 | * modify it under the terms of the GNU Lesser General Public
9 | * License as published by the Free Software Foundation; either
10 | * version 2.1 of the License, or (at your option) any later version.
11 | *
12 | * This library is distributed in the hope that it will be useful,
13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 | * Lesser General Public License for more details.
16 | *
17 | * You should have received a copy of the GNU General Public License
18 | * along with this program. If not, see .
19 | */
20 |
21 | #include
22 |
23 | #include
24 | #include
25 | #include
26 |
27 | void nmea_time_now(nmeaTIME *stm) {
28 | struct timeval tp;
29 | struct tm tt;
30 |
31 | gettimeofday(&tp, NULL);
32 | gmtime_r(&tp.tv_sec, &tt);
33 |
34 | stm->year = tt.tm_year;
35 | stm->mon = tt.tm_mon;
36 | stm->day = tt.tm_mday;
37 | stm->hour = tt.tm_hour;
38 | stm->min = tt.tm_min;
39 | stm->sec = tt.tm_sec;
40 | stm->hsec = (tp.tv_usec / 10000);
41 | }
42 |
--------------------------------------------------------------------------------
/src/tok.c:
--------------------------------------------------------------------------------
1 | /*
2 | * This file is part of nmealib.
3 | *
4 | * Copyright (c) 2008 Timur Sinitsyn
5 | * Copyright (c) 2011 Ferry Huberts
6 | *
7 | * This library is free software; you can redistribute it and/or
8 | * modify it under the terms of the GNU Lesser General Public
9 | * License as published by the Free Software Foundation; either
10 | * version 2.1 of the License, or (at your option) any later version.
11 | *
12 | * This library is distributed in the hope that it will be useful,
13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 | * Lesser General Public License for more details.
16 | *
17 | * You should have received a copy of the GNU General Public License
18 | * along with this program. If not, see .
19 | */
20 |
21 | /*! \file tok.h */
22 |
23 | #include
24 |
25 | #include
26 | #include
27 | #include
28 | #include
29 | #include
30 |
31 | #include
32 |
33 | #define NMEA_TOKS_COMPARE (1)
34 | #define NMEA_TOKS_PERCENT (2)
35 | #define NMEA_TOKS_WIDTH (3)
36 | #define NMEA_TOKS_TYPE (4)
37 |
38 | #define NMEA_CONVSTR_BUF (256)
39 |
40 | /**
41 | * \brief Calculate control sum of binary buffer
42 | */
43 | int nmea_calc_crc(const char *buff, int buff_sz)
44 | {
45 | int chsum = 0,
46 | it;
47 |
48 | for(it = 0; it < buff_sz; ++it)
49 | chsum ^= (int)buff[it];
50 |
51 | return chsum;
52 | }
53 |
54 | /**
55 | * \brief Convert string to number
56 | */
57 | int nmea_atoi(const char *str, int str_sz, int radix)
58 | {
59 | char *tmp_ptr;
60 | char buff[NMEA_CONVSTR_BUF];
61 | int res = 0;
62 |
63 | if(str_sz < NMEA_CONVSTR_BUF)
64 | {
65 | memcpy(&buff[0], str, str_sz);
66 | buff[str_sz] = '\0';
67 | res = strtol(&buff[0], &tmp_ptr, radix);
68 | }
69 |
70 | return res;
71 | }
72 |
73 | /**
74 | * \brief Convert string to fraction number
75 | */
76 | double nmea_atof(const char *str, int str_sz)
77 | {
78 | char *tmp_ptr;
79 | char buff[NMEA_CONVSTR_BUF];
80 | double res = 0;
81 |
82 | if(str_sz < NMEA_CONVSTR_BUF)
83 | {
84 | memcpy(&buff[0], str, str_sz);
85 | buff[str_sz] = '\0';
86 | res = strtod(&buff[0], &tmp_ptr);
87 | }
88 |
89 | return res;
90 | }
91 |
92 | /**
93 | * \brief Formating string (like standart printf) with CRC tail (*CRC)
94 | */
95 | int nmea_printf(char *buff, int buff_sz, const char *format, ...)
96 | {
97 | int retval, add = 0;
98 | va_list arg_ptr;
99 |
100 | if(buff_sz <= 0)
101 | return 0;
102 |
103 | va_start(arg_ptr, format);
104 |
105 | retval = NMEA_POSIX(vsnprintf)(buff, buff_sz, format, arg_ptr);
106 |
107 | if(retval > 0)
108 | {
109 | add = NMEA_POSIX(snprintf)(
110 | buff + retval, buff_sz - retval, "*%02x\r\n",
111 | nmea_calc_crc(buff + 1, retval - 1));
112 | }
113 |
114 | retval += add;
115 |
116 | if(retval < 0 || retval > buff_sz)
117 | {
118 | memset(buff, ' ', buff_sz);
119 | retval = buff_sz;
120 | }
121 |
122 | va_end(arg_ptr);
123 |
124 | return retval;
125 | }
126 |
127 | /**
128 | * \brief Analyse string (specificate for NMEA sentences)
129 | */
130 | int nmea_scanf(const char *buff, int buff_sz, const char *format, ...)
131 | {
132 | const char *beg_tok;
133 | const char *end_buf = buff + buff_sz;
134 |
135 | va_list arg_ptr;
136 | int tok_type = NMEA_TOKS_COMPARE;
137 | int width = 0;
138 | const char *beg_fmt = 0;
139 | int snum = 0, unum = 0;
140 |
141 | int tok_count = 0;
142 | void *parg_target;
143 |
144 | va_start(arg_ptr, format);
145 |
146 | for(; *format && buff < end_buf; ++format)
147 | {
148 | switch(tok_type)
149 | {
150 | case NMEA_TOKS_COMPARE:
151 | if('%' == *format)
152 | tok_type = NMEA_TOKS_PERCENT;
153 | else if(*buff++ != *format)
154 | goto fail;
155 | break;
156 | case NMEA_TOKS_PERCENT:
157 | width = 0;
158 | beg_fmt = format;
159 | tok_type = NMEA_TOKS_WIDTH;
160 | /* no break */
161 | case NMEA_TOKS_WIDTH:
162 | if(isdigit(*format))
163 | break;
164 | {
165 | tok_type = NMEA_TOKS_TYPE;
166 | if(format > beg_fmt)
167 | width = nmea_atoi(beg_fmt, (int)(format - beg_fmt), 10);
168 | }
169 | /* no break */
170 | case NMEA_TOKS_TYPE:
171 | beg_tok = buff;
172 |
173 | if(!width && ('c' == *format || 'C' == *format) && *buff != format[1])
174 | width = 1;
175 |
176 | if(width)
177 | {
178 | if(buff + width <= end_buf)
179 | buff += width;
180 | else
181 | goto fail;
182 | }
183 | else
184 | {
185 | if(!format[1] || (0 == (buff = (char *)memchr(buff, format[1], end_buf - buff))))
186 | buff = end_buf;
187 | }
188 |
189 | if(buff > end_buf)
190 | goto fail;
191 |
192 | tok_type = NMEA_TOKS_COMPARE;
193 | tok_count++;
194 |
195 | parg_target = 0; width = (int)(buff - beg_tok);
196 |
197 | switch(*format)
198 | {
199 | case 'c':
200 | case 'C':
201 | parg_target = (void *)va_arg(arg_ptr, char *);
202 | if(width && 0 != (parg_target))
203 | *((char *)parg_target) = *beg_tok;
204 | break;
205 | case 's':
206 | case 'S':
207 | parg_target = (void *)va_arg(arg_ptr, char *);
208 | if(width && 0 != (parg_target))
209 | {
210 | memcpy(parg_target, beg_tok, width);
211 | ((char *)parg_target)[width] = '\0';
212 | }
213 | break;
214 | case 'f':
215 | case 'g':
216 | case 'G':
217 | case 'e':
218 | case 'E':
219 | parg_target = (void *)va_arg(arg_ptr, double *);
220 | if(width && 0 != (parg_target))
221 | *((double *)parg_target) = nmea_atof(beg_tok, width);
222 | break;
223 | default:
224 | break;
225 | };
226 |
227 | if(parg_target)
228 | break;
229 | if(0 == (parg_target = (void *)va_arg(arg_ptr, int *)))
230 | break;
231 | if(!width)
232 | break;
233 |
234 | switch(*format)
235 | {
236 | case 'd':
237 | case 'i':
238 | snum = nmea_atoi(beg_tok, width, 10);
239 | memcpy(parg_target, &snum, sizeof(int));
240 | break;
241 | case 'u':
242 | unum = nmea_atoi(beg_tok, width, 10);
243 | memcpy(parg_target, &unum, sizeof(unsigned int));
244 | break;
245 | case 'x':
246 | case 'X':
247 | unum = nmea_atoi(beg_tok, width, 16);
248 | memcpy(parg_target, &unum, sizeof(unsigned int));
249 | break;
250 | case 'o':
251 | unum = nmea_atoi(beg_tok, width, 8);
252 | memcpy(parg_target, &unum, sizeof(unsigned int));
253 | break;
254 | default:
255 | goto fail;
256 | };
257 |
258 | break;
259 |
260 | default:
261 | break;
262 | };
263 | }
264 |
265 | fail:
266 |
267 | va_end(arg_ptr);
268 |
269 | return tok_count;
270 | }
271 |
--------------------------------------------------------------------------------
/src/util.c:
--------------------------------------------------------------------------------
1 | /*
2 | * This file is part of nmealib.
3 | *
4 | * Copyright (c) 2011 Ferry Huberts
5 | *
6 | * This library is free software; you can redistribute it and/or
7 | * modify it under the terms of the GNU Lesser General Public
8 | * License as published by the Free Software Foundation; either
9 | * version 2.1 of the License, or (at your option) any later version.
10 | *
11 | * This library is distributed in the hope that it will be useful,
12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 | * Lesser General Public License for more details.
15 | *
16 | * You should have received a copy of the GNU General Public License
17 | * along with this program. If not, see .
18 | */
19 |
20 | #include
21 |
22 | #include
23 | #include
24 |
25 | /**
26 | * Determine whether the given string contains characters that are not allowed
27 | * for fields in an NMEA string.
28 | *
29 | * @param str
30 | * The string to check
31 | * @param strName
32 | * The name of the string to report when invalid characters are encountered
33 | * @param report
34 | * A pointer to a buffer in which to place the report string when an invalid
35 | * nmea character is detected
36 | * @param reportSize
37 | * The size of the report buffer
38 | *
39 | * @return
40 | * - true when the string has invalid characters
41 | * - false otherwise
42 | */
43 | bool nmea_string_has_invalid_chars(const char * str, const char * strName,
44 | char * report, size_t reportSize) {
45 | static const char invalidChars[] = { '$', '*', ',', '!', '\\', '^', '~' };
46 | static const char * invalidCharsNames[] = { "sentence delimiter ($)",
47 | "checksum field delimiter (*)", "comma (,)", "exclamation mark (!)",
48 | "backslash (\\)", "^ (^)", "tilde (~)" };
49 |
50 | size_t i;
51 | size_t j;
52 |
53 | if (!str) {
54 | return false;
55 | }
56 |
57 | for (i = 0; i < strlen(str); i++) {
58 | char c = str[i];
59 |
60 | if ((c < 32) || (c > 126)) {
61 | if (report) {
62 | snprintf((char*) report, reportSize, "Configured %s (%s),"
63 | " character %lu, can not contain non-printable"
64 | " characters (codes outside the range [32, 126])",
65 | strName, str, (unsigned long)i + 1);
66 | report[reportSize - 1] = '\0';
67 | }
68 | return true;
69 | }
70 |
71 | for (j = 0; j < sizeof(invalidChars); j++) {
72 | if (c == invalidChars[j]) {
73 | if (report) {
74 | snprintf((char *) report, reportSize, "Configured %s (%s),"
75 | " character %lu, can not contain %s characters",
76 | strName, str, (unsigned long)i + 1, invalidCharsNames[j]);
77 | report[reportSize - 1] = '\0';
78 | }
79 | return true;
80 | }
81 | }
82 | }
83 |
84 | return false;
85 | }
86 |
--------------------------------------------------------------------------------