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 #include <osmocore/utils.h>
  31
  32 #include <osmocom/bb/common/osmocom_data.h>
  33 #include <osmocom/bb/common/logging.h>
  34 #include <osmocom/bb/common/gps.h>
  35
  36 struct osmo_gps gps = {
  37         0,
  38         "/dev/ttyACM0",
  39         0,
  40
  41         0,
  42         0,
  43         0,0,
  44 };
  45
  46 static struct bsc_fd gps_bfd;
  47 static struct termios gps_termios, gps_old_termios;
  48
  49 static int osmo_gps_line(char *line)
  50 {
  51         time_t gps_now, host_now;
  52         struct tm *tm;
  53         int32_t diff;
  54         double latitude, longitude;
  55
  56         if (!!strncmp(line, "$GPGLL", 6))
  57                 return 0;
  58         line += 7;
  59         if (strlen(line) < 37)
  60                 return 0;
  61         line[37] = '\0';
  62         /* ddmm.mmmm,N,dddmm.mmmm,E,hhmmss.mmm,A */
  63
  64         /* valid position */
  65         if (line[36] != 'A') {
  66                 LOGP(DGPS, LOGL_INFO, "%s (invalid)\n", line);
  67                 gps.valid = 0;
  68                 return 0;
  69         }
  70         gps.valid = 1;
  71
  72         /* time stamp */
  73         gps_now = line[30] - '0';
  74         gps_now += (line[29] - '0') * 10;
  75         gps_now += (line[28] - '0') * 60;
  76         gps_now += (line[27] - '0') * 600;
  77         gps_now += (line[26] - '0') * 3600;
  78         gps_now += (line[25] - '0') * 36000;
  79         time(&host_now);
  80         /* calculate the number of seconds the host differs from GPS */
  81         diff = host_now % 86400 - gps_now;
  82         if (diff < 0)
  83                 diff += 86400;
  84         if (diff >= 43200)
  85                 diff -= 86400;
  86         /* apply the "date" part to the GPS time */
  87         gps_now = host_now - diff;
  88         gps.gmt = gps_now;
  89         tm = localtime(&gps_now);
  90
  91         /* position */
  92         latitude = (double)(line[0] - '0') * 10.0;
  93         latitude += (double)(line[1] - '0');
  94         latitude += (double)(line[2] - '0') / 6.0;
  95         latitude += (double)(line[3] - '0') / 60.0;
  96         latitude += (double)(line[5] - '0') / 600.0;
  97         latitude += (double)(line[6] - '0') / 6000.0;
  98         latitude += (double)(line[7] - '0') / 60000.0;
  99         latitude += (double)(line[8] - '0') / 600000.0;
 100         if (line[10] == 'S')
 101                 latitude = 0.0 - latitude;
 102         gps.latitude = latitude;
 103         longitude = (double)(line[12] - '0') * 100.0;
 104         longitude += (double)(line[13] - '0') * 10.0;
 105         longitude += (double)(line[14] - '0');
 106         longitude += (double)(line[15] - '0') / 6.0;
 107         longitude += (double)(line[16] - '0') / 60.0;
 108         longitude += (double)(line[18] - '0') / 600.0;
 109         longitude += (double)(line[19] - '0') / 6000.0;
 110         longitude += (double)(line[20] - '0') / 60000.0;
 111         longitude += (double)(line[21] - '0') / 600000.0;
 112         if (line[23] == 'W')
 113                 longitude = 360.0 - longitude;
 114         gps.longitude = longitude;
 115
 116         LOGP(DGPS, LOGL_DEBUG, "%s\n", line);
 117         LOGP(DGPS, LOGL_INFO, " time=%02d:%02d:%02d %04d-%02d-%02d, "
 118                 "diff-to-host=%d, latitude=%do%.4f, longitude=%do%.4f\n",
 119                 tm->tm_hour, tm->tm_min, tm->tm_sec, tm->tm_year + 1900,
 120                 tm->tm_mday, tm->tm_mon + 1, diff,
 121                 (int)gps.latitude,
 122                 (gps.latitude - ((int)gps.latitude)) * 60.0,
 123                 (int)gps.longitude,
 124                 (gps.longitude - ((int)gps.longitude)) * 60.0);
 125         return 0;
 126 }
 127
 128 static int nmea_checksum(char *line)
 129 {
 130         uint8_t checksum = 0;
 131
 132         while (*line) {
 133                 if (*line == '$') {
 134                         line++;
 135                         continue;
 136                 }
 137                 if (*line == '*')
 138                         break;
 139                 checksum ^= *line++;
 140         }
 141         return (strtoul(line+1, NULL, 16) == checksum);
 142 }
 143
 144 int osmo_gps_cb(struct bsc_fd *bfd, unsigned int what)
 145 {
 146         char buff[128];
 147         static char line[128];
 148         static int lpos = 0;
 149         int i = 0, len;
 150
 151         len = read(bfd->fd, buff, sizeof(buff));
 152         if (len <= 0) {
 153                 fprintf(stderr, "error reading GPS device (errno=%d)\n", errno);
 154                 return len;
 155         }
 156         while(i < len) {
 157                 if (buff[i] == 13) {
 158                         i++;
 159                         continue;
 160                 }
 161                 if (buff[i] == 10) {
 162                         line[lpos] = '\0';
 163                         lpos = 0;
 164                         i++;
 165                         if (!nmea_checksum(line))
 166                                 fprintf(stderr, "NMEA checksum error\n");
 167                         else
 168                                 osmo_gps_line(line);
 169                         continue;
 170                 }
 171                 line[lpos++] = buff[i++];
 172                 if (lpos == sizeof(line))
 173                         lpos--;
 174         }
 175
 176         return 0;
 177 }
 178
 179 int osmo_gps_open(void)
 180 {
 181         int baud = 0;
 182
 183         if (gps_bfd.fd > 0)
 184                 return 0;
 185
 186         LOGP(DGPS, LOGL_INFO, "Open GPS device '%s'\n", gps.device);
 187
 188         gps_bfd.data = NULL;
 189         gps_bfd.when = BSC_FD_READ;
 190         gps_bfd.cb = osmo_gps_cb;
 191         gps_bfd.fd = open(gps.device, O_RDONLY);
 192         if (gps_bfd.fd < 0)
 193                 return gps_bfd.fd;
 194
 195         switch (gps.baud) {
 196         case   4800:
 197                 baud = B4800;      break;
 198         case   9600:
 199                 baud = B9600;      break;
 200         case  19200:
 201                 baud = B19200;     break;
 202         case  38400:
 203                 baud = B38400;     break;
 204         case  57600:
 205                 baud = B57600;     break;
 206         case 115200:
 207                 baud = B115200;    break;
 208         }
 209
 210         if (isatty(gps_bfd.fd))
 211         {
 212                 /* get termios */
 213                 tcgetattr(gps_bfd.fd, &gps_old_termios);
 214                 tcgetattr(gps_bfd.fd, &gps_termios);
 215                 /* set baud */
 216                 if (baud) {
 217                         gps_termios.c_cflag |= baud;
 218                         cfsetispeed(&gps_termios, baud);
 219                         cfsetospeed(&gps_termios, baud);
 220                 }
 221                 if (tcsetattr(gps_bfd.fd, TCSANOW, &gps_termios))
 222                         printf("Failed to set termios for GPS\n");
 223         }
 224
 225         bsc_register_fd(&gps_bfd);
 226
 227         return 0;
 228 }
 229
 230 void osmo_gps_close(void)
 231 {
 232         if (gps_bfd.fd <= 0)
 233                 return;
 234
 235         LOGP(DGPS, LOGL_INFO, "Close GPS device\n");
 236
 237         bsc_unregister_fd(&gps_bfd);
 238
 239         if (isatty(gps_bfd.fd))
 240                 tcsetattr(gps_bfd.fd, TCSANOW, &gps_old_termios);
 241
 242         close(gps_bfd.fd);
 243         gps_bfd.fd = -1; /* -1 or 0 indicates: 'close' */
 244 }
 245
 246 void osmo_gps_init(void)
 247 {
 248         memset(&gps_bfd, 0, sizeof(gps_bfd));
 249 }
 250
 251