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 }, satellites[3] = { 0 }, hdop[5] = { 0 }; 56 int gps_valid = 0; 57 char gps_fields = 0; 58 59 static void 60 nmea_txt_cb(void) 61 { 62 char *ids[] = { "ERROR", "WARNING", "NOTICE", }; 63 64 if (nmea_params[3].num < 0 || nmea_params[3].num > 2) 65 nmea_params[3].num = 0; 66 67 DEBUG(3, "%s: %s\n", ids[nmea_params[3].num], nmea_params[4].str); 68 } 69 70 static void 71 do_adjust_clock(struct tm *tm) 72 { 73 char tmp[256]; 74 75 strftime(tmp, 256, "%Y-%m-%dT%H:%M:%S", tm); 76 DEBUG(3, "date: %s UTC\n", tmp); 77 78 if (adjust_clock) { 79 time_t sec = timegm(tm); 80 struct timeval cur; 81 82 gettimeofday(&cur, NULL); 83 84 if ((sec < 0) || (llabs(cur.tv_sec - sec) > MAX_TIME_OFFSET)) { 85 struct timeval tv = { 0 }; 86 tv.tv_sec = sec; 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); 91 } 92 } else { 93 nmea_bad_time = 0; 94 } 95 } 96 } 97 98 static void 99 parse_gps_coords(char *latstr, char *vhem, char *lonstr, char *hhem) 100 { 101 float minutes; 102 float degrees; 103 float lat = strtof(latstr, NULL); 104 float lon = strtof(lonstr, NULL); 105 106 degrees = floor(lat / 100.0); 107 minutes = lat - (degrees * 100.0); 108 lat = degrees + minutes / 60.0; 109 110 degrees = floor(lon / 100.0); 111 minutes = lon - (degrees * 100.0); 112 lon = degrees + minutes / 60.0; 113 114 if (*vhem == 'S') 115 lat *= -1.0; 116 if (*hhem == 'W') 117 lon *= -1.0; 118 119 snprintf(latitude, sizeof(latitude), "%f", lat); 120 snprintf(longitude, sizeof(longitude), "%f", lon); 121 122 DEBUG(3, "position: %s %s\n", latitude, longitude); 123 gps_fields |= GPS_FIELD_LAT | GPS_FIELD_LON; 124 125 gps_timestamp(); 126 } 127 128 static void 129 nmea_rmc_cb(void) 130 { 131 struct tm tm; 132 133 if (*nmea_params[2].str != 'A') { 134 gps_valid = 0; 135 DEBUG(4, "waiting for valid signal\n"); 136 return; 137 } 138 139 gps_valid = 1; 140 memset(&tm, 0, sizeof(tm)); 141 tm.tm_isdst = 1; 142 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); 146 } 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); 150 } 151 else if (tm.tm_year == 0) { 152 DEBUG(4, "waiting for valid date\n"); 153 return; 154 } 155 else { 156 tm.tm_year += 100; /* year starts with 1900 */ 157 tm.tm_mon -= 1; /* month starts with 0 */ 158 159 do_adjust_clock(&tm); 160 } 161 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)); 165 } else { 166 parse_gps_coords(nmea_params[3].str, nmea_params[4].str, nmea_params[5].str, nmea_params[6].str); 167 } 168 } 169 170 static void 171 nmea_zda_cb(void) 172 { 173 struct tm tm; 174 175 if (!gps_valid) 176 return; 177 178 memset(&tm, 0, sizeof(tm)); 179 tm.tm_isdst = 1; 180 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); 184 return; 185 } 186 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); 192 return; 193 } 194 195 if (tm.tm_year == 0) { 196 DEBUG(4, "waiting for valid date\n"); 197 return; 198 } 199 200 tm.tm_mon -= 1; /* month starts with 0 */ 201 tm.tm_year -= 1900; /* full 4-digit year, tm expects years till 1900 */ 202 203 do_adjust_clock(&tm); 204 } 205 206 static void 207 nmea_gll_cb(void) 208 { 209 if (*nmea_params[6].str != 'A') { 210 gps_valid = 0; 211 DEBUG(4, "waiting for valid signal\n"); 212 return; 213 } 214 215 gps_valid = 1; 216 217 parse_gps_coords(nmea_params[1].str, nmea_params[2].str, nmea_params[3].str, nmea_params[4].str); 218 } 219 220 static void 221 nmea_gga_cb(void) 222 { 223 if (!gps_valid) 224 return; 225 strncpy(satellites, nmea_params[7].str, sizeof(satellites)); 226 strncpy(hdop, nmea_params[8].str, sizeof(hdop)); 227 strncpy(elevation, nmea_params[9].str, sizeof(elevation)); 228 gps_fields |= GPS_FIELD_SAT | GPS_FIELD_HDP | GPS_FIELD_ALT; 229 DEBUG(4, "satellites: %s\n", satellites); 230 DEBUG(4, "HDOP: %s\n", hdop); 231 DEBUG(4, "height: %s\n", elevation); 232 } 233 234 static void 235 nmea_vtg_cb(void) 236 { 237 if (!gps_valid) 238 return; 239 strncpy(course, nmea_params[1].str, sizeof(course)); 240 strncpy(speed, nmea_params[7].str, sizeof(speed)); 241 gps_fields |= GPS_FIELD_COG | GPS_FIELD_SPD; 242 DEBUG(4, "course: %s\n", course); 243 DEBUG(4, "speed: %s\n", speed); 244 } 245 246 static struct nmea_msg { 247 char *msg; 248 int cnt; 249 void (*handler) (void); 250 } nmea_msgs[] = { 251 { 252 .msg = "TXT", 253 .cnt = 5, 254 .handler = nmea_txt_cb, 255 }, { 256 .msg = "RMC", 257 .cnt = 11, 258 .handler = nmea_rmc_cb, 259 }, { 260 .msg = "GGA", 261 .cnt = 14, 262 .handler = nmea_gga_cb, 263 }, { 264 .msg = "GLL", 265 .cnt = 7, 266 .handler = nmea_gll_cb, 267 }, { 268 .msg = "VTG", 269 .cnt = 9, 270 .handler = nmea_vtg_cb, 271 }, { 272 .msg = "ZDA", 273 .cnt = 5, 274 .handler = nmea_zda_cb, 275 }, 276 }; 277 278 static int 279 nmea_verify_checksum(char *s) 280 { 281 char *csum = strrchr(s, '*'); 282 int isum, c = 0; 283 284 if (!csum) 285 return -1; 286 287 *csum = '\0'; 288 csum++; 289 isum = strtol(csum, NULL, 16); 290 291 while(*s) 292 c ^= *s++; 293 294 if (isum != c) 295 return -1; 296 297 return 0; 298 } 299 300 static int 301 nmea_tokenize(char *msg) 302 { 303 int cnt = 0; 304 char *tok = strsep(&msg, ","); 305 306 while (tok && cnt < MAX_NMEA_PARAM) { 307 nmea_params[cnt].str = tok; 308 nmea_params[cnt].num = atoi(tok); 309 cnt++; 310 tok = strsep(&msg, ","); 311 } 312 313 return cnt; 314 } 315 316 static void 317 nmea_process(char *a) 318 { 319 char *csum; 320 int cnt; 321 unsigned int i; 322 323 if (strncmp(a, "$GP", 3) && 324 strncmp(a, "$GN", 3)) 325 return; 326 327 a++; 328 csum = strrchr(a, '*'); 329 if (!csum) 330 return; 331 332 if (nmea_verify_checksum(a)) { 333 ERROR("nmea message has invalid checksum\n"); 334 return; 335 } 336 337 cnt = nmea_tokenize(&a[2]); 338 if (cnt < 0) { 339 ERROR("failed to tokenize %s\n", a);\ 340 return; 341 } 342 343 for (i = 0; i < ARRAY_SIZE(nmea_msgs); i++) { 344 if (strcmp(nmea_params[0].str, nmea_msgs[i].msg) && 345 strcmp(nmea_params[3].str, nmea_msgs[i].msg)) 346 continue; 347 if (nmea_msgs[i].cnt <= cnt) 348 nmea_msgs[i].handler(); 349 else 350 ERROR("%s datagram has wrong parameter count got %d but expected %d\n", nmea_msgs[i].msg, cnt, nmea_msgs[i].cnt); 351 return; 352 } 353 } 354 355 static int 356 nmea_consume(struct ustream *s, char **a) 357 { 358 char *eol = strstr(*a, "\n"); 359 360 if (!eol) 361 return -1; 362 363 *eol++ = '\0'; 364 365 nmea_process(*a); 366 367 ustream_consume(s, eol - *a); 368 *a = eol; 369 370 return 0; 371 } 372 373 static void 374 nmea_msg_cb(struct ustream *s, int bytes) 375 { 376 int len; 377 char *a = ustream_get_read_buf(s, &len); 378 379 while (!nmea_consume(s, &a)) 380 ; 381 } 382 383 static void nmea_notify_cb(struct ustream *s) 384 { 385 if (!s->eof) 386 return; 387 388 ERROR("tty error, shutting down\n"); 389 exit(-1); 390 } 391 392 int 393 nmea_open(char *dev, struct ustream_fd *s, speed_t speed) 394 { 395 struct termios tio; 396 int tty; 397 398 tty = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK); 399 if (tty < 0) { 400 ERROR("%s: device open failed: %s\n", dev, strerror(errno)); 401 return -1; 402 } 403 404 tcgetattr(tty, &tio); 405 tio.c_cflag |= CREAD; 406 tio.c_cflag |= CS8; 407 tio.c_iflag |= IGNPAR; 408 tio.c_lflag &= ~(ICANON); 409 tio.c_lflag &= ~(ECHO); 410 tio.c_lflag &= ~(ECHOE); 411 tio.c_lflag &= ~(ISIG); 412 tio.c_cc[VMIN] = 1; 413 tio.c_cc[VTIME] = 0; 414 cfsetispeed(&tio, speed); 415 cfsetospeed(&tio, speed); 416 tcsetattr(tty, TCSANOW, &tio); 417 418 s->stream.string_data = true; 419 s->stream.notify_read = nmea_msg_cb; 420 s->stream.notify_state = nmea_notify_cb; 421 422 ustream_fd_init(s, tty); 423 424 tcflush(tty, TCIFLUSH); 425 426 return 0; 427 } 428
This page was automatically generated by LXR 0.3.1. • OpenWrt