src/host/layer23/src/common/gps.c

   1 /*
   2  * (C) 2010 by Andreas Eversberg <jolly@eversberg.eu>
   3  *
   4  * All Rights Reserved
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation; either version 2 of the License, or
   9  * (at your option) any later version.
  10  *
  11  * This program is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  * GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public License along
  17  * with this program; if not, write to the Free Software Foundation, Inc.,
  18  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  19  *
  20  */
  21
  22 #include <stdio.h>
  23 #include <sys/file.h>
  24 #include <termios.h>
  25 #include <unistd.h>
  26 #include <stdlib.h>
  27 #include <errno.h>
  28 #include <time.h>
  29 #include <stdbool.h>
  30
  31 #ifdef _HAVE_GPSD
  32 #include <gps.h>
  33 #endif
  34
  35 #include <osmocom/core/utils.h>
  36
  37 #include <osmocom/bb/common/osmocom_data.h>
  38 #include <osmocom/bb/common/logging.h>
  39 #include <osmocom/bb/common/gps.h>
  40
  41 struct osmo_gps g = {
  42         0,
  43         GPS_TYPE_UNDEF,
  44 #ifdef _HAVE_GPSD
  45     "localhost",
  46         "2947",
  47 #endif
  48         "/dev/ttyACM0",
  49         0,
  50         0,
  51         0,
  52         0,0
  53 };
  54
  55 static struct bsc_fd gps_bfd;
  56
  57 #ifdef _HAVE_GPSD
  58
  59 static struct gps_data_t* gdata;
  60
  61 int osmo_gpsd_cb(struct bsc_fd *bfd, unsigned int what)
  62 {
  63         struct tm *tm;
  64         unsigned diff = 0;
  65
  66         g.valid = 0;
  67
  68         /* gps is offline */
  69         if (gdata->online)
  70             goto gps_not_ready;
  71
  72         /* gps has no data */
  73         if (gps_waiting(gdata))
  74             goto gps_not_ready;
  75
  76         /* polling returned an error */
  77         if (gps_poll(gdata))
  78             goto gps_not_ready;
  79
  80         /* data are valid */
  81         if (gdata->set & LATLON_SET) {
  82                 g.valid = 1;
  83                 g.gmt = gdata->fix.time;
  84                 tm = localtime(&g.gmt);
  85                 diff = time(NULL) - g.gmt;
  86                 g.latitude = gdata->fix.latitude;
  87                 g.longitude = gdata->fix.longitude;
  88
  89                 LOGP(DGPS, LOGL_INFO, " time=%02d:%02d:%02d %04d-%02d-%02d, "
  90                         "diff-to-host=%d, latitude=%do%.4f, longitude=%do%.4f\n",
  91                         tm->tm_hour, tm->tm_min, tm->tm_sec, tm->tm_year + 1900,
  92                         tm->tm_mday, tm->tm_mon + 1, diff,
  93                         (int)g.latitude,
  94                         (g.latitude - ((int)g.latitude)) * 60.0,
  95                         (int)g.longitude,
  96                         (g.longitude - ((int)g.longitude)) * 60.0);
  97         }
  98
  99         return 0;
 100
 101 gps_not_ready:
 102         LOGP(DGPS, LOGL_DEBUG, "gps is offline");
 103         return -1;
 104 }
 105
 106 int osmo_gpsd_open(void)
 107 {
 108         LOGP(DGPS, LOGL_INFO, "Connecting to gpsd at '%s:%s'\n", g.gpsd_host, g.gpsd_port);
 109
 110         gps_bfd.data = NULL;
 111         gps_bfd.when = BSC_FD_READ;
 112         gps_bfd.cb = osmo_gpsd_cb;
 113
 114         gdata = gps_open(g.gpsd_host, g.gpsd_port);
 115         if (gdata == NULL) {
 116                 LOGP(DGPS, LOGL_ERROR, "Can't connect to gpsd\n");
 117                 return -1;
 118         }
 119         gps_bfd.fd = gdata->gps_fd;
 120         if (gps_bfd.fd < 0)
 121                 return gps_bfd.fd;
 122
 123         if (gps_stream(gdata, WATCH_ENABLE, NULL) == -1) {
 124                 LOGP(DGPS, LOGL_ERROR, "Error in gps_stream()\n");
 125                 return -1;
 126         }
 127
 128         bsc_register_fd(&gps_bfd);
 129
 130         return 0;
 131 }
 132
 133 void osmo_gpsd_close(void)
 134 {
 135         if (gps_bfd.fd <= 0)
 136                 return;
 137
 138         LOGP(DGPS, LOGL_INFO, "Disconnecting from gpsd\n");
 139
 140         bsc_unregister_fd(&gps_bfd);
 141
 142         gps_close(gdata);
 143         gps_bfd.fd = -1; /* -1 or 0 indicates: 'close' */
 144 }
 145
 146 #endif
 147
 148 static struct termios gps_termios, gps_old_termios;
 149
 150 static int osmo_serialgps_line(char *line)
 151 {
 152         time_t gps_now, host_now;
 153         struct tm *tm;
 154         int32_t diff;
 155         double latitude, longitude;
 156
 157         if (!!strncmp(line, "$GPGLL", 6))
 158                 return 0;
 159         line += 7;
 160         if (strlen(line) < 37)
 161                 return 0;
 162         line[37] = '\0';
 163         /* ddmm.mmmm,N,dddmm.mmmm,E,hhmmss.mmm,A */
 164
 165         /* valid position */
 166         if (line[36] != 'A') {
 167                 LOGP(DGPS, LOGL_INFO, "%s (invalid)\n", line);
 168                 g.valid = 0;
 169                 return 0;
 170         }
 171         g.valid = 1;
 172
 173         /* time stamp */
 174         gps_now = line[30] - '0';
 175         gps_now += (line[29] - '0') * 10;
 176         gps_now += (line[28] - '0') * 60;
 177         gps_now += (line[27] - '0') * 600;
 178         gps_now += (line[26] - '0') * 3600;
 179         gps_now += (line[25] - '0') * 36000;
 180         time(&host_now);
 181         /* calculate the number of seconds the host differs from GPS */
 182         diff = host_now % 86400 - gps_now;
 183         if (diff < 0)
 184                 diff += 86400;
 185         if (diff >= 43200)
 186                 diff -= 86400;
 187         /* apply the "date" part to the GPS time */
 188         gps_now = host_now - diff;
 189         g.gmt = gps_now;
 190         tm = localtime(&gps_now);
 191
 192         /* position */
 193         latitude = (double)(line[0] - '0') * 10.0;
 194         latitude += (double)(line[1] - '0');
 195         latitude += (double)(line[2] - '0') / 6.0;
 196         latitude += (double)(line[3] - '0') / 60.0;
 197         latitude += (double)(line[5] - '0') / 600.0;
 198         latitude += (double)(line[6] - '0') / 6000.0;
 199         latitude += (double)(line[7] - '0') / 60000.0;
 200         latitude += (double)(line[8] - '0') / 600000.0;
 201         if (line[10] == 'S')
 202                 latitude = 0.0 - latitude;
 203         g.latitude = latitude;
 204         longitude = (double)(line[12] - '0') * 100.0;
 205         longitude += (double)(line[13] - '0') * 10.0;
 206         longitude += (double)(line[14] - '0');
 207         longitude += (double)(line[15] - '0') / 6.0;
 208         longitude += (double)(line[16] - '0') / 60.0;
 209         longitude += (double)(line[18] - '0') / 600.0;
 210         longitude += (double)(line[19] - '0') / 6000.0;
 211         longitude += (double)(line[20] - '0') / 60000.0;
 212         longitude += (double)(line[21] - '0') / 600000.0;
 213         if (line[23] == 'W')
 214                 longitude = 360.0 - longitude;
 215         g.longitude = longitude;
 216
 217         LOGP(DGPS, LOGL_DEBUG, "%s\n", line);
 218         LOGP(DGPS, LOGL_INFO, " time=%02d:%02d:%02d %04d-%02d-%02d, "
 219                 "diff-to-host=%d, latitude=%do%.4f, longitude=%do%.4f\n",
 220                 tm->tm_hour, tm->tm_min, tm->tm_sec, tm->tm_year + 1900,
 221                 tm->tm_mday, tm->tm_mon + 1, diff,
 222                 (int)g.latitude,
 223                 (g.latitude - ((int)g.latitude)) * 60.0,
 224                 (int)g.longitude,
 225                 (g.longitude - ((int)g.longitude)) * 60.0);
 226         return 0;
 227 }
 228
 229 static int nmea_checksum(char *line)
 230 {
 231         uint8_t checksum = 0;
 232
 233         while (*line) {
 234                 if (*line == '$') {
 235                         line++;
 236                         continue;
 237                 }
 238                 if (*line == '*')
 239                         break;
 240                 checksum ^= *line++;
 241         }
 242         return (strtoul(line+1, NULL, 16) == checksum);
 243 }
 244
 245 int osmo_serialgps_cb(struct bsc_fd *bfd, unsigned int what)
 246 {
 247         char buff[128];
 248         static char line[128];
 249         static int lpos = 0;
 250         int i = 0, len;
 251
 252         len = read(bfd->fd, buff, sizeof(buff));
 253         if (len <= 0) {
 254                 fprintf(stderr, "error reading GPS device (errno=%d)\n", errno);
 255                 return len;
 256         }
 257         while(i < len) {
 258                 if (buff[i] == 13) {
 259                         i++;
 260                         continue;
 261                 }
 262                 if (buff[i] == 10) {
 263                         line[lpos] = '\0';
 264                         lpos = 0;
 265                         i++;
 266                         if (!nmea_checksum(line))
 267                                 fprintf(stderr, "NMEA checksum error\n");
 268                         else
 269                                 osmo_serialgps_line(line);
 270                         continue;
 271                 }
 272                 line[lpos++] = buff[i++];
 273                 if (lpos == sizeof(line))
 274                         lpos--;
 275         }
 276
 277         return 0;
 278 }
 279
 280 int osmo_serialgps_open(void)
 281 {
 282         int baud = 0;
 283
 284         if (gps_bfd.fd > 0)
 285                 return 0;
 286
 287         LOGP(DGPS, LOGL_INFO, "Open GPS device '%s'\n", g.device);
 288
 289         gps_bfd.data = NULL;
 290         gps_bfd.when = BSC_FD_READ;
 291         gps_bfd.cb = osmo_serialgps_cb;
 292         gps_bfd.fd = open(g.device, O_RDONLY);
 293         if (gps_bfd.fd < 0)
 294                 return gps_bfd.fd;
 295
 296         switch (g.baud) {
 297         case   4800:
 298                 baud = B4800;      break;
 299         case   9600:
 300                 baud = B9600;      break;
 301         case  19200:
 302                 baud = B19200;     break;
 303         case  38400:
 304                 baud = B38400;     break;
 305         case  57600:
 306                 baud = B57600;     break;
 307         case 115200:
 308                 baud = B115200;    break;
 309         }
 310
 311         if (isatty(gps_bfd.fd))
 312         {
 313                 /* get termios */
 314                 tcgetattr(gps_bfd.fd, &gps_old_termios);
 315                 tcgetattr(gps_bfd.fd, &gps_termios);
 316                 /* set baud */
 317                 if (baud) {
 318                         gps_termios.c_cflag |= baud;
 319                         cfsetispeed(&gps_termios, baud);
 320                         cfsetospeed(&gps_termios, baud);
 321                 }
 322                 if (tcsetattr(gps_bfd.fd, TCSANOW, &gps_termios))
 323                         printf("Failed to set termios for GPS\n");
 324         }
 325
 326         bsc_register_fd(&gps_bfd);
 327
 328         return 0;
 329 }
 330
 331 void osmo_serialgps_close(void)
 332 {
 333         if (gps_bfd.fd <= 0)
 334                 return;
 335
 336         LOGP(DGPS, LOGL_INFO, "Close GPS device\n");
 337
 338         bsc_unregister_fd(&gps_bfd);
 339
 340         if (isatty(gps_bfd.fd))
 341                 tcsetattr(gps_bfd.fd, TCSANOW, &gps_old_termios);
 342
 343         close(gps_bfd.fd);
 344         gps_bfd.fd = -1; /* -1 or 0 indicates: 'close' */
 345 }
 346
 347 void osmo_gps_init(void)
 348 {
 349         memset(&gps_bfd, 0, sizeof(gps_bfd));
 350 }
 351
 352 int osmo_gps_open(void)
 353 {
 354         switch (g.gps_type) {
 355 #ifdef _HAVE_GPSD
 356                 case GPS_TYPE_GPSD:
 357                         return osmo_gpsd_open();
 358 #endif
 359                 case GPS_TYPE_SERIAL:
 360                         return osmo_serialgps_open();
 361
 362                 default:
 363                         return 0;
 364         }
 365 }
 366
 367 void osmo_gps_close(void)
 368 {
 369         switch (g.gps_type) {
 370 #ifdef _HAVE_GPSD
 371                 case GPS_TYPE_GPSD:
 372                         return osmo_gpsd_close();
 373 #endif
 374                 case GPS_TYPE_SERIAL:
 375                         return osmo_serialgps_close();
 376
 377                 default:
 378                         return;
 379         }
 380 }
 381