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.
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.
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.
16 * Copyright (C) 2014 John Crispin <blogic@openwrt.org>
23 #include <sys/types.h>
25 #include <sys/ioctl.h>
38 #include <libubox/utils.h>
43 #define MAX_NMEA_PARAM 20
44 #define MAX_TIME_OFFSET 2
45 #define MAX_BAD_TIME 3
50 } nmea_params[MAX_NMEA_PARAM];
52 static int nmea_bad_time;
53 char longitude[32] = { 0 }, latitude[32] = { 0 }, course[16] = { 0 }, speed[16] = { 0 }, elivation[16] = { 0 };
59 char *ids[] = { "ERROR", "WARNING", "NOTICE", };
61 if (nmea_params[3].num < 0 || nmea_params[3].num > 2)
62 nmea_params[3].num = 0;
64 DEBUG(3, "%s: %s\n", ids[nmea_params[3].num], nmea_params[4].str);
73 if (*nmea_params[2].str != 'A') {
75 DEBUG(4, "waiting for valid signal\n");
80 memset(&tm, 0, sizeof(tm));
83 if (!strptime(nmea_params[1].str, "%H%M%S", &tm))
84 ERROR("failed to parse time\n");
85 else if (!strptime(nmea_params[9].str, "%d%m%y", &tm))
86 ERROR("failed to parse date\n");
88 /* is there a libc api for the tz adjustment ? */
89 struct timeval tv = { mktime(&tm), 0 };
92 strftime(tmp, 256, "%D %02H:%02M:%02S", &tm);
93 DEBUG(3, "date: %s UTC\n", tmp);
95 tv.tv_sec -= timezone;
99 gettimeofday(&cur, NULL);
101 if (abs(cur.tv_sec - tv.tv_sec) > MAX_TIME_OFFSET) {
102 if (++nmea_bad_time > MAX_BAD_TIME) {
103 LOG("system time differs from GPS time by more than %d seconds. Using %s UTC as the new time\n", MAX_TIME_OFFSET, tmp);
104 settimeofday(&tv, NULL);
111 if (strlen(nmea_params[3].str) != 9 || strlen(nmea_params[5].str) != 10) {
112 ERROR("lat/lng have invalid string length\n");
114 int latd, latm, lats;
115 int lngd, lngm, lngs;
117 DEBUG(4, "position: %s, %s\n",
118 nmea_params[3].str, nmea_params[5].str);
119 latm = atoi(&nmea_params[3].str[2]);
120 nmea_params[3].str[2] = '\0';
121 latd = atoi(nmea_params[3].str);
122 lats = atoi(&nmea_params[3].str[5]);
123 if (*nmea_params[4].str != 'N')
126 lngm = atoi(&nmea_params[5].str[3]);
127 nmea_params[5].str[3] = '\0';
128 lngd = atoi(nmea_params[5].str);
129 lngs = atoi(&nmea_params[5].str[6]);
130 if (*nmea_params[6].str != 'E')
141 #define ms_to_deg(x, y) (((x * 10000) + y) / 60)
143 DEBUG(4, "position: %d°%d.%04d, %d°%d.%04d\n",
144 latd, latm, lats, lngd, lngm, lngs);
145 DEBUG(4, "position: %d°%d'%.1f\" %d°%d'%.1f\"\n",
146 latd, latm, flats, lngd, lngm, flngs);
148 snprintf(latitude, sizeof(latitude), "%d.%04d", latd, ms_to_deg(latm, lats));
149 snprintf(longitude, sizeof(longitude), "%d.%04d", lngd, ms_to_deg(lngm, lngs));
150 DEBUG(3, "position: %s %s\n", latitude, longitude);
160 strncpy(elivation, nmea_params[9].str, sizeof(elivation));
161 DEBUG(4, "height: %s\n", elivation);
169 strncpy(course, nmea_params[1].str, sizeof(course));
170 strncpy(speed, nmea_params[6].str, sizeof(speed));
171 DEBUG(4, "course: %s\n", course);
172 DEBUG(4, "speed: %s\n", speed);
175 static struct nmea_msg {
178 void (*handler) (void);
183 .handler = nmea_txt_cb,
187 .handler = nmea_rmc_cb,
191 .handler = nmea_gga_cb,
195 .handler = nmea_vtg_cb,
200 nmea_verify_checksum(char *s)
202 char *csum = strrchr(s, '*');
210 isum = strtol(csum, NULL, 16);
222 nmea_tokenize(char *msg)
225 char *tok = strsep(&msg, ",");
227 while (tok && cnt < MAX_NMEA_PARAM) {
228 nmea_params[cnt].str = tok;
229 nmea_params[cnt].num = atoi(tok);
231 tok = strsep(&msg, ",");
238 nmea_process(char *a)
243 if (strncmp(a, "$GP", 3))
247 csum = strrchr(a, '*');
251 if (nmea_verify_checksum(a)) {
252 ERROR("nmea message has invalid checksum\n");
256 cnt = nmea_tokenize(&a[2]);
258 ERROR("failed to tokenize %s\n", a);\
262 for (i = 0; i < ARRAY_SIZE(nmea_msgs); i++) {
263 if (strcmp(nmea_params[0].str, nmea_msgs[i].msg))
265 if (nmea_msgs[i].cnt <= cnt)
266 nmea_msgs[i].handler();
268 ERROR("%s datagram has wrong parameter count got %d but expected %d\n", nmea_msgs[i].msg, cnt, nmea_msgs[i].cnt);
274 nmea_consume(struct ustream *s, char **a)
276 char *eol = strstr(*a, "\n");
285 ustream_consume(s, eol - *a);
292 nmea_msg_cb(struct ustream *s, int bytes)
295 char *a = ustream_get_read_buf(s, &len);
297 while (!nmea_consume(s, &a))
301 static void nmea_notify_cb(struct ustream *s)
306 ERROR("tty error, shutting down\n");
311 nmea_open(char *dev, struct ustream_fd *s, speed_t speed)
316 tty = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
318 ERROR("%s: device open failed: %s\n", dev, strerror(errno));
322 tcgetattr(tty, &tio);
323 tio.c_cflag |= CREAD;
325 tio.c_iflag |= IGNPAR;
326 tio.c_lflag &= ~(ICANON);
327 tio.c_lflag &= ~(ECHO);
328 tio.c_lflag &= ~(ECHOE);
329 tio.c_lflag &= ~(ISIG);
332 cfsetispeed(&tio, speed);
333 cfsetospeed(&tio, speed);
334 tcsetattr(tty, TCSANOW, &tio);
336 s->stream.string_data = true;
337 s->stream.notify_read = nmea_msg_cb;
338 s->stream.notify_state = nmea_notify_cb;
340 ustream_fd_init(s, tty);
342 tcflush(tty, TCIFLUSH);