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