• source navigation  • diff markup  • identifier search  • freetext search  • 

Sources/ugps/nmea.c

  1 /*
  2  *   This program is free software; you can redistribute it and/or modify
  3  *   it under the terms of the GNU General Public License as published by
  4  *   the Free Software Foundation; either version 2 of the License, or
  5  *   (at your option) any later version.
  6  *
  7  *   This program is distributed in the hope that it will be useful,
  8  *   but WITHOUT ANY WARRANTY; without even the implied warranty of
  9  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 10  *   GNU General Public License for more details.
 11  *
 12  *   You should have received a copy of the GNU General Public License
 13  *   along with this program; if not, write to the Free Software
 14  *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307, USA.
 15  *
 16  *   Copyright (C) 2014 John Crispin <blogic@openwrt.org> 
 17  */
 18 
 19 #define _DEFAULT_SOURCE
 20 #define _XOPEN_SOURCE
 21 #define _BSD_SOURCE
 22 #include <time.h>
 23 
 24 #include <sys/types.h>
 25 #include <sys/stat.h>
 26 #include <sys/ioctl.h>
 27 #include <sys/time.h>
 28 
 29 #include <fcntl.h>
 30 #include <time.h>
 31 #include <stdlib.h>
 32 #include <stdio.h>
 33 #include <unistd.h>
 34 #include <errno.h>
 35 #include <math.h>
 36 
 37 #include <string.h>
 38 #include <termios.h>
 39 
 40 #include <libubox/utils.h>
 41 
 42 #include "log.h"
 43 #include "nmea.h"
 44 
 45 #define MAX_NMEA_PARAM  20
 46 #define MAX_TIME_OFFSET 5
 47 #define MAX_BAD_TIME    3
 48 
 49 struct nmea_param {
 50         char *str;
 51         int num;
 52 } nmea_params[MAX_NMEA_PARAM];
 53 
 54 static int nmea_bad_time;
 55 char longitude[33] = { 0 }, latitude[33] = { 0 }, course[17] = { 0 }, speed[17] = { 0 }, elevation[17] = { 0 };
 56 int gps_valid = 0;
 57 char gps_fields = 0;
 58 
 59 static void
 60 nmea_txt_cb(void)
 61 {
 62         char *ids[] = { "ERROR", "WARNING", "NOTICE", };
 63 
 64         if (nmea_params[3].num < 0 || nmea_params[3].num > 2)
 65                 nmea_params[3].num = 0;
 66 
 67         DEBUG(3, "%s: %s\n", ids[nmea_params[3].num], nmea_params[4].str);
 68 }
 69 
 70 static void
 71 do_adjust_clock(struct tm *tm)
 72 {
 73         char tmp[256];
 74 
 75         strftime(tmp, 256, "%Y-%m-%dT%H:%M:%S", tm);
 76         DEBUG(3, "date: %s UTC\n", tmp);
 77 
 78         if (adjust_clock) {
 79                 time_t sec = timegm(tm);
 80                 struct timeval cur;
 81 
 82                 gettimeofday(&cur, NULL);
 83 
 84                 if ((sec < 0) || (llabs(cur.tv_sec - sec) > MAX_TIME_OFFSET)) {
 85                         struct timeval tv = { 0 };
 86                         tv.tv_sec = sec;
 87                         if (++nmea_bad_time > MAX_BAD_TIME) {
 88                                 LOG("system time differs from GPS time by more than %d seconds. Using %s UTC as the new time\n", MAX_TIME_OFFSET, tmp);
 89                                 /* only set datetime if specified by command line argument! */
 90                                 settimeofday(&tv, NULL);
 91                         }
 92                 } else {
 93                         nmea_bad_time = 0;
 94                 }
 95         }
 96 }
 97 
 98 static void
 99 parse_gps_coords(char *latstr, char *vhem, char *lonstr, char *hhem)
100 {
101         float minutes;
102         float degrees;
103         float lat = strtof(latstr, NULL);
104         float lon = strtof(lonstr, NULL);
105 
106         degrees = floor(lat / 100.0);
107         minutes = lat - (degrees * 100.0);
108         lat = degrees + minutes / 60.0;
109 
110         degrees = floor(lon / 100.0);
111         minutes = lon - (degrees * 100.0);
112         lon = degrees + minutes / 60.0;
113 
114         if (*vhem == 'S')
115                 lat *= -1.0;
116         if (*hhem == 'W')
117                 lon *= -1.0;
118 
119         snprintf(latitude, sizeof(latitude), "%f", lat);
120         snprintf(longitude, sizeof(longitude), "%f", lon);
121 
122         DEBUG(3, "position: %s %s\n", latitude, longitude);
123         gps_fields |= GPS_FIELD_LAT | GPS_FIELD_LON;
124 
125         gps_timestamp();
126 }
127 
128 static void
129 nmea_rmc_cb(void)
130 {
131         struct tm tm;
132 
133         if (*nmea_params[2].str != 'A') {
134                 gps_valid = 0;
135                 DEBUG(4, "waiting for valid signal\n");
136                 return;
137         }
138 
139         gps_valid = 1;
140         memset(&tm, 0, sizeof(tm));
141         tm.tm_isdst = 1;
142 
143         if (sscanf(nmea_params[1].str, "%02d%02d%02d",
144                 &tm.tm_hour, &tm.tm_min, &tm.tm_sec) != 3) {
145                 ERROR("failed to parse time '%s'\n", nmea_params[1].str);
146         }
147         else if (sscanf(nmea_params[9].str, "%02d%02d%02d",
148                 &tm.tm_mday, &tm.tm_mon, &tm.tm_year) != 3) {
149                 ERROR("failed to parse date '%s'\n", nmea_params[9].str);
150         }
151         else if (tm.tm_year == 0) {
152                 DEBUG(4, "waiting for valid date\n");
153                 return;
154         }
155         else {
156                 tm.tm_year += 100; /* year starts with 1900 */
157                 tm.tm_mon -= 1; /* month starts with 0 */
158 
159                 do_adjust_clock(&tm);
160         }
161 
162         if (strlen(nmea_params[3].str) < 9 || strlen(nmea_params[5].str) < 10) {
163                 ERROR("lat/lng have invalid string length %zu<9, %zu<10\n",
164                        strlen(nmea_params[3].str), strlen(nmea_params[5].str));
165         } else {
166                 parse_gps_coords(nmea_params[3].str, nmea_params[4].str, nmea_params[5].str, nmea_params[6].str);
167         }
168 }
169 
170 static void
171 nmea_zda_cb(void)
172 {
173         struct tm tm;
174 
175         if (!gps_valid)
176                 return;
177 
178         memset(&tm, 0, sizeof(tm));
179         tm.tm_isdst = 1;
180 
181         if (sscanf(nmea_params[1].str, "%02d%02d%02d",
182                 &tm.tm_hour, &tm.tm_min, &tm.tm_sec) != 3) {
183                 ERROR("failed to parse time '%s'\n", nmea_params[1].str);
184                 return;
185         }
186 
187         if ((sscanf(nmea_params[2].str, "%02d", &tm.tm_mday) != 1) ||
188             (sscanf(nmea_params[3].str, "%02d", &tm.tm_mon) != 1) ||
189             (sscanf(nmea_params[4].str, "%04d", &tm.tm_year) != 1)) {
190                 ERROR("failed to parse time '%s,%s,%s'\n",
191                         nmea_params[2].str, nmea_params[3].str, nmea_params[4].str);
192                 return;
193         }
194 
195         if (tm.tm_year == 0) {
196                 DEBUG(4, "waiting for valid date\n");
197                 return;
198         }
199 
200         tm.tm_mon -= 1; /* month starts with 0 */
201         tm.tm_year -= 1900; /* full 4-digit year, tm expects years till 1900 */
202 
203         do_adjust_clock(&tm);
204 }
205 
206 static void
207 nmea_gll_cb(void)
208 {
209         if (*nmea_params[6].str != 'A') {
210                 gps_valid = 0;
211                 DEBUG(4, "waiting for valid signal\n");
212                 return;
213         }
214 
215         gps_valid = 1;
216 
217         parse_gps_coords(nmea_params[1].str, nmea_params[2].str, nmea_params[3].str, nmea_params[4].str);
218 }
219 
220 static void
221 nmea_gga_cb(void)
222 {
223         if (!gps_valid)
224                 return;
225         strncpy(elevation, nmea_params[9].str, sizeof(elevation));
226         gps_fields |= GPS_FIELD_ALT;
227         DEBUG(4, "height: %s\n", elevation);
228 }
229 
230 static void
231 nmea_vtg_cb(void)
232 {
233         if (!gps_valid)
234                 return;
235         strncpy(course, nmea_params[1].str, sizeof(course));
236         strncpy(speed, nmea_params[7].str, sizeof(speed));
237         gps_fields |= GPS_FIELD_COG | GPS_FIELD_SPD;
238         DEBUG(4, "course: %s\n", course);
239         DEBUG(4, "speed: %s\n", speed);
240 }
241 
242 static struct nmea_msg {
243         char *msg;
244         int cnt;
245         void (*handler) (void);
246 } nmea_msgs[] = {
247         {
248                 .msg = "TXT",
249                 .cnt = 5,
250                 .handler = nmea_txt_cb,
251         }, {
252                 .msg = "RMC",
253                 .cnt = 11,
254                 .handler = nmea_rmc_cb,
255         }, {
256                 .msg = "GGA",
257                 .cnt = 14,
258                 .handler = nmea_gga_cb,
259         }, {
260                 .msg = "GLL",
261                 .cnt = 7,
262                 .handler = nmea_gll_cb,
263         }, {
264                 .msg = "VTG",
265                 .cnt = 9,
266                 .handler = nmea_vtg_cb,
267         }, {
268                 .msg = "ZDA",
269                 .cnt = 5,
270                 .handler = nmea_zda_cb,
271         },
272 };
273 
274 static int
275 nmea_verify_checksum(char *s)
276 {
277         char *csum = strrchr(s, '*');
278         int isum, c = 0;
279 
280         if (!csum)
281                 return -1;
282 
283         *csum = '\0';
284         csum++;
285         isum = strtol(csum, NULL, 16);
286 
287         while(*s)
288                 c ^= *s++;
289 
290         if (isum != c)
291                 return -1;
292 
293         return 0;
294 }
295 
296 static int
297 nmea_tokenize(char *msg)
298 {
299         int cnt = 0;
300         char *tok = strsep(&msg, ",");
301 
302         while (tok && cnt < MAX_NMEA_PARAM) {
303                 nmea_params[cnt].str = tok;
304                 nmea_params[cnt].num = atoi(tok);
305                 cnt++;
306                 tok = strsep(&msg, ",");
307         }
308 
309         return cnt;
310 }
311 
312 static void
313 nmea_process(char *a)
314 {
315         char *csum;
316         int cnt;
317         unsigned int i;
318 
319         if (strncmp(a, "$GP", 3) &&
320             strncmp(a, "$GN", 3))
321                 return;
322 
323         a++;
324         csum = strrchr(a, '*');
325         if (!csum)
326                 return;
327 
328         if (nmea_verify_checksum(a)) {
329                 ERROR("nmea message has invalid checksum\n");
330                 return;
331         }
332 
333         cnt = nmea_tokenize(&a[2]);
334         if (cnt < 0) {
335                 ERROR("failed to tokenize %s\n", a);\
336                 return;
337         }
338 
339         for (i = 0; i < ARRAY_SIZE(nmea_msgs); i++) {
340                 if (strcmp(nmea_params[0].str, nmea_msgs[i].msg) &&
341                     strcmp(nmea_params[3].str, nmea_msgs[i].msg))
342                         continue;
343                 if (nmea_msgs[i].cnt <= cnt)
344                         nmea_msgs[i].handler();
345                 else
346                         ERROR("%s datagram has wrong parameter count got %d but expected %d\n", nmea_msgs[i].msg, cnt, nmea_msgs[i].cnt);
347                 return;
348         }
349 }
350 
351 static int
352 nmea_consume(struct ustream *s, char **a)
353 {
354         char *eol = strstr(*a, "\n");
355 
356         if (!eol)
357                 return -1;
358 
359         *eol++ = '\0';
360 
361         nmea_process(*a);
362 
363         ustream_consume(s, eol - *a);
364         *a = eol;
365 
366         return 0;
367 }
368 
369 static void
370 nmea_msg_cb(struct ustream *s, int bytes)
371 {
372         int len;
373         char *a = ustream_get_read_buf(s, &len);
374 
375         while (!nmea_consume(s, &a))
376                 ;
377 }
378 
379 static void nmea_notify_cb(struct ustream *s)
380 {
381         if (!s->eof)
382                 return;
383 
384         ERROR("tty error, shutting down\n");
385         exit(-1);
386 }
387 
388 int
389 nmea_open(char *dev, struct ustream_fd *s, speed_t speed)
390 {
391         struct termios tio;
392         int tty;
393 
394         tty = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
395         if (tty < 0) {
396                 ERROR("%s: device open failed: %s\n", dev, strerror(errno));
397                 return -1;
398         }
399 
400         tcgetattr(tty, &tio);
401         tio.c_cflag |= CREAD;
402         tio.c_cflag |= CS8;
403         tio.c_iflag |= IGNPAR;
404         tio.c_lflag &= ~(ICANON);
405         tio.c_lflag &= ~(ECHO);
406         tio.c_lflag &= ~(ECHOE);
407         tio.c_lflag &= ~(ISIG);
408         tio.c_cc[VMIN] = 1;
409         tio.c_cc[VTIME] = 0;
410         cfsetispeed(&tio, speed);
411         cfsetospeed(&tio, speed);
412         tcsetattr(tty, TCSANOW, &tio);
413 
414         s->stream.string_data = true;
415         s->stream.notify_read = nmea_msg_cb;
416         s->stream.notify_state = nmea_notify_cb;
417 
418         ustream_fd_init(s, tty);
419 
420         tcflush(tty, TCIFLUSH);
421 
422         return 0;
423 }
424 

This page was automatically generated by LXR 0.3.1.  •  OpenWrt