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>
37 #include <libubox/utils.h>
42 #define MAX_NMEA_PARAM 20
43 #define MAX_TIME_OFFSET 2
44 #define MAX_BAD_TIME 3
49 } nmea_params[MAX_NMEA_PARAM];
51 static int nmea_bad_time;
52 char longitude[32] = { 0 }, latitude[32] = { 0 }, course[16] = { 0 }, speed[16] = { 0 }, elivation[16] = { 0 };
58 char *ids[] = { "ERROR", "WARNING", "NOTICE", };
60 if (nmea_params[3].num < 0 || nmea_params[3].num > 2)
61 nmea_params[3].num = 0;
63 DEBUG(3, "%s: %s\n", ids[nmea_params[3].num], nmea_params[4].str);
72 if (*nmea_params[2].str != 'A') {
74 DEBUG(4, "waiting for valid signal\n");
79 memset(&tm, 0, sizeof(tm));
82 if (!strptime(nmea_params[1].str, "%H%M%S", &tm))
83 ERROR("failed to parse time\n");
84 else if (!strptime(nmea_params[9].str, "%d%m%y", &tm))
85 ERROR("failed to parse date\n");
87 /* is there a libc api for the tz adjustment ? */
88 struct timeval tv = { mktime(&tm), 0 };
91 strftime(tmp, 256, "%D %02H:%02M:%02S", &tm);
92 DEBUG(3, "date: %s UTC\n", tmp);
94 tv.tv_sec -= timezone;
98 gettimeofday(&cur, NULL);
100 if (abs(cur.tv_sec - tv.tv_sec) > MAX_TIME_OFFSET) {
101 if (++nmea_bad_time > MAX_BAD_TIME) {
102 LOG("system time differs from GPS time by more than %d seconds. Using %s UTC as the new time\n", MAX_TIME_OFFSET, tmp);
103 settimeofday(&tv, NULL);
110 if (strlen(nmea_params[3].str) != 9 || strlen(nmea_params[5].str) != 10) {
111 ERROR("lat/lng have invalid string length\n");
113 int latd, latm, lats;
114 int lngd, lngm, lngs;
116 DEBUG(4, "position: %s, %s\n",
117 nmea_params[3].str, nmea_params[5].str);
118 latm = atoi(&nmea_params[3].str[2]);
119 nmea_params[3].str[2] = '\0';
120 latd = atoi(nmea_params[3].str);
121 lats = atoi(&nmea_params[3].str[5]);
122 if (*nmea_params[4].str != 'N')
125 lngm = atoi(&nmea_params[5].str[3]);
126 nmea_params[5].str[3] = '\0';
127 lngd = atoi(nmea_params[5].str);
128 lngs = atoi(&nmea_params[5].str[6]);
129 if (*nmea_params[6].str != 'E')
140 #define ms_to_deg(x, y) (((x * 10000) + y) / 60)
142 DEBUG(4, "position: %d°%d.%04d, %d°%d.%04d\n",
143 latd, latm, lats, lngd, lngm, lngs);
144 DEBUG(4, "position: %d°%d'%.1f\" %d°%d'%.1f\"\n",
145 latd, latm, flats, lngd, lngm, flngs);
147 snprintf(latitude, sizeof(latitude), "%d.%04d", latd, ms_to_deg(latm, lats));
148 snprintf(longitude, sizeof(longitude), "%d.%04d", lngd, ms_to_deg(lngm, lngs));
149 DEBUG(3, "position: %s %s\n", latitude, longitude);
159 strncpy(elivation, nmea_params[9].str, sizeof(elivation));
160 DEBUG(4, "height: %s\n", elivation);
168 strncpy(course, nmea_params[1].str, sizeof(course));
169 strncpy(speed, nmea_params[6].str, sizeof(speed));
170 DEBUG(4, "course: %s\n", course);
171 DEBUG(4, "speed: %s\n", speed);
174 static struct nmea_msg {
177 void (*handler) (void);
182 .handler = nmea_txt_cb,
186 .handler = nmea_rmc_cb,
190 .handler = nmea_gga_cb,
194 .handler = nmea_vtg_cb,
199 nmea_verify_checksum(char *s)
201 char *csum = strrchr(s, '*');
209 isum = strtol(csum, NULL, 16);
221 nmea_tokenize(char *msg)
224 char *tok = strsep(&msg, ",");
226 while (tok && cnt < MAX_NMEA_PARAM) {
227 nmea_params[cnt].str = tok;
228 nmea_params[cnt].num = atoi(tok);
230 tok = strsep(&msg, ",");
237 nmea_process(char *a)
242 if (strncmp(a, "$GP", 3))
246 csum = strrchr(a, '*');
250 if (nmea_verify_checksum(a)) {
251 ERROR("nmea message has invalid checksum\n");
255 cnt = nmea_tokenize(&a[2]);
257 ERROR("failed to tokenize %s\n", a);\
261 for (i = 0; i < ARRAY_SIZE(nmea_msgs); i++) {
262 if (strcmp(nmea_params[0].str, nmea_msgs[i].msg))
264 if (nmea_msgs[i].cnt <= cnt)
265 nmea_msgs[i].handler();
267 ERROR("%s datagram has wrong parameter count got %d but expected %d\n", nmea_msgs[i].msg, cnt, nmea_msgs[i].cnt);
273 nmea_consume(struct ustream *s, char **a)
275 char *eol = strstr(*a, "\n");
284 ustream_consume(s, eol - *a);
291 nmea_msg_cb(struct ustream *s, int bytes)
294 char *a = ustream_get_read_buf(s, &len);
296 while (!nmea_consume(s, &a))
300 static void nmea_notify_cb(struct ustream *s)
305 ERROR("tty error, shutting down\n");
310 nmea_open(char *dev, struct ustream_fd *s, speed_t speed)
315 tty = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
317 ERROR("%s: device open failed\n", dev);
321 tcgetattr(tty, &tio);
322 tio.c_cflag |= CREAD;
324 tio.c_iflag |= IGNPAR;
325 tio.c_lflag &= ~(ICANON);
326 tio.c_lflag &= ~(ECHO);
327 tio.c_lflag &= ~(ECHOE);
328 tio.c_lflag &= ~(ISIG);
331 cfsetispeed(&tio, speed);
332 cfsetospeed(&tio, speed);
333 tcsetattr(tty, TCSANOW, &tio);
335 s->stream.string_data = true;
336 s->stream.notify_read = nmea_msg_cb;
337 s->stream.notify_state = nmea_notify_cb;
339 ustream_fd_init(s, tty);
341 tcflush(tty, TCIFLUSH);