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 }, 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 }