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