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