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>
39 #include <libubox/utils.h>
44 #define MAX_NMEA_PARAM 20
45 #define MAX_TIME_OFFSET 5
46 #define MAX_BAD_TIME 3
51 } nmea_params[MAX_NMEA_PARAM];
53 static int nmea_bad_time;
54 char longitude[32] = { 0 }, latitude[32] = { 0 }, course[16] = { 0 }, speed[16] = { 0 }, elevation[16] = { 0 };
60 char *ids[] = { "ERROR", "WARNING", "NOTICE", };
62 if (nmea_params[3].num < 0 || nmea_params[3].num > 2)
63 nmea_params[3].num = 0;
65 DEBUG(3, "%s: %s\n", ids[nmea_params[3].num], nmea_params[4].str);
74 if (*nmea_params[2].str != 'A') {
76 DEBUG(4, "waiting for valid signal\n");
81 memset(&tm, 0, sizeof(tm));
84 if (sscanf(nmea_params[1].str, "%02d%02d%02d",
85 &tm.tm_hour, &tm.tm_min, &tm.tm_sec) != 3) {
86 ERROR("failed to parse time '%s'\n", nmea_params[1].str);
88 else if (sscanf(nmea_params[9].str, "%02d%02d%02d",
89 &tm.tm_mday, &tm.tm_mon, &tm.tm_year) != 3) {
90 ERROR("failed to parse date '%s'\n", nmea_params[9].str);
93 tm.tm_year += 100; /* year starts with 1900 */
94 tm.tm_mon -= 1; /* month starts with 0 */
96 strftime(tmp, 256, "%Y-%m-%dT%H:%M:%S", &tm);
97 DEBUG(3, "date: %s UTC\n", tmp);
100 struct timeval tv = { timegm(&tm), 0 };
103 gettimeofday(&cur, NULL);
105 if (abs(cur.tv_sec - tv.tv_sec) > MAX_TIME_OFFSET) {
106 if (++nmea_bad_time > MAX_BAD_TIME) {
107 LOG("system time differs from GPS time by more than %d seconds. Using %s UTC as the new time\n", MAX_TIME_OFFSET, tmp);
108 /* only set datetime if specified by command line argument! */
109 settimeofday(&tv, NULL);
117 if (strlen(nmea_params[3].str) < 9 || strlen(nmea_params[5].str) < 10) {
118 ERROR("lat/lng have invalid string length %d<9, %d<10\n",
119 strlen(nmea_params[3].str), strlen(nmea_params[5].str));
123 float lat = strtof(nmea_params[3].str, NULL);
124 float lon = strtof(nmea_params[5].str, NULL);
126 degrees = floor(lat / 100.0);
127 minutes = lat - (degrees * 100.0);
128 lat = degrees + minutes / 60.0;
130 degrees = floor(lon / 100.0);
131 minutes = lon - (degrees * 100.0);
132 lon = degrees + minutes / 60.0;
134 if (*nmea_params[4].str == 'S')
136 if (*nmea_params[6].str == 'W')
139 snprintf(latitude, sizeof(latitude), "%f", lat);
140 snprintf(longitude, sizeof(longitude), "%f", lon);
142 DEBUG(3, "position: %s %s\n", latitude, longitude);
152 strncpy(elevation, nmea_params[9].str, sizeof(elevation));
153 DEBUG(4, "height: %s\n", elevation);
161 strncpy(course, nmea_params[1].str, sizeof(course));
162 strncpy(speed, nmea_params[7].str, sizeof(speed));
163 DEBUG(4, "course: %s\n", course);
164 DEBUG(4, "speed: %s\n", speed);
167 static struct nmea_msg {
170 void (*handler) (void);
175 .handler = nmea_txt_cb,
179 .handler = nmea_rmc_cb,
183 .handler = nmea_gga_cb,
187 .handler = nmea_vtg_cb,
192 nmea_verify_checksum(char *s)
194 char *csum = strrchr(s, '*');
202 isum = strtol(csum, NULL, 16);
214 nmea_tokenize(char *msg)
217 char *tok = strsep(&msg, ",");
219 while (tok && cnt < MAX_NMEA_PARAM) {
220 nmea_params[cnt].str = tok;
221 nmea_params[cnt].num = atoi(tok);
223 tok = strsep(&msg, ",");
230 nmea_process(char *a)
235 if (strncmp(a, "$GP", 3))
239 csum = strrchr(a, '*');
243 if (nmea_verify_checksum(a)) {
244 ERROR("nmea message has invalid checksum\n");
248 cnt = nmea_tokenize(&a[2]);
250 ERROR("failed to tokenize %s\n", a);\
254 for (i = 0; i < ARRAY_SIZE(nmea_msgs); i++) {
255 if (strcmp(nmea_params[0].str, nmea_msgs[i].msg))
257 if (nmea_msgs[i].cnt <= cnt)
258 nmea_msgs[i].handler();
260 ERROR("%s datagram has wrong parameter count got %d but expected %d\n", nmea_msgs[i].msg, cnt, nmea_msgs[i].cnt);
266 nmea_consume(struct ustream *s, char **a)
268 char *eol = strstr(*a, "\n");
277 ustream_consume(s, eol - *a);
284 nmea_msg_cb(struct ustream *s, int bytes)
287 char *a = ustream_get_read_buf(s, &len);
289 while (!nmea_consume(s, &a))
293 static void nmea_notify_cb(struct ustream *s)
298 ERROR("tty error, shutting down\n");
303 nmea_open(char *dev, struct ustream_fd *s, speed_t speed)
308 tty = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
310 ERROR("%s: device open failed: %s\n", dev, strerror(errno));
314 tcgetattr(tty, &tio);
315 tio.c_cflag |= CREAD;
317 tio.c_iflag |= IGNPAR;
318 tio.c_lflag &= ~(ICANON);
319 tio.c_lflag &= ~(ECHO);
320 tio.c_lflag &= ~(ECHOE);
321 tio.c_lflag &= ~(ISIG);
324 cfsetispeed(&tio, speed);
325 cfsetospeed(&tio, speed);
326 tcsetattr(tty, TCSANOW, &tio);
328 s->stream.string_data = true;
329 s->stream.notify_read = nmea_msg_cb;
330 s->stream.notify_state = nmea_notify_cb;
332 ustream_fd_init(s, tty);
334 tcflush(tty, TCIFLUSH);