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 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 %d<9, %d<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, i;
 237
 238         if (strncmp(a, "$GP", 3))
 239                 return;
 240
 241         a++;
 242         csum = strrchr(a, '*');
 243         if (!csum)
 244                 return;
 245
 246         if (nmea_verify_checksum(a)) {
 247                 ERROR("nmea message has invalid checksum\n");
 248                 return;
 249         }
 250
 251         cnt = nmea_tokenize(&a[2]);
 252         if (cnt < 0) {
 253                 ERROR("failed to tokenize %s\n", a);\
 254                 return;
 255         }
 256
 257         for (i = 0; i < ARRAY_SIZE(nmea_msgs); i++) {
 258                 if (strcmp(nmea_params[0].str, nmea_msgs[i].msg))
 259                         continue;
 260                 if (nmea_msgs[i].cnt <= cnt)
 261                         nmea_msgs[i].handler();
 262                 else
 263                         ERROR("%s datagram has wrong parameter count got %d but expected %d\n", nmea_msgs[i].msg, cnt, nmea_msgs[i].cnt);
 264                 return;
 265         }
 266 }
 267
 268 static int
 269 nmea_consume(struct ustream *s, char **a)
 270 {
 271         char *eol = strstr(*a, "\n");
 272
 273         if (!eol)
 274                 return -1;
 275
 276         *eol++ = '\0';
 277
 278         nmea_process(*a);
 279
 280         ustream_consume(s, eol - *a);
 281         *a = eol;
 282
 283         return 0;
 284 }
 285
 286 static void
 287 nmea_msg_cb(struct ustream *s, int bytes)
 288 {
 289         int len;
 290         char *a = ustream_get_read_buf(s, &len);
 291
 292         while (!nmea_consume(s, &a))
 293                 ;
 294 }
 295
 296 static void nmea_notify_cb(struct ustream *s)
 297 {
 298         if (!s->eof)
 299                 return;
 300
 301         ERROR("tty error, shutting down\n");
 302         exit(-1);
 303 }
 304
 305 int
 306 nmea_open(char *dev, struct ustream_fd *s, speed_t speed)
 307 {
 308         struct termios tio;
 309         int tty;
 310
 311         tty = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
 312         if (tty < 0) {
 313                 ERROR("%s: device open failed: %s\n", dev, strerror(errno));
 314                 return -1;
 315         }
 316
 317         tcgetattr(tty, &tio);
 318         tio.c_cflag |= CREAD;
 319         tio.c_cflag |= CS8;
 320         tio.c_iflag |= IGNPAR;
 321         tio.c_lflag &= ~(ICANON);
 322         tio.c_lflag &= ~(ECHO);
 323         tio.c_lflag &= ~(ECHOE);
 324         tio.c_lflag &= ~(ISIG);
 325         tio.c_cc[VMIN] = 1;
 326         tio.c_cc[VTIME] = 0;
 327         cfsetispeed(&tio, speed);
 328         cfsetospeed(&tio, speed);
 329         tcsetattr(tty, TCSANOW, &tio);
 330
 331         s->stream.string_data = true;
 332         s->stream.notify_read = nmea_msg_cb;
 333         s->stream.notify_state = nmea_notify_cb;
 334
 335         ustream_fd_init(s, tty);
 336
 337         tcflush(tty, TCIFLUSH);
 338
 339         return 0;
 340 }