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