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 _BSD_SOURCE
  20 #define _XOPEN_SOURCE
  21 #include <time.h>
  22
  23 #include <sys/types.h>
  24 #include <sys/stat.h>
  25 #include <sys/ioctl.h>
  26 #include <sys/time.h>
  27
  28 #include <fcntl.h>
  29 #include <time.h>
  30 #include <stdlib.h>
  31 #include <stdio.h>
  32 #include <unistd.h>
  33 #include <errno.h>
  34
  35 #include <string.h>
  36 #include <termios.h>
  37
  38 #include <libubox/utils.h>
  39
  40 #include "log.h"
  41 #include "nmea.h"
  42
  43 #define MAX_NMEA_PARAM  20
  44 #define MAX_TIME_OFFSET 2
  45 #define MAX_BAD_TIME    3
  46
  47 struct nmea_param {
  48         char *str;
  49         int num;
  50 } nmea_params[MAX_NMEA_PARAM];
  51
  52 static int nmea_bad_time;
  53 char longitude[32] = { 0 }, latitude[32] = { 0 }, course[16] = { 0 }, speed[16] = { 0 }, elivation[16] = { 0 };
  54 int gps_valid = 0;
  55
  56 static void
  57 nmea_txt_cb(void)
  58 {
  59         char *ids[] = { "ERROR", "WARNING", "NOTICE", };
  60
  61         if (nmea_params[3].num < 0 || nmea_params[3].num > 2)
  62                 nmea_params[3].num = 0;
  63
  64         DEBUG(3, "%s: %s\n", ids[nmea_params[3].num], nmea_params[4].str);
  65 }
  66
  67 static void
  68 nmea_rmc_cb(void)
  69 {
  70         struct tm tm;
  71         char tmp[256];
  72
  73         if (*nmea_params[2].str != 'A') {
  74                 gps_valid = 0;
  75                 DEBUG(4, "waiting for valid signal\n");
  76                 return;
  77         }
  78
  79         gps_valid = 1;
  80         memset(&tm, 0, sizeof(tm));
  81         tm.tm_isdst = 1;
  82
  83         if (!strptime(nmea_params[1].str, "%H%M%S", &tm))
  84                 ERROR("failed to parse time\n");
  85         else if (!strptime(nmea_params[9].str, "%d%m%y", &tm))
  86                 ERROR("failed to parse date\n");
  87         else {
  88                 /* is there a libc api for the tz adjustment ? */
  89                 struct timeval tv = { mktime(&tm), 0 };
  90                 struct timeval cur;
  91
  92                 strftime(tmp, 256, "%D %02H:%02M:%02S", &tm);
  93                 DEBUG(3, "date: %s UTC\n", tmp);
  94
  95                 tv.tv_sec -= timezone;
  96                 if (daylight)
  97                         tv.tv_sec += 3600;
  98
  99                 gettimeofday(&cur, NULL);
 100
 101                 if (abs(cur.tv_sec - tv.tv_sec) > MAX_TIME_OFFSET) {
 102                         if (++nmea_bad_time > MAX_BAD_TIME) {
 103                                 LOG("system time differs from GPS time by more than %d seconds. Using %s UTC as the new time\n", MAX_TIME_OFFSET, tmp);
 104                                 settimeofday(&tv, NULL);
 105                         }
 106                 } else {
 107                         nmea_bad_time = 0;
 108                 }
 109         }
 110
 111         if (strlen(nmea_params[3].str) < 9 || strlen(nmea_params[5].str) < 10) {
 112                 ERROR("lat/lng have invalid string length %d<9, %d<10\n",
 113                        strlen(nmea_params[3].str), strlen(nmea_params[5].str));
 114         } else {
 115                 int latd, latm, lats;
 116                 int lngd, lngm, lngs;
 117                 float flats, flngs;
 118                 DEBUG(4, "position: %s, %s\n",
 119                         nmea_params[3].str, nmea_params[5].str);
 120                 latm = atoi(&nmea_params[3].str[2]);
 121                 nmea_params[3].str[2] = '\0';
 122                 latd = atoi(nmea_params[3].str);
 123                 lats = atoi(&nmea_params[3].str[5]);
 124                 if (*nmea_params[4].str != 'N')
 125                         latm *= -1;
 126
 127                 lngm = atoi(&nmea_params[5].str[3]);
 128                 nmea_params[5].str[3] = '\0';
 129                 lngd = atoi(nmea_params[5].str);
 130                 lngs = atoi(&nmea_params[5].str[6]);
 131                 if (*nmea_params[6].str != 'E')
 132                         lngm *= -1;
 133
 134                 flats = lats;
 135                 flats *= 60;
 136                 flats /= 10000;
 137
 138                 flngs = lngs;
 139                 flngs *= 60;
 140                 flngs /= 10000;
 141
 142 #define ms_to_deg(x, y) (((x * 10000) + y) / 60)
 143
 144                 DEBUG(4, "position: %d°%d.%04d, %d°%d.%04d\n",
 145                         latd, latm, lats, lngd, lngm, lngs);
 146                 DEBUG(4, "position: %d°%d'%.1f\" %d°%d'%.1f\"\n",
 147                         latd, latm, flats, lngd, lngm, flngs);
 148
 149                 snprintf(latitude, sizeof(latitude), "%d.%04d", latd, ms_to_deg(latm, lats));
 150                 snprintf(longitude, sizeof(longitude), "%d.%04d", lngd, ms_to_deg(lngm, lngs));
 151                 DEBUG(3, "position: %s %s\n", latitude, longitude);
 152                 gps_timestamp();
 153         }
 154 }
 155
 156 static void
 157 nmea_gga_cb(void)
 158 {
 159         if (!gps_valid)
 160                 return;
 161         strncpy(elivation, nmea_params[9].str, sizeof(elivation));
 162         DEBUG(4, "height: %s\n", elivation);
 163 }
 164
 165 static void
 166 nmea_vtg_cb(void)
 167 {
 168         if (!gps_valid)
 169                 return;
 170         strncpy(course, nmea_params[1].str, sizeof(course));
 171         strncpy(speed, nmea_params[6].str, sizeof(speed));
 172         DEBUG(4, "course: %s\n", course);
 173         DEBUG(4, "speed: %s\n", speed);
 174 }
 175
 176 static struct nmea_msg {
 177         char *msg;
 178         int cnt;
 179         void (*handler) (void);
 180 } nmea_msgs[] = {
 181         {
 182                 .msg = "TXT",
 183                 .cnt = 5,
 184                 .handler = nmea_txt_cb,
 185         }, {
 186                 .msg = "RMC",
 187                 .cnt = 11,
 188                 .handler = nmea_rmc_cb,
 189         }, {
 190                 .msg = "GGA",
 191                 .cnt = 14,
 192                 .handler = nmea_gga_cb,
 193         }, {
 194                 .msg = "VTG",
 195                 .cnt = 9,
 196                 .handler = nmea_vtg_cb,
 197         },
 198 };
 199
 200 static int
 201 nmea_verify_checksum(char *s)
 202 {
 203         char *csum = strrchr(s, '*');
 204         int isum, c = 0;
 205
 206         if (!csum)
 207                 return -1;
 208
 209         *csum = '\0';
 210         csum++;
 211         isum = strtol(csum, NULL, 16);
 212
 213         while(*s)
 214                 c ^= *s++;
 215
 216         if (isum != c)
 217                 return -1;
 218
 219         return 0;
 220 }
 221
 222 static int
 223 nmea_tokenize(char *msg)
 224 {
 225         int cnt = 0;
 226         char *tok = strsep(&msg, ",");
 227
 228         while (tok && cnt < MAX_NMEA_PARAM) {
 229                 nmea_params[cnt].str = tok;
 230                 nmea_params[cnt].num = atoi(tok);
 231                 cnt++;
 232                 tok = strsep(&msg, ",");
 233         }
 234
 235         return cnt;
 236 }
 237
 238 static void
 239 nmea_process(char *a)
 240 {
 241         char *csum;
 242         int cnt, i;
 243
 244         if (strncmp(a, "$GP", 3))
 245                 return;
 246
 247         a++;
 248         csum = strrchr(a, '*');
 249         if (!csum)
 250                 return;
 251
 252         if (nmea_verify_checksum(a)) {
 253                 ERROR("nmea message has invalid checksum\n");
 254                 return;
 255         }
 256
 257         cnt = nmea_tokenize(&a[2]);
 258         if (cnt < 0) {
 259                 ERROR("failed to tokenize %s\n", a);\
 260                 return;
 261         }
 262
 263         for (i = 0; i < ARRAY_SIZE(nmea_msgs); i++) {
 264                 if (strcmp(nmea_params[0].str, nmea_msgs[i].msg))
 265                         continue;
 266                 if (nmea_msgs[i].cnt <= cnt)
 267                         nmea_msgs[i].handler();
 268                 else
 269                         ERROR("%s datagram has wrong parameter count got %d but expected %d\n", nmea_msgs[i].msg, cnt, nmea_msgs[i].cnt);
 270                 return;
 271         }
 272 }
 273
 274 static int
 275 nmea_consume(struct ustream *s, char **a)
 276 {
 277         char *eol = strstr(*a, "\n");
 278
 279         if (!eol)
 280                 return -1;
 281
 282         *eol++ = '\0';
 283
 284         nmea_process(*a);
 285
 286         ustream_consume(s, eol - *a);
 287         *a = eol;
 288
 289         return 0;
 290 }
 291
 292 static void
 293 nmea_msg_cb(struct ustream *s, int bytes)
 294 {
 295         int len;
 296         char *a = ustream_get_read_buf(s, &len);
 297
 298         while (!nmea_consume(s, &a))
 299                 ;
 300 }
 301
 302 static void nmea_notify_cb(struct ustream *s)
 303 {
 304         if (!s->eof)
 305                 return;
 306
 307         ERROR("tty error, shutting down\n");
 308         exit(-1);
 309 }
 310
 311 int
 312 nmea_open(char *dev, struct ustream_fd *s, speed_t speed)
 313 {
 314         struct termios tio;
 315         int tty;
 316
 317         tty = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
 318         if (tty < 0) {
 319                 ERROR("%s: device open failed: %s\n", dev, strerror(errno));
 320                 return -1;
 321         }
 322
 323         tcgetattr(tty, &tio);
 324         tio.c_cflag |= CREAD;
 325         tio.c_cflag |= CS8;
 326         tio.c_iflag |= IGNPAR;
 327         tio.c_lflag &= ~(ICANON);
 328         tio.c_lflag &= ~(ECHO);
 329         tio.c_lflag &= ~(ECHOE);
 330         tio.c_lflag &= ~(ISIG);
 331         tio.c_cc[VMIN] = 1;
 332         tio.c_cc[VTIME] = 0;
 333         cfsetispeed(&tio, speed);
 334         cfsetospeed(&tio, speed);
 335         tcsetattr(tty, TCSANOW, &tio);
 336
 337         s->stream.string_data = true;
 338         s->stream.notify_read = nmea_msg_cb;
 339         s->stream.notify_state = nmea_notify_cb;
 340
 341         ustream_fd_init(s, tty);
 342
 343         tcflush(tty, TCIFLUSH);
 344
 345         return 0;
 346 }