err.no Git - mapper/blob - src/gps.c

   1 /*
   2  * This file is part of mapper
   3  *
   4  * Copyright (C) 2007 Kaj-Michael Lang
   5  * Copyright (C) 2006-2007 John Costigan.
   6  *
   7  * POI and GPS-Info code originally written by Cezary Jackiewicz.
   8  *
   9  * Default map data provided by http://www.openstreetmap.org/
  10  *
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License as published by
  13  * the Free Software Foundation; either version 2 of the License, or
  14  * (at your option) any later version.
  15  *
  16  * This program is distributed in the hope that it will be useful,
  17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  19  * GNU General Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License along
  22  * with this program; if not, write to the Free Software Foundation, Inc.,
  23  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  24  */
  25
  26 #include "config.h"
  27
  28 #include <unistd.h>
  29 #include <stdlib.h>
  30 #include <string.h>
  31 #include <strings.h>
  32 #include <stddef.h>
  33 #include <libintl.h>
  34 #include <locale.h>
  35 #include <math.h>
  36 #include <errno.h>
  37 #include <sys/wait.h>
  38 #include <glib/gstdio.h>
  39 #include <fcntl.h>
  40
  41 #include "gps.h"
  42 #include "latlon.h"
  43 #include "map.h"
  44 #include "gps-nmea-parse.h"
  45 #include "gps-conn.h"
  46 #include "track.h"
  47
  48 #ifdef WITH_GPSBT
  49 #include <gpsbt.h>
  50 static gpsbt_t ctx = {0};
  51 #endif
  52
  53 static gboolean gps_channel_cb_error(GIOChannel *src, GIOCondition condition, gpointer data);
  54 static gboolean gps_channel_cb_input(GIOChannel *src, GIOCondition condition, gpointer data);
  55 static gboolean gps_channel_cb_connect(GIOChannel * src, GIOCondition condition, gpointer data);
  56 static gboolean gps_connect_socket(Gps *gps);
  57 static gboolean gps_connect_file(Gps *gps);
  58
  59 #define GPSD_NMEA "r+\r\n"
  60 #define GPSD_PORT (2947)
  61
  62 /**
  63  * Init GPS structure
  64  */
  65 Gps *
  66 gps_new(GpsIOSourceType type)
  67 {
  68 Gps *gps;
  69
  70 gps=g_slice_new0(Gps);
  71 gps->data.fix=FIX_NOFIX;
  72 gps->io.fd=-1;
  73 gps->io.address=NULL;
  74 gps->io.type=type;
  75 gps->io.conn=RCVR_OFF;
  76 gps->data.lat=64.20;
  77 gps->data.lon=22.20;
  78 gps_data_integerize(&gps->data);
  79 return gps;
  80 }
  81
  82 void
  83 gps_set_address(Gps *gps, gchar *address, guint port)
  84 {
  85 if (gps->io.address)
  86         g_free(gps->io.address);
  87 if (address)
  88         gps->io.address=g_strdup(address);
  89 gps->io.port=port;
  90 }
  91
  92 void
  93 gps_set_type(Gps *gps, GpsIOSourceType type)
  94 {
  95 gps->io.type=type;
  96 }
  97
  98 void
  99 gps_free(Gps *gps)
 100 {
 101 gps_disconnect(gps);
 102 if (gps->io.address)
 103         g_free(gps->io.address);
 104 g_slice_free(Gps, gps);
 105 }
 106
 107 /**
 108  * Convert the float lat/lon/speed/heading data into integer units.
 109  * Also calculate offsets.
 110  */
 111 void
 112 gps_data_integerize(GpsData *gpsdata)
 113 {
 114 gdouble tmp;
 115
 116 tmp=(gpsdata->heading*(1.f/180.f))*G_PI;
 117 latlon2unit(gpsdata->lat, gpsdata->lon, gpsdata->unitx, gpsdata->unity);
 118 gpsdata->vel_offsetx=(gint)(floor(gpsdata->speed*sin(tmp)+0.5f));
 119 gpsdata->vel_offsety=-(gint)(floor(gpsdata->speed*cos(tmp)+0.5f));
 120 }
 121
 122 /**
 123  * Place a request to connect about 1 second after the function is called.
 124  */
 125 void
 126 gps_connect_later(Gps *gps)
 127 {
 128 g_assert(gps);
 129 g_assert(gps->io.clater_sid!=0);
 130 gps->io.clater_sid=g_timeout_add(1000, (GSourceFunc)gps_connect_now, gps);
 131 }
 132
 133 gboolean
 134 gps_connect(Gps *gps)
 135 {
 136 switch (gps->io.type) {
 137         case GPS_IO_FILE:
 138                 return gps_connect_file(gps);
 139         break;
 140         case GPS_IO_RFCOMM:
 141         case GPS_IO_TCP:
 142         case GPS_IO_GPSD:
 143                 return gps_connect_socket(gps);
 144         break;
 145         case GPS_IO_GYPSY:
 146                 return FALSE;
 147         break;
 148         default:
 149         break;
 150 }
 151 return FALSE;
 152 }
 153
 154 /**
 155  * Helper to open a file or device node
 156  */
 157 static gboolean
 158 gps_connect_file(Gps *gps)
 159 {
 160 if (-1==(gps->io.fd=open(gps->io.address, O_RDONLY))) {
 161         gps_disconnect(gps);
 162         gps_connect_later(gps);
 163         return FALSE;
 164 }
 165 return TRUE;
 166 }
 167
 168 /**
 169  * Helper to connect to a socket file descriptor.
 170  *
 171  */
 172 static gboolean
 173 gps_connect_socket(Gps *gps)
 174 {
 175 gint r, e;
 176 switch (gps->io.type) {
 177 #ifdef WITH_BLUEZ
 178         case GPS_IO_RFCOMM:
 179                 r=connect(gps->io.fd, (struct sockaddr *)&gps->io.rcvr_addr_rc, sizeof(gps->io.rcvr_addr_rc));
 180         break;
 181 #endif
 182         case GPS_IO_TCP:
 183         case GPS_IO_GPSD:
 184                 r=connect(gps->io.fd, (struct sockaddr *)&gps->io.rcvr_addr_in, sizeof(gps->io.rcvr_addr_in));
 185         break;
 186         default:
 187                 return FALSE;
 188 }
 189 e=errno;
 190
 191 /* The socket is non blocking so handle it */
 192 if (r != 0) {
 193         switch (e) {
 194         case EAGAIN:
 195         case EBUSY:
 196         case EINPROGRESS:
 197         case EALREADY:
 198                 g_printf("*** Connection in progress... %d %d\n", e, r);
 199                 perror("INFO: ");
 200                 return TRUE;
 201         break;
 202         case EHOSTUNREACH:
 203                 g_printf("*** Bluetooth/GPS device not found.\n");
 204                 gps_disconnect(gps);
 205 #if 0
 206         set_action_activate("gps_enable", FALSE);
 207         popup_error(_window, _("No bluetooth or GPS device found."));
 208 #endif
 209                 return FALSE;
 210         break;
 211         default:
 212                 /* Connection failed.  Disconnect and try again later. */
 213                 g_printf("### Connect failed, retrying... %d %d\n", e, r);
 214                 perror("ERROR: ");
 215                 gps_disconnect(gps);
 216                 gps_connect_later(gps);
 217                 return FALSE;
 218         break;
 219         }
 220 }
 221 return TRUE;
 222 }
 223
 224 /**
 225  * Disconnect the GPS device.
 226  * - Add channel callbacks are removed
 227  * - Channel is close and shutdown
 228  * - File descriptor is closed
 229  * - Anything special for disconnecting is done
 230  */
 231 void
 232 gps_disconnect(Gps *gps)
 233 {
 234 g_assert(gps);
 235
 236 /* Remove watches. */
 237 if (gps->io.clater_sid) {
 238         g_source_remove(gps->io.clater_sid);
 239         gps->io.clater_sid=0;
 240 }
 241 if (gps->io.error_sid) {
 242         g_source_remove(gps->io.error_sid);
 243         gps->io.error_sid=0;
 244 }
 245 if (gps->io.connect_sid) {
 246         g_source_remove(gps->io.connect_sid);
 247         gps->io.connect_sid=0;
 248 }
 249 if (gps->io.input_sid) {
 250         g_source_remove(gps->io.input_sid);
 251         gps->io.input_sid=0;
 252 }
 253
 254 /* Destroy the GIOChannel object. */
 255 if (gps->io.channel) {
 256         g_io_channel_shutdown(gps->io.channel, FALSE, NULL);
 257         g_io_channel_unref(gps->io.channel);
 258         gps->io.channel=NULL;
 259 }
 260
 261 /* Close the file descriptor. */
 262 if (gps->io.fd!=-1) {
 263         close(gps->io.fd);
 264         gps->io.fd=-1;
 265 }
 266
 267 #ifdef WITH_GPSBT
 268 if (gps->io.type==GPS_IO_GPSD) {
 269         gint r;
 270         r=gpsbt_stop(&ctx);
 271         if (r!=0)
 272                 g_warning("Failed to stop gpsd\n");
 273 }
 274 #endif
 275
 276 #ifdef WITH_HILDON_DBUS_BT
 277 if (gps->io.type==GPS_IO_HILDON_DBUS && gps->io.rfcomm_req_proxy) {
 278                 GError *error = NULL;
 279
 280                 dbus_g_proxy_call(gps->io.rfcomm_req_proxy, BTCOND_RFCOMM_CANCEL_CONNECT_REQ, &error, G_TYPE_STRING, gps->io.address, G_TYPE_STRING, "SPP", G_TYPE_INVALID, G_TYPE_INVALID);
 281                 error = NULL;
 282                 dbus_g_proxy_call(gps->io.rfcomm_req_proxy,     BTCOND_RFCOMM_DISCONNECT_REQ, &error,G_TYPE_STRING, gps->io.address, G_TYPE_STRING, "SPP", G_TYPE_INVALID, G_TYPE_INVALID);
 283 }
 284 #endif
 285
 286 #ifdef WITH_BLUEZ_DBUS_BT
 287 if (gps->io.type==GPS_IO_BLUEZ_DBUS && gps->io.rfcomm_req_proxy) {
 288         GError *error = NULL;
 289
 290
 291 }
 292 #endif
 293
 294 }
 295
 296 static gboolean
 297 gps_simulate(Gps *gps)
 298 {
 299 g_assert(gps);
 300
 301 gps->data.lat+=g_random_double_range(-0.0002,0.0002);
 302 gps->data.lon+=g_random_double_range(-0.0002,0.0002);
 303 BOUND(gps->data.lat, -80.0, 80.0);
 304 BOUND(gps->data.lon, -80.0, 80.0);
 305 gps->data.speed=1+g_random_double_range(0.0,5.0);
 306 gps->data.heading+=g_random_double_range(-2.0,2.0);
 307 gps->data.time=time(NULL);
 308 gps_data_integerize(&gps->data);
 309 track_add(gps, FALSE);
 310 gps->data.lheading=gps->data.heading;
 311 map_refresh_mark();
 312
 313 return TRUE;
 314 }
 315
 316 /**
 317  * Reset GPS read buffer
 318  */
 319 static void
 320 gps_read_buffer_prepare(Gps *gps)
 321 {
 322 gps->io.curr=gps->io.buffer;
 323 *gps->io.curr = '\0';
 324 gps->io.last=gps->io.buffer+GPS_READ_BUF_SIZE-1;
 325 }
 326
 327 /**
 328  * Connect file descriptor to channel and add watches.
 329  */
 330 static void
 331 gps_read_channel_connect(Gps *gps)
 332 {
 333 gps->io.channel=g_io_channel_unix_new(gps->io.fd);
 334 g_io_channel_set_encoding(gps->io.channel, NULL, NULL);
 335 g_io_channel_set_flags(gps->io.channel, G_IO_FLAG_NONBLOCK, NULL);
 336 gps->io.error_sid=g_io_add_watch_full(gps->io.channel, G_PRIORITY_HIGH_IDLE, G_IO_ERR | G_IO_HUP, gps_channel_cb_error, NULL, NULL);
 337 gps->io.connect_sid=g_io_add_watch_full(gps->io.channel, G_PRIORITY_HIGH_IDLE, G_IO_OUT, gps_channel_cb_connect, NULL, NULL);
 338 gps->io.clater_sid=0;
 339 }
 340
 341 /**
 342  * Connect to the receiver.
 343  * This method assumes that fd is -1 and _channel is NULL.  If unsure, call
 344  * gps_disconnect() first.
 345  * Since this is an idle function, this function returns whether or not it
 346  * should be called again, which is always FALSE.
 347  *
 348  * This function prepares for the connection, the gps_connect does the "connecting" if needed.
 349  */
 350 gboolean
 351 gps_connect_now(Gps *gps)
 352 {
 353 g_assert(gps);
 354 g_debug("GPS: Connecting GPS type %d to %s:%d\n", gps->io.type, gps->io.address, gps->io.port);
 355
 356 switch (gps->io.type) {
 357         case GPS_IO_FILE:
 358                 if (!gps->io.address)
 359                         return FALSE;
 360                 if (*gps->io.address=='/')
 361                         gps->io.fd=open(gps->io.address, O_RDONLY);
 362                 else
 363                         return FALSE;
 364         break;
 365         case GPS_IO_TCP:
 366         case GPS_IO_GPSD:
 367                 if (gps->io.conn==RCVR_DOWN && gps->io.address) {
 368                         gps->io.fd=socket(AF_INET, SOCK_STREAM, 0);
 369                         if (gps->io.fd==-1) {
 370                                 g_debug("Failed to create socket\n");
 371                                 gps_connect_later(gps);
 372                                 return FALSE;
 373                         }
 374                         memcpy(&gps->io.rcvr_addr_in.sin_addr.s_addr, gps->io.address, strlen(gps->io.address));
 375                         gps->io.rcvr_addr_in.sin_family = AF_INET;
 376                         gps->io.rcvr_addr_in.sin_port = htons(gps->io.port);
 377                 } else {
 378                         return FALSE;
 379                 }
 380 #ifdef WITH_GPSBT
 381                 if (gps->io.type==GPS_IO_GPSD) {
 382                         gint r;
 383                         gchar ebuf[128];
 384                         r=gpsbt_start(NULL, 0, 0, 0, ebuf, 128, 0, &ctx);
 385                         if (r!=0) {
 386                                 g_warning("Failed to start gpsd");
 387                                 return FALSE;
 388                         }
 389                 }
 390 #endif
 391         break;
 392         case GPS_IO_HILDON_DBUS:
 393 #ifdef WITH_HILDON_DBUS_BT
 394                 if (gps->io.rfcomm_req_proxy) {
 395                         gboolean mybool = TRUE;
 396                         dbus_g_proxy_begin_call(_rfcomm_req_proxy,
 397                                 BTCOND_RFCOMM_CONNECT_REQ, (DBusGProxyCallNotify)gps_connect_response, NULL, NULL,
 398                                 G_TYPE_STRING, gps->io.address,
 399                                 G_TYPE_STRING, "SPP",
 400                                 G_TYPE_BOOLEAN, &mybool,
 401                                 G_TYPE_INVALID);
 402                 }
 403 #else
 404                 return FALSE;
 405 #endif
 406         break;
 407         case GPS_IO_RFCOMM:
 408 #ifdef WITH_BLUEZ
 409                 if (!gps->io.address)
 410                         return FALSE;
 411
 412                 if (gps->io.conn==RCVR_DOWN && gps->io.address) {
 413                         gps->io.fd=socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
 414
 415                         /* If file descriptor creation failed, try again later.  Note that
 416                          * there is no need to call rcvr_disconnect() because the file
 417                          * descriptor creation is the first step, so if it fails, there's
 418                          * nothing to clean up. */
 419                         if (gps->io.fd==-1) {
 420                                 g_debug("Failed to create socket\n");
 421                                 gps_connect_later(gps);
 422                                 return FALSE;
 423                         }
 424                         gps->io.rcvr_addr_rc.rc_family=AF_BLUETOOTH;
 425                         gps->io.rcvr_addr_rc.rc_channel=1;
 426                         str2ba(gps->io.address, &gps->io.rcvr_addr_rc.rc_bdaddr);
 427                 }
 428 #endif
 429                 return FALSE;
 430         break;
 431         case GPS_IO_SIMULATION:
 432                 /* Set a periodic cb to generate random movement */
 433                 gps->data.lat=64.22;
 434                 gps->data.lat=22.22;
 435                 gps->io.conn=RCVR_FIXED;
 436                 gps->data.fix=FIX_2D;
 437                 gps_data_integerize(&gps->data);
 438                 gps->io.input_sid=g_timeout_add(1000, (GSourceFunc)gps_simulate, gps);
 439                 return FALSE;
 440         break;
 441         default:
 442                 return FALSE;
 443         break;
 444 }
 445
 446 gps_read_buffer_prepare(gps);
 447 gps_read_channel_connect(gps);
 448 gps_connect(gps);
 449 return FALSE;
 450 }
 451
 452 /**
 453  * Connection callback
 454  */
 455 static gboolean
 456 gps_channel_cb_connect(GIOChannel * src, GIOCondition condition, gpointer data)
 457 {
 458 gint error, size = sizeof(error);
 459 Gps *gps=(Gps *)data;
 460
 461 g_assert(data);
 462
 463 gps->io.curr=gps->io.buffer;
 464 gps->io.last=gps->io.buffer+GPS_READ_BUF_SIZE-1;
 465
 466 switch (gps->io.type) {
 467         case GPS_IO_TCP:
 468         case GPS_IO_RFCOMM:
 469         case GPS_IO_GPSD:
 470                 if ((getsockopt(gps->io.fd, SOL_SOCKET, SO_ERROR, &error, &size) || error)) {
 471                         g_printerr("Error connecting to receiver; retrying...\n");
 472                         gps_disconnect(gps);
 473                         gps_connect_later(gps);
 474                         gps->io.connect_sid=0;
 475                         gps->io.errors++;
 476                         return FALSE;
 477                 }
 478                 /* Set gpsd to NMEA mode */
 479                 if (gps->io.type==GPS_IO_GPSD) {
 480                         GIOStatus status;
 481                         gsize written;
 482                         status=g_io_channel_write_chars(gps->io.channel, GPSD_NMEA, -1, &written, NULL);
 483                         if (status!=G_IO_STATUS_NORMAL || written!=sizeof(GPSD_NMEA)) {
 484                                 g_printerr("Failed to set gpsd to NMEA mode\n");
 485                                 gps_disconnect(gps);
 486                                 gps_connect_later(gps);
 487                                 gps->io.connect_sid=0;
 488                                 gps->io.errors++;
 489                                 return FALSE;
 490                         }
 491                 }
 492         break;
 493         case GPS_IO_FILE:
 494
 495         break;
 496         default:
 497                 g_warning("Connection from non-connecting source\n");
 498         break;
 499 }
 500
 501 g_printf("Connected to receiver!\n");
 502 gps_conn_set_state(gps, RCVR_UP);
 503 gps->io.input_sid=g_io_add_watch_full(gps->io.channel, G_PRIORITY_HIGH_IDLE, G_IO_IN | G_IO_PRI, gps_channel_cb_input, gps, NULL);
 504 gps->io.errors=0;
 505 gps->io.connect_sid=0;
 506 return FALSE;
 507 }
 508
 509 static gboolean
 510 gps_channel_cb_error(GIOChannel *src, GIOCondition condition, gpointer data)
 511 {
 512 Gps *gps=(Gps *)data;
 513
 514 g_assert(data);
 515 /* An error has occurred - re-connect(). */
 516 gps_disconnect(gps);
 517 track_add(NULL, FALSE);
 518 /* _speed_excess = FALSE; */
 519
 520 if (gps->io.conn > RCVR_OFF) {
 521         gps_conn_set_state(gps, RCVR_DOWN);
 522         gps_connect_later(gps);
 523 }
 524 return FALSE;
 525 }
 526
 527 static gboolean
 528 gps_channel_cb_input(GIOChannel *src, GIOCondition condition, gpointer data)
 529 {
 530 gsize bytes_read;
 531 GIOStatus status;
 532 gchar *eol;
 533 Gps *gps=(Gps *)data;
 534
 535 g_debug("%s(%d)\n", __PRETTY_FUNCTION__, condition);
 536 g_assert(data);
 537
 538 status=g_io_channel_read_chars(gps->io.channel, gps->io.curr, gps->io.last-gps->io.curr, &bytes_read, NULL);
 539 switch (status) {
 540         case G_IO_STATUS_NORMAL:
 541         gps->io.curr += bytes_read;
 542         *gps->io.curr = '\0';   /* append a \0 so we can read as string */
 543         while ((eol = strchr(gps->io.buffer, '\n'))) {
 544                 gchar *sptr = gps->io.buffer + 1;       /* Skip the $ */
 545                 guint csum = 0;
 546                 if (*gps->io.buffer=='$') {
 547                         /* This is the beginning of a sentence; okay to parse. */
 548                         *eol = '\0';    /* overwrite \n with \0 */
 549                         while (*sptr && *sptr != '*')
 550                                 csum ^= *sptr++;
 551
 552                         /* If we're at a \0 (meaning there is no checksum), or if the
 553                          * checksum is good, then parse the sentence. */
 554                         if (!*sptr || csum == strtol(sptr + 1, NULL, 16)) {
 555                                 gchar *data;
 556
 557                                 if (*sptr)
 558                                         *sptr = '\0';   /* take checksum out of the buffer. */
 559
 560                                 data=g_strdup(gps->io.buffer);
 561                                 g_idle_add_full(G_PRIORITY_HIGH_IDLE, (GSourceFunc)gps_nmea_parse, data, NULL);
 562                         } else {
 563                                 /* There was a checksum, and it was bad. */
 564                                 g_printerr("%s: Bad checksum in NMEA sentence:\n%s\n", __PRETTY_FUNCTION__, gps->io.buffer);
 565                         }
 566                 }
 567
 568                 /* If eol is at or after (_gps_read_buf_curr - 1) */
 569                 if (eol >= (gps->io.curr - 1)) {
 570                         /* Last read was a newline - reset read buffer */
 571                         gps->io.curr = gps->io.buffer;
 572                         *gps->io.curr = '\0';
 573                 } else {
 574                         /* Move the next line to the front of the buffer. */
 575                         memmove(gps->io.buffer, eol + 1, gps->io.curr - eol);   /* include terminating 0 */
 576                         /* Subtract _curr so that it's pointing at the new \0. */
 577                         gps->io.curr -= (eol - gps->io.buffer + 1);
 578                 }
 579         }
 580         break;
 581         case G_IO_STATUS_ERROR:
 582         case G_IO_STATUS_EOF:
 583                 gps_disconnect(gps);
 584                 gps_connect_later(gps);
 585                 gps->io.errors++;
 586                 return FALSE;
 587         break;
 588         case G_IO_STATUS_AGAIN:
 589                 return TRUE;
 590         break;
 591         default:
 592                 g_assert_not_reached();
 593         break;
 594 }
 595
 596 return TRUE;
 597 }
 598