• source navigation  • diff markup  • identifier search  • freetext search  • 

Sources/ugps/nmea.c

  1 /*
  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.
  6  *
  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.
 11  *
 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.
 15  *
 16  *   Copyright (C) 2014 John Crispin <blogic@openwrt.org> 
 17  */
 18 
 19 #define _DEFAULT_SOURCE
 20 #define _XOPEN_SOURCE
 21 #define _BSD_SOURCE
 22 #include <time.h>
 23 
 24 #include <sys/types.h>
 25 #include <sys/stat.h>
 26 #include <sys/ioctl.h>
 27 #include <sys/time.h>
 28 
 29 #include <fcntl.h>
 30 #include <time.h>
 31 #include <stdlib.h>
 32 #include <stdio.h>
 33 #include <unistd.h>
 34 #include <errno.h>
 35 #include <math.h>
 36 
 37 #include <string.h>
 38 #include <termios.h>
 39 
 40 #include <libubox/utils.h>
 41 
 42 #include "log.h"
 43 #include "nmea.h"
 44 
 45 #define MAX_NMEA_PARAM  20
 46 #define MAX_TIME_OFFSET 5
 47 #define MAX_BAD_TIME    3
 48 
 49 struct nmea_param {
 50         char *str;
 51         int num;
 52 } nmea_params[MAX_NMEA_PARAM];
 53 
 54 static int nmea_bad_time;
 55 char longitude[33] = { 0 }, latitude[33] = { 0 }, course[17] = { 0 }, speed[17] = { 0 }, elevation[17] = { 0 };
 56 int gps_valid = 0;
 57 
 58 static void
 59 nmea_txt_cb(void)
 60 {
 61         char *ids[] = { "ERROR", "WARNING", "NOTICE", };
 62 
 63         if (nmea_params[3].num < 0 || nmea_params[3].num > 2)
 64                 nmea_params[3].num = 0;
 65 
 66         DEBUG(3, "%s: %s\n", ids[nmea_params[3].num], nmea_params[4].str);
 67 }
 68 
 69 static void
 70 nmea_rmc_cb(void)
 71 {
 72         struct tm tm;
 73         char tmp[256];
 74 
 75         if (*nmea_params[2].str != 'A') {
 76                 gps_valid = 0;
 77                 DEBUG(4, "waiting for valid signal\n");
 78                 return;
 79         }
 80 
 81         gps_valid = 1;
 82         memset(&tm, 0, sizeof(tm));
 83         tm.tm_isdst = 1;
 84 
 85         if (sscanf(nmea_params[1].str, "%02d%02d%02d",
 86                 &tm.tm_hour, &tm.tm_min, &tm.tm_sec) != 3) {
 87                 ERROR("failed to parse time '%s'\n", nmea_params[1].str);
 88         }
 89         else if (sscanf(nmea_params[9].str, "%02d%02d%02d",
 90                 &tm.tm_mday, &tm.tm_mon, &tm.tm_year) != 3) {
 91                 ERROR("failed to parse date '%s'\n", nmea_params[9].str);
 92         }
 93         else {
 94                 tm.tm_year += 100; /* year starts with 1900 */
 95                 tm.tm_mon -= 1; /* month starts with 0 */
 96 
 97                 strftime(tmp, 256, "%Y-%m-%dT%H:%M:%S", &tm);
 98                 DEBUG(3, "date: %s UTC\n", tmp);
 99 
100                 if (adjust_clock) {
101                         time_t sec = timegm(&tm);
102                         struct timeval tv = { 0 };
103                         struct timeval cur;
104 
105                         gettimeofday(&cur, NULL);
106 
107                         if ((sec < 0) || (abs(cur.tv_sec - tv.tv_sec) > MAX_TIME_OFFSET)) {
108                                 tv.tv_sec = sec;
109                                 if (++nmea_bad_time > MAX_BAD_TIME) {
110                                         LOG("system time differs from GPS time by more than %d seconds. Using %s UTC as the new time\n", MAX_TIME_OFFSET, tmp);
111                                         /* only set datetime if specified by command line argument! */
112                                         settimeofday(&tv, NULL);
113                                 }
114                         } else {
115                                 nmea_bad_time = 0;
116                         }
117                 }
118         }
119 
120         if (strlen(nmea_params[3].str) < 9 || strlen(nmea_params[5].str) < 10) {
121                 ERROR("lat/lng have invalid string length %zu<9, %zu<10\n",
122                        strlen(nmea_params[3].str), strlen(nmea_params[5].str));
123         } else {
124                 float minutes;
125                 float degrees;
126                 float lat = strtof(nmea_params[3].str, NULL);
127                 float lon = strtof(nmea_params[5].str, NULL);
128 
129                 degrees = floor(lat / 100.0);
130                 minutes = lat - (degrees * 100.0);
131                 lat = degrees + minutes / 60.0;
132 
133                 degrees = floor(lon / 100.0);
134                 minutes = lon - (degrees * 100.0);
135                 lon = degrees + minutes / 60.0;
136 
137                 if (*nmea_params[4].str == 'S')
138                         lat *= -1.0;
139                 if (*nmea_params[6].str == 'W')
140                         lon *= -1.0;
141 
142                 snprintf(latitude, sizeof(latitude), "%f", lat);
143                 snprintf(longitude, sizeof(longitude), "%f", lon);
144 
145                 DEBUG(3, "position: %s %s\n", latitude, longitude);
146                 gps_timestamp();
147         }
148 }
149 
150 static void
151 nmea_gga_cb(void)
152 {
153         if (!gps_valid)
154                 return;
155         strncpy(elevation, nmea_params[9].str, sizeof(elevation));
156         DEBUG(4, "height: %s\n", elevation);
157 }
158 
159 static void
160 nmea_vtg_cb(void)
161 {
162         if (!gps_valid)
163                 return;
164         strncpy(course, nmea_params[1].str, sizeof(course));
165         strncpy(speed, nmea_params[7].str, sizeof(speed));
166         DEBUG(4, "course: %s\n", course);
167         DEBUG(4, "speed: %s\n", speed);
168 }
169 
170 static struct nmea_msg {
171         char *msg;
172         int cnt;
173         void (*handler) (void);
174 } nmea_msgs[] = {
175         {
176                 .msg = "TXT",
177                 .cnt = 5,
178                 .handler = nmea_txt_cb,
179         }, {
180                 .msg = "RMC",
181                 .cnt = 11,
182                 .handler = nmea_rmc_cb,
183         }, {
184                 .msg = "GGA",
185                 .cnt = 14,
186                 .handler = nmea_gga_cb,
187         }, {
188                 .msg = "VTG",
189                 .cnt = 9,
190                 .handler = nmea_vtg_cb,
191         },
192 };
193 
194 static int
195 nmea_verify_checksum(char *s)
196 {
197         char *csum = strrchr(s, '*');
198         int isum, c = 0;
199 
200         if (!csum)
201                 return -1;
202 
203         *csum = '\0';
204         csum++;
205         isum = strtol(csum, NULL, 16);
206 
207         while(*s)
208                 c ^= *s++;
209 
210         if (isum != c)
211                 return -1;
212 
213         return 0;
214 }
215 
216 static int
217 nmea_tokenize(char *msg)
218 {
219         int cnt = 0;
220         char *tok = strsep(&msg, ",");
221 
222         while (tok && cnt < MAX_NMEA_PARAM) {
223                 nmea_params[cnt].str = tok;
224                 nmea_params[cnt].num = atoi(tok);
225                 cnt++;
226                 tok = strsep(&msg, ",");
227         }
228 
229         return cnt;
230 }
231 
232 static void
233 nmea_process(char *a)
234 {
235         char *csum;
236         int cnt;
237         unsigned int i;
238 
239         if (strncmp(a, "$GP", 3))
240                 return;
241 
242         a++;
243         csum = strrchr(a, '*');
244         if (!csum)
245                 return;
246 
247         if (nmea_verify_checksum(a)) {
248                 ERROR("nmea message has invalid checksum\n");
249                 return;
250         }
251 
252         cnt = nmea_tokenize(&a[2]);
253         if (cnt < 0) {
254                 ERROR("failed to tokenize %s\n", a);\
255                 return;
256         }
257 
258         for (i = 0; i < ARRAY_SIZE(nmea_msgs); i++) {
259                 if (strcmp(nmea_params[0].str, nmea_msgs[i].msg))
260                         continue;
261                 if (nmea_msgs[i].cnt <= cnt)
262                         nmea_msgs[i].handler();
263                 else
264                         ERROR("%s datagram has wrong parameter count got %d but expected %d\n", nmea_msgs[i].msg, cnt, nmea_msgs[i].cnt);
265                 return;
266         }
267 }
268 
269 static int
270 nmea_consume(struct ustream *s, char **a)
271 {
272         char *eol = strstr(*a, "\n");
273 
274         if (!eol)
275                 return -1;
276 
277         *eol++ = '\0';
278 
279         nmea_process(*a);
280 
281         ustream_consume(s, eol - *a);
282         *a = eol;
283 
284         return 0;
285 }
286 
287 static void
288 nmea_msg_cb(struct ustream *s, int bytes)
289 {
290         int len;
291         char *a = ustream_get_read_buf(s, &len);
292 
293         while (!nmea_consume(s, &a))
294                 ;
295 }
296 
297 static void nmea_notify_cb(struct ustream *s)
298 {
299         if (!s->eof)
300                 return;
301 
302         ERROR("tty error, shutting down\n");
303         exit(-1);
304 }
305 
306 int
307 nmea_open(char *dev, struct ustream_fd *s, speed_t speed)
308 {
309         struct termios tio;
310         int tty;
311 
312         tty = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
313         if (tty < 0) {
314                 ERROR("%s: device open failed: %s\n", dev, strerror(errno));
315                 return -1;
316         }
317 
318         tcgetattr(tty, &tio);
319         tio.c_cflag |= CREAD;
320         tio.c_cflag |= CS8;
321         tio.c_iflag |= IGNPAR;
322         tio.c_lflag &= ~(ICANON);
323         tio.c_lflag &= ~(ECHO);
324         tio.c_lflag &= ~(ECHOE);
325         tio.c_lflag &= ~(ISIG);
326         tio.c_cc[VMIN] = 1;
327         tio.c_cc[VTIME] = 0;
328         cfsetispeed(&tio, speed);
329         cfsetospeed(&tio, speed);
330         tcsetattr(tty, TCSANOW, &tio);
331 
332         s->stream.string_data = true;
333         s->stream.notify_read = nmea_msg_cb;
334         s->stream.notify_state = nmea_notify_cb;
335 
336         ustream_fd_init(s, tty);
337 
338         tcflush(tty, TCIFLUSH);
339 
340         return 0;
341 }
342 

This page was automatically generated by LXR 0.3.1.  •  OpenWrt