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 <dbus/dbus.h>
  40 #include <dbus/dbus-glib.h>
  41 #include <sys/types.h>
  42 #include <sys/socket.h>
  43 #include <arpa/inet.h>
  44 #include <netdb.h>
  45 #include <fcntl.h>
  46
  47 #include "gps.h"
  48 #include "latlon.h"
  49 #include "gps-conn.h"
  50 #include "gps-nmea-parse.h"
  51 #include "position.h"
  52
  53 #ifdef WITH_HILDON_DBUS_BT
  54 #include <bt-dbus.h>
  55 #endif
  56
  57 #ifdef WITH_GPSBT
  58 #include <gpsbt.h>
  59 static gpsbt_t ctx;
  60 #endif
  61
  62 static gboolean gps_channel_cb_error(GIOChannel *src, GIOCondition condition, gpointer data);
  63 static gboolean gps_channel_cb_input(GIOChannel *src, GIOCondition condition, gpointer data);
  64 static gboolean gps_channel_cb_connect(GIOChannel * src, GIOCondition condition, gpointer data);
  65 static gboolean gps_connect_socket(Gps *gps);
  66 static gboolean gps_connect_file(Gps *gps, gchar *file);
  67
  68 #define GPSD_NMEA "r+\r\n"
  69 #define GPSD_PORT (2947)
  70 #define LOCALHOST "127.0.0.1"
  71 #define GPS_DEVICE_FILE "/dev/ttyS0"
  72
  73 const GpsTypes gps_types[] = {
  74 #ifdef WITH_BLUEZ
  75         { GPS_IO_RFCOMM,                "Bluetooth connection (old)", TRUE, TRUE, FALSE},
  76 #endif
  77 #ifdef WITH_HILDON_DBUS_BT
  78         { GPS_IO_HILDON_DBUS,   "Bluetooth connection (btconn)", TRUE, TRUE, FALSE },
  79 #endif
  80 #ifdef WITH_BLUEZ_DBUS_BT
  81         { GPS_IO_BLUEZ_DBUS,    "Bluetooth connection (new)", TRUE, TRUE, FALSE},
  82 #endif
  83         { GPS_IO_GPSD,                  "GPSD Connection", FALSE, TRUE, TRUE, GPSD_PORT, LOCALHOST },
  84         { GPS_IO_FILE,                  "File or device", FALSE, TRUE, FALSE, 0, GPS_DEVICE_FILE },
  85         { GPS_IO_TCP,                   "TCP Connection", FALSE, TRUE, TRUE, 1024, LOCALHOST },
  86 #ifdef WITH_GYPSY
  87         { GPS_IO_GYPSY,                 "Gypsy", FALSE, TRUE, FALSE },
  88 #endif
  89         { GPS_IO_SIMULATION,    "Simulation (random)", FALSE, FALSE, FALSE},
  90         { 0 }
  91 };
  92
  93 const GpsTypes *
  94 gps_get_supported_types(void)
  95 {
  96 return gps_types;
  97 }
  98
  99 /**
 100  * Init GPS structure
 101  */
 102 Gps *
 103 gps_new(GpsIOSourceType type)
 104 {
 105 Gps *gps;
 106
 107 gps=g_slice_new0(Gps);
 108 g_assert(gps);
 109 gps->io.type=type;
 110 gps_clear(gps);
 111 return gps;
 112 }
 113
 114 void
 115 gps_clear(Gps *gps)
 116 {
 117 g_assert(gps);
 118 gps->io.fd=-1;
 119 gps->io.conn=RCVR_OFF;
 120 gps->io.nmea_cnt=0;
 121 memset(&gps->data, 0, sizeof(gps->data));
 122 gps->data.fix=FIX_NOFIX;
 123 gps->data.lat=60.20;
 124 gps->data.lon=22.20;
 125 gps->data.satinview=0;
 126 gps->data.satinuse=0;
 127 gps_data_integerize(&gps->data);
 128 }
 129
 130 void
 131 gps_set_address(Gps *gps, gchar *address, guint port)
 132 {
 133 if (gps->io.address)
 134         g_free(gps->io.address);
 135 if (address)
 136         gps->io.address=g_strdup(address);
 137 gps->io.port=port;
 138 }
 139
 140 void
 141 gps_set_type(Gps *gps, GpsIOSourceType type)
 142 {
 143 gps->io.type=type;
 144 }
 145
 146 void
 147 gps_free(Gps *gps)
 148 {
 149 gps_disconnect(gps);
 150 if (gps->io.address)
 151         g_free(gps->io.address);
 152 g_slice_free(Gps, gps);
 153 }
 154
 155 /**
 156  * Calculate offsets.
 157  */
 158 void
 159 gps_data_integerize(GpsData *gpsdata)
 160 {
 161 gdouble tmp=(gpsdata->heading*(1.f/180.f))*G_PI;
 162
 163 gpsdata->vel_offset_lat=-(floor(gpsdata->speed*cos(tmp)+0.5f));
 164 gpsdata->vel_offset_lon=(floor(gpsdata->speed*sin(tmp)+0.5f));
 165 }
 166
 167 #ifdef WITH_HILDON_DBUS_BT
 168 void
 169 gps_connect_response(DBusGProxy *proxy, DBusGProxyCall *call_id, Gps *gps)
 170 {
 171 GError *error = NULL;
 172 gchar *fdpath = NULL;
 173
 174 if (gps->io.conn==RCVR_DOWN && gps->io.address) {
 175         if (!dbus_g_proxy_end_call(gps->io.rfcomm_req_proxy, call_id, &error, G_TYPE_STRING, &fdpath, G_TYPE_INVALID)) {
 176                 if (error->domain == DBUS_GERROR && error->code == DBUS_GERROR_REMOTE_EXCEPTION) {
 177                         /* If we're already connected, it's not an error, unless
 178                          * they don't give us the file descriptor path, in which
 179                          * case we re-connect.*/
 180                         if (!strcmp(BTCOND_ERROR_CONNECTED, dbus_g_error_get_name(error)) || !fdpath) {
 181                                 g_printerr("Caught remote method exception %s: %s", dbus_g_error_get_name(error), error->message);
 182                                 gps_disconnect(gps);
 183
 184                                 if (gps->connection_retry==NULL)
 185                                         return;
 186
 187                                 if (gps->connection_retry(gps, error->message)==TRUE) {
 188                                         gps_connect_later(gps);
 189                                 } else {
 190                                         gps_conn_set_state(gps, RCVR_OFF);
 191                                 }
 192                                 return;
 193                         }
 194                 } else {
 195                         /* Unknown error. */
 196                         g_printerr("Error: %s\n", error->message);
 197                         gps_disconnect(gps);
 198                         gps_connect_later(gps); /* Try again later. */
 199                         return;
 200                 }
 201         }
 202         gps_connect_file(gps, fdpath);
 203 }
 204 /* else { Looks like the middle of a disconnect.  Do nothing. } */
 205 }
 206 #endif
 207
 208 /**
 209  * Place a request to connect about 1 second after the function is called.
 210  */
 211 void
 212 gps_connect_later(Gps *gps)
 213 {
 214 g_assert(gps);
 215 g_assert(gps->io.clater_sid==0);
 216 gps->io.clater_sid=g_timeout_add(1000, (GSourceFunc)gps_connect_now, gps);
 217 }
 218
 219 gboolean
 220 gps_connect(Gps *gps)
 221 {
 222 switch (gps->io.type) {
 223         case GPS_IO_FILE:
 224                 return TRUE;
 225         break;
 226         case GPS_IO_RFCOMM:
 227         case GPS_IO_TCP:
 228         case GPS_IO_GPSD:
 229                 return gps_connect_socket(gps);
 230         break;
 231         case GPS_IO_GYPSY:
 232                 return FALSE;
 233         break;
 234         default:
 235         break;
 236 }
 237 return FALSE;
 238 }
 239
 240 /**
 241  * Helper to open a file or device node
 242  */
 243 static gboolean
 244 gps_connect_file(Gps *gps, gchar *file)
 245 {
 246 if (-1==(gps->io.fd=open(file, O_RDONLY))) {
 247         gps_disconnect(gps);
 248         gps_connect_later(gps);
 249         /* Add retry cb */
 250         return FALSE;
 251 }
 252 return TRUE;
 253 }
 254
 255 /**
 256  * Helper to connect to a socket file descriptor.
 257  *
 258  */
 259 static gboolean
 260 gps_connect_socket(Gps *gps)
 261 {
 262 gint r, e;
 263 g_assert(gps);
 264
 265 switch (gps->io.type) {
 266         case GPS_IO_RFCOMM:
 267 #ifdef WITH_BLUEZ
 268                 g_debug("RFCOMM: %d", gps->io.fd);
 269                 r=connect(gps->io.fd, (struct sockaddr *)&gps->io.rcvr_addr_rc, sizeof(gps->io.rcvr_addr_rc));
 270 #endif
 271         break;
 272         case GPS_IO_TCP:
 273         case GPS_IO_GPSD:
 274                 g_debug("TCP: %d", gps->io.fd);
 275                 r=connect(gps->io.fd, (struct sockaddr *)&gps->io.rcvr_addr_in, sizeof(gps->io.rcvr_addr_in));
 276         break;
 277         default:
 278                 return FALSE;
 279 }
 280 e=errno;
 281 g_debug("GPS: Error %d (%s)", e, strerror(e));
 282
 283 /* The socket is non blocking so handle it */
 284 if (r != 0) {
 285         switch (e) {
 286         case EAGAIN:
 287         case EBUSY:
 288         case EINPROGRESS:
 289         case EALREADY:
 290                 g_printerr("*** Connection in progress... %d %d\n", e, r);
 291                 perror("INFO: ");
 292                 return TRUE;
 293         break;
 294         case EHOSTUNREACH:
 295                 g_printerr("*** Bluetooth/GPS device not found.\n");
 296                 gps_disconnect(gps);
 297 #if 0
 298         set_action_activate("gps_enable", FALSE);
 299         popup_error(_window, _("No bluetooth or GPS device found."));
 300 #endif
 301                 return FALSE;
 302         break;
 303         case ECONNREFUSED:
 304                 g_printerr("*** Connection refused.\n");
 305                 gps_disconnect(gps);
 306                 return FALSE;
 307         break;
 308         default:
 309                 /* Connection failed.  Disconnect and try again later. */
 310                 g_printerr("### Connect failed, retrying... %d %d\n", e, r);
 311                 perror("ERROR: ");
 312                 gps_disconnect(gps);
 313                 gps_connect_later(gps);
 314                 return FALSE;
 315         break;
 316         }
 317 }
 318 return TRUE;
 319 }
 320
 321 /**
 322  * Disconnect the GPS device.
 323  * - Add channel callbacks are removed
 324  * - Channel is close and shutdown
 325  * - File descriptor is closed
 326  * - Anything special for disconnecting is done
 327  */
 328 void
 329 gps_disconnect(Gps *gps)
 330 {
 331 g_assert(gps);
 332
 333 g_debug("GPS: Disconnecting from gps");
 334 /* Remove watches. */
 335 if (gps->io.clater_sid) {
 336         g_source_remove(gps->io.clater_sid);
 337         gps->io.clater_sid=0;
 338 }
 339 if (gps->io.error_sid) {
 340         g_source_remove(gps->io.error_sid);
 341         gps->io.error_sid=0;
 342 }
 343 if (gps->io.connect_sid) {
 344         g_source_remove(gps->io.connect_sid);
 345         gps->io.connect_sid=0;
 346 }
 347 if (gps->io.input_sid) {
 348         g_source_remove(gps->io.input_sid);
 349         gps->io.input_sid=0;
 350 }
 351
 352 /* Destroy the GIOChannel object. */
 353 if (gps->io.channel) {
 354         g_debug("GPS: Shutting down IO channel");
 355         g_io_channel_shutdown(gps->io.channel, FALSE, NULL);
 356         g_io_channel_unref(gps->io.channel);
 357         gps->io.channel=NULL;
 358 }
 359
 360 /* Close the file descriptor. */
 361 if (gps->io.fd!=-1) {
 362         g_debug("GPS: Closing fd %d", gps->io.fd);
 363         close(gps->io.fd);
 364         gps->io.fd=-1;
 365 }
 366
 367 #ifdef WITH_GPSBT
 368 if (gps->io.type==GPS_IO_GPSD) {
 369         gint r;
 370
 371         g_debug("GPS: Requesting gpsd shutdown using gpsbt");
 372         r=gpsbt_stop(&ctx);
 373         if (r!=0)
 374                 g_warning("Failed to stop gpsd\n");
 375 }
 376 #endif
 377
 378 #ifdef WITH_HILDON_DBUS_BT
 379 if (gps->io.type==GPS_IO_HILDON_DBUS && gps->io.rfcomm_req_proxy) {
 380         GError *error = NULL;
 381
 382         g_debug("GPS: Requesting rfcomm disconnection");
 383         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);
 384         error = NULL;
 385         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);
 386 }
 387 #endif
 388
 389 #ifdef WITH_BLUEZ_DBUS_BT
 390 if (gps->io.type==GPS_IO_BLUEZ_DBUS && gps->io.rfcomm_req_proxy) {
 391         GError *error = NULL;
 392
 393 }
 394 #endif
 395
 396 gps_clear(gps);
 397 }
 398
 399 static void
 400 gps_simulate_start(Gps *gps)
 401 {
 402 g_assert(gps);
 403 if (_home->valid) {
 404         gps->data.lat=_home->lat;
 405         gps->data.lon=_home->lon;
 406 } else {
 407         gps->data.lat=60.45;
 408         gps->data.lon=22.26;
 409 }
 410
 411 gps->io.conn=RCVR_FIXED;
 412 gps->data.fix=FIX_2D;
 413 gps->data.satinview=0;
 414 gps->data.satinuse=0;
 415 gps_data_integerize(&gps->data);
 416 }
 417
 418 static gboolean
 419 gps_simulate_move(Gps *gps)
 420 {
 421 static gdouble slat=0, slon=0;
 422 gdouble plat, plon, accel;
 423 gfloat h;
 424
 425 g_assert(gps);
 426
 427 if (_dest->valid) {
 428         gdouble c;
 429
 430         c=calculate_course_rad(_dest->lat,_dest->lon, gps->data.lat, gps->data.lon);
 431         c-=G_PI;
 432
 433         accel=0.005*g_random_double();
 434         slat=accel*cos(c);
 435         slon=accel*sin(c);
 436         g_debug("Sim: %f %f %f %f", slat, slon, accel, _dest->angle);
 437 } else {
 438         if (g_random_double()<0.2) {
 439                 slat=g_random_double_range(-0.0009, 0.0009);
 440                 slon=g_random_double_range(-0.0009, 0.0009);
 441         }
 442 }
 443 plat=gps->data.lat;
 444 plon=gps->data.lon;
 445 gps->data.lat+=slat;
 446 gps->data.lon+=slon;
 447
 448 BOUND(gps->data.lat, -80.0, 80.0);
 449 BOUND(gps->data.lon, -80.0, 80.0);
 450
 451 g_debug("Sim: %f %f", gps->data.lat, gps->data.lon);
 452
 453 gps->data.speed=0;
 454
 455 h=calculate_course(plat, plon, gps->data.lat, gps->data.lon);
 456 gps->data.heading=(h<0) ? 360+h : h;
 457 gps->data.time=time(NULL);
 458 gps_data_integerize(&gps->data);
 459 if (gps->update_location!=NULL) {
 460         gps->update_location(gps);
 461 }
 462 gps->data.lheading=gps->data.heading;
 463
 464 return gps->io.conn==RCVR_FIXED ? TRUE : FALSE;
 465 }
 466
 467 /**
 468  * Reset GPS read buffer
 469  */
 470 static void
 471 gps_read_buffer_prepare(Gps *gps)
 472 {
 473 gps->io.curr=gps->io.buffer;
 474 *gps->io.curr = '\0';
 475 gps->io.last=gps->io.buffer+GPS_READ_BUF_SIZE-1;
 476 }
 477
 478 /**
 479  * Connect file descriptor to channel and add watches.
 480  */
 481 static void
 482 gps_connect_channel_connect(Gps *gps)
 483 {
 484 g_assert(gps->io.channel==NULL);
 485 gps->io.channel=g_io_channel_unix_new(gps->io.fd);
 486 g_io_channel_set_encoding(gps->io.channel, NULL, NULL);
 487 g_io_channel_set_flags(gps->io.channel, G_IO_FLAG_NONBLOCK, NULL);
 488 g_io_channel_set_buffered(gps->io.channel, FALSE);
 489 gps->io.error_sid=g_io_add_watch_full(gps->io.channel, G_PRIORITY_HIGH_IDLE, G_IO_ERR | G_IO_HUP | G_IO_NVAL, gps_channel_cb_error, gps, NULL);
 490 gps->io.connect_sid=g_io_add_watch_full(gps->io.channel, G_PRIORITY_HIGH_IDLE, G_IO_OUT, gps_channel_cb_connect, gps, NULL);
 491 gps->io.clater_sid=0;
 492 }
 493
 494 /**
 495  * Connect to the receiver.
 496  * This method assumes that fd is -1 and _channel is NULL.  If unsure, call
 497  * gps_disconnect() first.
 498  * Since this is an idle function, this function returns whether or not it
 499  * should be called again, which is always FALSE.
 500  *
 501  * This function prepares for the connection, the gps_connect does the "connecting" if needed.
 502  */
 503 gboolean
 504 gps_connect_now(Gps *gps)
 505 {
 506 GError *error = NULL;
 507
 508 g_assert(gps);
 509 g_debug("GPS: Connecting GPS type %d to %s:%d\n", gps->io.type, gps->io.address, gps->io.port);
 510
 511 switch (gps->io.type) {
 512         case GPS_IO_FILE:
 513                 if (!gps->io.address)
 514                         return FALSE;
 515                 gps_connect_file(gps, gps->io.address);
 516                 return FALSE;
 517         break;
 518         case GPS_IO_TCP:
 519         case GPS_IO_GPSD:
 520                 if (gps->io.conn<=RCVR_DOWN && gps->io.address) {
 521                         const gchar *ip;
 522                         struct hostent *hostinfo;
 523                         struct in_addr taddr;
 524
 525                         g_debug("TCP: Preparing to connect to %s:%d", gps->io.address, gps->io.port);
 526
 527                         gps->io.fd=socket(AF_INET, SOCK_STREAM, 0);
 528                         if (gps->io.fd==-1) {
 529                                 g_debug("Failed to create socket\n");
 530                                 gps_connect_later(gps);
 531                                 return FALSE;
 532                         }
 533
 534                         gps->io.rcvr_addr_in.sin_family=AF_INET;
 535                         gps->io.rcvr_addr_in.sin_port=htons(gps->io.port);
 536                         if (inet_aton(gps->io.address, &taddr)) {
 537                                 ip=gps->io.address;
 538                         } else {
 539                                 gchar ipbuf[INET_ADDRSTRLEN];
 540
 541                                 hostinfo=gethostbyname(gps->io.address);
 542                                 if (!hostinfo) {
 543                                         perror("TCP:");
 544                                         return FALSE;
 545                                 }
 546                                 if (hostinfo->h_addrtype!=AF_INET) {
 547                                         perror("TCP:");
 548                                         return FALSE;
 549                                 }
 550                                 ip=ip = inet_ntop (AF_INET, (struct in_addr *)hostinfo->h_addr_list[0], ipbuf, sizeof (ipbuf));
 551                         }
 552
 553                         if (inet_pton(AF_INET, ip, &gps->io.rcvr_addr_in.sin_addr.s_addr)<1) {
 554                                 perror("TCP:");
 555                                 return FALSE;
 556                         }
 557                 } else {
 558                         return FALSE;
 559                 }
 560 #ifdef WITH_GPSBT
 561                 if (gps->io.type==GPS_IO_GPSD) {
 562                         gchar errbuf[255];
 563
 564                         /* Force correct gpsd binary */
 565                         setenv ("GPSD_PROG", "/usr/sbin/gpsd", 1);
 566
 567                         memset(&ctx, 0, sizeof(ctx));
 568                         memset(&errbuf, 0, sizeof(errbuf));
 569                         errno=0;
 570
 571                         if (gpsbt_start(NULL, 0, 0, 0, errbuf, sizeof(errbuf), 0, &ctx)!=0) {
 572                                 g_warning("Failed to start gpsd: %d (%s) %s\n", errno, strerror(errno), errbuf);
 573                                 return FALSE;
 574                         } else {
 575                                 g_debug("Waiting for gpsd");
 576                                 sleep(2);
 577                                 g_debug("Done wating");
 578                         }
 579                 }
 580 #endif
 581         break;
 582 #ifdef WITH_HILDON_DBUS_BT
 583         case GPS_IO_HILDON_DBUS:
 584                 if (NULL == (gps->io.dbus_conn=dbus_g_bus_get(DBUS_BUS_SYSTEM, &error))) {
 585                         g_printerr("Failed to get D-Bus connection.");
 586                         return FALSE;
 587                 }
 588                 if (NULL == (gps->io.rfcomm_req_proxy = dbus_g_proxy_new_for_name(gps->io.dbus_conn, BTCOND_SERVICE, BTCOND_REQ_PATH, BTCOND_REQ_INTERFACE))) {
 589                         g_printerr("Failed to open connection to %s.\n", BTCOND_REQ_INTERFACE);
 590                         return FALSE;
 591                 }
 592
 593                 if (gps->io.rfcomm_req_proxy) {
 594                         gboolean mybool = TRUE;
 595                         dbus_g_proxy_begin_call(gps->io.rfcomm_req_proxy,
 596                                 BTCOND_RFCOMM_CONNECT_REQ, (DBusGProxyCallNotify)gps_connect_response, gps, NULL,
 597                                 G_TYPE_STRING, gps->io.address,
 598                                 G_TYPE_STRING, "SPP",
 599                                 G_TYPE_BOOLEAN, &mybool,
 600                                 G_TYPE_INVALID);
 601                 }
 602         break;
 603 #endif
 604 #ifdef WITH_BLUEZ
 605         case GPS_IO_RFCOMM:
 606                 if (!gps->io.address)
 607                         return FALSE;
 608
 609                 if (gps->io.conn<=RCVR_DOWN && gps->io.address) {
 610                         gps->io.fd=socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
 611
 612                         /* If file descriptor creation failed, try again later.  Note that
 613                          * there is no need to call rcvr_disconnect() because the file
 614                          * descriptor creation is the first step, so if it fails, there's
 615                          * nothing to clean up. */
 616                         if (gps->io.fd==-1) {
 617                                 g_debug("Failed to create socket");
 618                                 gps_connect_later(gps);
 619                                 return FALSE;
 620                         }
 621                         gps->io.rcvr_addr_rc.rc_family=AF_BLUETOOTH;
 622                         gps->io.rcvr_addr_rc.rc_channel=1;
 623                         str2ba(gps->io.address, &gps->io.rcvr_addr_rc.rc_bdaddr);
 624                 }
 625 #endif
 626         break;
 627         case GPS_IO_SIMULATION:
 628                 /* Set a periodic cb to generate random movement */
 629                 gps_simulate_start(gps);
 630                 gps->io.input_sid=g_timeout_add(1000, (GSourceFunc)gps_simulate_move, gps);
 631                 return FALSE;
 632         break;
 633         default:
 634                 g_printerr("Unknown GPS connection type\n");
 635                 return FALSE;
 636         break;
 637 }
 638
 639 gps_read_buffer_prepare(gps);
 640 gps_connect_channel_connect(gps);
 641 gps_connect(gps);
 642 return FALSE;
 643 }
 644
 645 /**
 646  * Connection callback
 647  */
 648 static gboolean
 649 gps_channel_cb_connect(GIOChannel *src, GIOCondition condition, gpointer data)
 650 {
 651 gint error, size = sizeof(error);
 652 Gps *gps=(Gps *)data;
 653
 654 g_assert(data);
 655 g_assert(gps->io.channel);
 656
 657 gps->io.curr=gps->io.buffer;
 658 gps->io.last=gps->io.buffer+GPS_READ_BUF_SIZE-1;
 659
 660 switch (gps->io.type) {
 661         case GPS_IO_TCP:
 662         case GPS_IO_RFCOMM:
 663         case GPS_IO_GPSD:
 664                 if ((getsockopt(gps->io.fd, SOL_SOCKET, SO_ERROR, &error, &size) || error)) {
 665                         g_printerr("Error connecting to receiver; retrying...\n");
 666                         gps_disconnect(gps);
 667                         gps_connect_later(gps);
 668                         gps->io.connect_sid=0;
 669                         gps->io.errors++;
 670                         return FALSE;
 671                 }
 672                 /* Set gpsd to NMEA mode */
 673                 if (gps->io.type==GPS_IO_GPSD) {
 674                         GIOStatus status;
 675                         gsize written;
 676                         GError *error = NULL;
 677
 678                         g_debug("GPS: Requesting NMEA mode from GPSD using cmd %s", GPSD_NMEA);
 679                         status=g_io_channel_write_chars(gps->io.channel, GPSD_NMEA, strlen(GPSD_NMEA), &written, &error);
 680                         if (status==G_IO_STATUS_NORMAL && written==strlen(GPSD_NMEA)) {
 681                                 g_debug("GPS: NMEA mode set (%d %d)", (gint)written, (gint)status);
 682                         } else {
 683                                 g_printerr("Failed to set gpsd to NMEA mode: %d %d %s\n",(gint)status,(gint)written, error->message);
 684                                 g_error_free(error);
 685                                 gps_disconnect(gps);
 686                                 gps_connect_later(gps);
 687                                 gps->io.connect_sid=0;
 688                                 gps->io.errors++;
 689                                 return FALSE;
 690                         }
 691                 }
 692         break;
 693         case GPS_IO_FILE:
 694
 695         break;
 696         default:
 697                 g_warning("Connection from non-connecting source\n");
 698         break;
 699 }
 700
 701 g_printf("Connected to receiver!\n");
 702 gps_conn_set_state(gps, RCVR_UP);
 703 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);
 704 gps->io.errors=0;
 705 gps->io.connect_sid=0;
 706 return FALSE;
 707 }
 708
 709 static gboolean
 710 gps_channel_cb_error(GIOChannel *src, GIOCondition condition, gpointer data)
 711 {
 712 Gps *gps=(Gps *)data;
 713
 714 g_assert(data);
 715 g_debug("GPS: Channel error %d", condition);
 716
 717 /* An error has occurred - re-connect(). */
 718 gps_disconnect(gps);
 719
 720 /* Do error cb */
 721 if (gps->connection_error!=NULL)
 722         gps->connection_error(gps, "GPS Connection error");
 723
 724 if (gps->io.conn > RCVR_OFF) {
 725         gps_conn_set_state(gps, RCVR_DOWN);
 726         gps_connect_later(gps);
 727 }
 728
 729 return FALSE;
 730 }
 731
 732 static gboolean
 733 gps_channel_cb_input(GIOChannel *src, GIOCondition condition, gpointer data)
 734 {
 735 gsize bytes_read;
 736 GIOStatus status;
 737 gchar *eol;
 738 Gps *gps=(Gps *)data;
 739
 740 g_debug("%s(%d)\n", __PRETTY_FUNCTION__, condition);
 741 g_assert(data);
 742
 743 status=g_io_channel_read_chars(gps->io.channel, gps->io.curr, gps->io.last-gps->io.curr, &bytes_read, NULL);
 744 switch (status) {
 745         case G_IO_STATUS_NORMAL:
 746         gps->io.curr += bytes_read;
 747         *gps->io.curr = '\0';   /* append a \0 so we can read as string */
 748         while ((eol = strchr(gps->io.buffer, '\n'))) {
 749                 gchar *sptr = gps->io.buffer + 1;       /* Skip the $ */
 750                 guint csum = 0;
 751                 if (*gps->io.buffer=='$') {
 752                         /* This is the beginning of a sentence; okay to parse. */
 753                         *eol = '\0';    /* overwrite \n with \0 */
 754                         while (*sptr && *sptr != '*')
 755                                 csum ^= *sptr++;
 756
 757                         /* If we're at a \0 (meaning there is no checksum), or if the
 758                          * checksum is good, then parse the sentence. */
 759                         if (!*sptr || csum == strtol(sptr + 1, NULL, 16)) {
 760                                 gchar *data;
 761
 762                                 if (*sptr)
 763                                         *sptr = '\0';   /* take checksum out of the buffer. */
 764
 765                                 gps->io.nmea_cnt++;
 766                                 gps->io.nmea=gps->io.buffer;
 767
 768                                 g_assert(gps->io.nmea);
 769                                 g_debug("NMEA1 %d: (%s)", gps->io.nmea_cnt, gps->io.nmea);
 770
 771                                 gps_nmea_parse(gps);
 772                         } else {
 773                                 /* There was a checksum, and it was bad. */
 774                                 g_printerr("%s: Bad checksum in NMEA sentence:\n%s\n", __PRETTY_FUNCTION__, gps->io.buffer);
 775                         }
 776                 }
 777
 778                 /* If eol is at or after (_gps_read_buf_curr - 1) */
 779                 if (eol >= (gps->io.curr - 1)) {
 780                         /* Last read was a newline - reset read buffer */
 781                         gps->io.curr = gps->io.buffer;
 782                         *gps->io.curr = '\0';
 783                 } else {
 784                         /* Move the next line to the front of the buffer. */
 785                         memmove(gps->io.buffer, eol + 1, gps->io.curr - eol);   /* include terminating 0 */
 786                         /* Subtract _curr so that it's pointing at the new \0. */
 787                         gps->io.curr -= (eol - gps->io.buffer + 1);
 788                 }
 789         }
 790         gps->io.errors=0;
 791         gps->io.again=0;
 792         break;
 793         case G_IO_STATUS_ERROR:
 794         case G_IO_STATUS_EOF:
 795                 gps_disconnect(gps);
 796                 gps_connect_later(gps);
 797                 gps->io.errors++;
 798                 if (gps->io.errors>10) {
 799                         if (gps->connection_error==NULL)
 800                                 return FALSE;
 801                         else {
 802                                 gps->connection_error(gps, "GPS data read error.");
 803                         }
 804                 }
 805                 return FALSE;
 806         break;
 807         case G_IO_STATUS_AGAIN:
 808                 gps->io.again++;
 809                 if (gps->io.again>20) {
 810                         gps_disconnect(gps);
 811                         gps_connect_later(gps);
 812                         if (gps->connection_error==NULL)
 813                                 return FALSE;
 814                         else {
 815                                 gps->connection_error(gps, "GPS connection deadlock.");
 816                         }
 817                 }
 818                 return TRUE;
 819         break;
 820         default:
 821                 g_assert_not_reached();
 822         break;
 823 }
 824
 825 return TRUE;
 826 }
 827