5a691b92b669f48751da85d1e343babd76afd480
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
);