8e6160632f59b91f18be298d4192b24699258e63
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>
19 #define _DEFAULT_SOURCE
24 #include <sys/types.h>
26 #include <sys/ioctl.h>
40 #include <libubox/utils.h>
45 #define MAX_NMEA_PARAM 20
46 #define MAX_TIME_OFFSET 5
47 #define MAX_BAD_TIME 3
52 } nmea_params
[MAX_NMEA_PARAM
];
54 static int nmea_bad_time
;
55 char longitude
[33] = { 0 }, latitude
[33] = { 0 }, course
[17] = { 0 }, speed
[17] = { 0 }, elevation
[17] = { 0 };
62 char *ids
[] = { "ERROR", "WARNING", "NOTICE", };
64 if (nmea_params
[3].num
< 0 || nmea_params
[3].num
> 2)
65 nmea_params
[3].num
= 0;
67 DEBUG(3, "%s: %s\n", ids
[nmea_params
[3].num
], nmea_params
[4].str
);
71 do_adjust_clock(struct tm
*tm
)
75 strftime(tmp
, 256, "%Y-%m-%dT%H:%M:%S", tm
);
76 DEBUG(3, "date: %s UTC\n", tmp
);
79 time_t sec
= timegm(tm
);
82 gettimeofday(&cur
, NULL
);
84 if ((sec
< 0) || (llabs(cur
.tv_sec
- sec
) > MAX_TIME_OFFSET
)) {
85 struct timeval tv
= { 0 };
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
);
99 parse_gps_coords(char *latstr
, char *vhem
, char *lonstr
, char *hhem
)
103 float lat
= strtof(latstr
, NULL
);
104 float lon
= strtof(lonstr
, NULL
);
106 degrees
= floor(lat
/ 100.0);
107 minutes
= lat
- (degrees
* 100.0);
108 lat
= degrees
+ minutes
/ 60.0;
110 degrees
= floor(lon
/ 100.0);
111 minutes
= lon
- (degrees
* 100.0);
112 lon
= degrees
+ minutes
/ 60.0;
119 snprintf(latitude
, sizeof(latitude
), "%f", lat
);
120 snprintf(longitude
, sizeof(longitude
), "%f", lon
);
122 DEBUG(3, "position: %s %s\n", latitude
, longitude
);
123 gps_fields
|= GPS_FIELD_LAT
| GPS_FIELD_LON
;
133 if (*nmea_params
[2].str
!= 'A') {
135 DEBUG(4, "waiting for valid signal\n");
140 memset(&tm
, 0, sizeof(tm
));
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
);
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
);
151 else if (tm
.tm_year
== 0) {
152 DEBUG(4, "waiting for valid date\n");
156 tm
.tm_year
+= 100; /* year starts with 1900 */
157 tm
.tm_mon
-= 1; /* month starts with 0 */
159 do_adjust_clock(&tm
);
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
));
166 parse_gps_coords(nmea_params
[3].str
, nmea_params
[4].str
, nmea_params
[5].str
, nmea_params
[6].str
);
178 memset(&tm
, 0, sizeof(tm
));
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
);
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
);
195 if (tm
.tm_year
== 0) {
196 DEBUG(4, "waiting for valid date\n");
200 tm
.tm_mon
-= 1; /* month starts with 0 */
201 tm
.tm_year
-= 1900; /* full 4-digit year, tm expects years till 1900 */
203 do_adjust_clock(&tm
);
209 if (*nmea_params
[6].str
!= 'A') {
211 DEBUG(4, "waiting for valid signal\n");
217 parse_gps_coords(nmea_params
[1].str
, nmea_params
[2].str
, nmea_params
[3].str
, nmea_params
[4].str
);
225 strncpy(elevation
, nmea_params
[9].str
, sizeof(elevation
));
226 gps_fields
|= GPS_FIELD_ALT
;
227 DEBUG(4, "height: %s\n", elevation
);
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
);
242 static struct nmea_msg
{
245 void (*handler
) (void);
250 .handler
= nmea_txt_cb
,
254 .handler
= nmea_rmc_cb
,
258 .handler
= nmea_gga_cb
,
262 .handler
= nmea_gll_cb
,
266 .handler
= nmea_vtg_cb
,
270 .handler
= nmea_zda_cb
,
275 nmea_verify_checksum(char *s
)
277 char *csum
= strrchr(s
, '*');
285 isum
= strtol(csum
, NULL
, 16);
297 nmea_tokenize(char *msg
)
300 char *tok
= strsep(&msg
, ",");
302 while (tok
&& cnt
< MAX_NMEA_PARAM
) {
303 nmea_params
[cnt
].str
= tok
;
304 nmea_params
[cnt
].num
= atoi(tok
);
306 tok
= strsep(&msg
, ",");
313 nmea_process(char *a
)
319 if (strncmp(a
, "$GP", 3))
323 csum
= strrchr(a
, '*');
327 if (nmea_verify_checksum(a
)) {
328 ERROR("nmea message has invalid checksum\n");
332 cnt
= nmea_tokenize(&a
[2]);
334 ERROR("failed to tokenize %s\n", a
);\
338 for (i
= 0; i
< ARRAY_SIZE(nmea_msgs
); i
++) {
339 if (strcmp(nmea_params
[0].str
, nmea_msgs
[i
].msg
))
341 if (nmea_msgs
[i
].cnt
<= cnt
)
342 nmea_msgs
[i
].handler();
344 ERROR("%s datagram has wrong parameter count got %d but expected %d\n", nmea_msgs
[i
].msg
, cnt
, nmea_msgs
[i
].cnt
);
350 nmea_consume(struct ustream
*s
, char **a
)
352 char *eol
= strstr(*a
, "\n");
361 ustream_consume(s
, eol
- *a
);
368 nmea_msg_cb(struct ustream
*s
, int bytes
)
371 char *a
= ustream_get_read_buf(s
, &len
);
373 while (!nmea_consume(s
, &a
))
377 static void nmea_notify_cb(struct ustream
*s
)
382 ERROR("tty error, shutting down\n");
387 nmea_open(char *dev
, struct ustream_fd
*s
, speed_t speed
)
392 tty
= open(dev
, O_RDWR
| O_NOCTTY
| O_NONBLOCK
);
394 ERROR("%s: device open failed: %s\n", dev
, strerror(errno
));
398 tcgetattr(tty
, &tio
);
399 tio
.c_cflag
|= CREAD
;
401 tio
.c_iflag
|= IGNPAR
;
402 tio
.c_lflag
&= ~(ICANON
);
403 tio
.c_lflag
&= ~(ECHO
);
404 tio
.c_lflag
&= ~(ECHOE
);
405 tio
.c_lflag
&= ~(ISIG
);
408 cfsetispeed(&tio
, speed
);
409 cfsetospeed(&tio
, speed
);
410 tcsetattr(tty
, TCSANOW
, &tio
);
412 s
->stream
.string_data
= true;
413 s
->stream
.notify_read
= nmea_msg_cb
;
414 s
->stream
.notify_state
= nmea_notify_cb
;
416 ustream_fd_init(s
, tty
);
418 tcflush(tty
, TCIFLUSH
);