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 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(elevation, nmea_params[9].str, sizeof(elevation)); 226 gps_fields |= GPS_FIELD_ALT; 227 DEBUG(4, "height: %s\n", elevation); 228 } 229 230 static void 231 nmea_vtg_cb(void) 232 { 233 if (!gps_valid) 234 return; 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); 240 } 241 242 static struct nmea_msg { 243 char *msg; 244 int cnt; 245 void (*handler) (void); 246 } nmea_msgs[] = { 247 { 248 .msg = "TXT", 249 .cnt = 5, 250 .handler = nmea_txt_cb, 251 }, { 252 .msg = "RMC", 253 .cnt = 11, 254 .handler = nmea_rmc_cb, 255 }, { 256 .msg = "GGA", 257 .cnt = 14, 258 .handler = nmea_gga_cb, 259 }, { 260 .msg = "GLL", 261 .cnt = 7, 262 .handler = nmea_gll_cb, 263 }, { 264 .msg = "VTG", 265 .cnt = 9, 266 .handler = nmea_vtg_cb, 267 }, { 268 .msg = "ZDA", 269 .cnt = 5, 270 .handler = nmea_zda_cb, 271 }, 272 }; 273 274 static int 275 nmea_verify_checksum(char *s) 276 { 277 char *csum = strrchr(s, '*'); 278 int isum, c = 0; 279 280 if (!csum) 281 return -1; 282 283 *csum = '\0'; 284 csum++; 285 isum = strtol(csum, NULL, 16); 286 287 while(*s) 288 c ^= *s++; 289 290 if (isum != c) 291 return -1; 292 293 return 0; 294 } 295 296 static int 297 nmea_tokenize(char *msg) 298 { 299 int cnt = 0; 300 char *tok = strsep(&msg, ","); 301 302 while (tok && cnt < MAX_NMEA_PARAM) { 303 nmea_params[cnt].str = tok; 304 nmea_params[cnt].num = atoi(tok); 305 cnt++; 306 tok = strsep(&msg, ","); 307 } 308 309 return cnt; 310 } 311 312 static void 313 nmea_process(char *a) 314 { 315 char *csum; 316 int cnt; 317 unsigned int i; 318 319 if (strncmp(a, "$GP", 3) && 320 strncmp(a, "$GN", 3)) 321 return; 322 323 a++; 324 csum = strrchr(a, '*'); 325 if (!csum) 326 return; 327 328 if (nmea_verify_checksum(a)) { 329 ERROR("nmea message has invalid checksum\n"); 330 return; 331 } 332 333 cnt = nmea_tokenize(&a[2]); 334 if (cnt < 0) { 335 ERROR("failed to tokenize %s\n", a);\ 336 return; 337 } 338 339 for (i = 0; i < ARRAY_SIZE(nmea_msgs); i++) { 340 if (strcmp(nmea_params[0].str, nmea_msgs[i].msg) && 341 strcmp(nmea_params[3].str, nmea_msgs[i].msg)) 342 continue; 343 if (nmea_msgs[i].cnt <= cnt) 344 nmea_msgs[i].handler(); 345 else 346 ERROR("%s datagram has wrong parameter count got %d but expected %d\n", nmea_msgs[i].msg, cnt, nmea_msgs[i].cnt); 347 return; 348 } 349 } 350 351 static int 352 nmea_consume(struct ustream *s, char **a) 353 { 354 char *eol = strstr(*a, "\n"); 355 356 if (!eol) 357 return -1; 358 359 *eol++ = '\0'; 360 361 nmea_process(*a); 362 363 ustream_consume(s, eol - *a); 364 *a = eol; 365 366 return 0; 367 } 368 369 static void 370 nmea_msg_cb(struct ustream *s, int bytes) 371 { 372 int len; 373 char *a = ustream_get_read_buf(s, &len); 374 375 while (!nmea_consume(s, &a)) 376 ; 377 } 378 379 static void nmea_notify_cb(struct ustream *s) 380 { 381 if (!s->eof) 382 return; 383 384 ERROR("tty error, shutting down\n"); 385 exit(-1); 386 } 387 388 int 389 nmea_open(char *dev, struct ustream_fd *s, speed_t speed) 390 { 391 struct termios tio; 392 int tty; 393 394 tty = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK); 395 if (tty < 0) { 396 ERROR("%s: device open failed: %s\n", dev, strerror(errno)); 397 return -1; 398 } 399 400 tcgetattr(tty, &tio); 401 tio.c_cflag |= CREAD; 402 tio.c_cflag |= CS8; 403 tio.c_iflag |= IGNPAR; 404 tio.c_lflag &= ~(ICANON); 405 tio.c_lflag &= ~(ECHO); 406 tio.c_lflag &= ~(ECHOE); 407 tio.c_lflag &= ~(ISIG); 408 tio.c_cc[VMIN] = 1; 409 tio.c_cc[VTIME] = 0; 410 cfsetispeed(&tio, speed); 411 cfsetospeed(&tio, speed); 412 tcsetattr(tty, TCSANOW, &tio); 413 414 s->stream.string_data = true; 415 s->stream.notify_read = nmea_msg_cb; 416 s->stream.notify_state = nmea_notify_cb; 417 418 ustream_fd_init(s, tty); 419 420 tcflush(tty, TCIFLUSH); 421 422 return 0; 423 } 424
This page was automatically generated by LXR 0.3.1. • OpenWrt