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