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