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 = {0};
  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", 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         default:
 303                 /* Connection failed.  Disconnect and try again later. */
 304                 g_printerr("### Connect failed, retrying... %d %d\n", e, r);
 305                 perror("ERROR: ");
 306                 gps_disconnect(gps);
 307                 gps_connect_later(gps);
 308                 return FALSE;
 309         break;
 310         }
 311 }
 312 return TRUE;
 313 }
 314
 315 /**
 316  * Disconnect the GPS device.
 317  * - Add channel callbacks are removed
 318  * - Channel is close and shutdown
 319  * - File descriptor is closed
 320  * - Anything special for disconnecting is done
 321  */
 322 void
 323 gps_disconnect(Gps *gps)
 324 {
 325 g_assert(gps);
 326
 327 g_debug("GPS: Disconnecting from gps");
 328 /* Remove watches. */
 329 if (gps->io.clater_sid) {
 330         g_source_remove(gps->io.clater_sid);
 331         gps->io.clater_sid=0;
 332 }
 333 if (gps->io.error_sid) {
 334         g_source_remove(gps->io.error_sid);
 335         gps->io.error_sid=0;
 336 }
 337 if (gps->io.connect_sid) {
 338         g_source_remove(gps->io.connect_sid);
 339         gps->io.connect_sid=0;
 340 }
 341 if (gps->io.input_sid) {
 342         g_source_remove(gps->io.input_sid);
 343         gps->io.input_sid=0;
 344 }
 345
 346 /* Destroy the GIOChannel object. */
 347 if (gps->io.channel) {
 348         g_debug("GPS: Shutting down IO channel");
 349         g_io_channel_shutdown(gps->io.channel, FALSE, NULL);
 350         g_io_channel_unref(gps->io.channel);
 351         gps->io.channel=NULL;
 352 }
 353
 354 /* Close the file descriptor. */
 355 if (gps->io.fd!=-1) {
 356         g_debug("GPS: Closing fd %d", gps->io.fd);
 357         close(gps->io.fd);
 358         gps->io.fd=-1;
 359 }
 360
 361 #ifdef WITH_GPSBT
 362 if (gps->io.type==GPS_IO_GPSD) {
 363         gint r;
 364
 365         g_debug("GPS: Requesting gpsd shutdown using gpsbt");
 366         r=gpsbt_stop(&ctx);
 367         if (r!=0)
 368                 g_warning("Failed to stop gpsd\n");
 369 }
 370 #endif
 371
 372 #ifdef WITH_HILDON_DBUS_BT
 373 if (gps->io.type==GPS_IO_HILDON_DBUS && gps->io.rfcomm_req_proxy) {
 374         GError *error = NULL;
 375
 376         g_debug("GPS: Requesting rfcomm disconnection");
 377         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);
 378         error = NULL;
 379         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);
 380 }
 381 #endif
 382
 383 #ifdef WITH_BLUEZ_DBUS_BT
 384 if (gps->io.type==GPS_IO_BLUEZ_DBUS && gps->io.rfcomm_req_proxy) {
 385         GError *error = NULL;
 386
 387 }
 388 #endif
 389
 390 }
 391
 392 static void
 393 gps_simulate_start(Gps *gps)
 394 {
 395 gps->data.lat=60.45;
 396 gps->data.lon=22.26;
 397 gps->io.conn=RCVR_FIXED;
 398 gps->data.fix=FIX_2D;
 399 gps_data_integerize(&gps->data);
 400 }
 401
 402 static gboolean
 403 gps_simulate_move(Gps *gps)
 404 {
 405 static gdouble slat=0, slon=0;
 406 gdouble plat, plon;
 407 gfloat h;
 408 g_assert(gps);
 409
 410 if (g_random_double()<0.5) {
 411         slat=g_random_double_range(-0.0003, 0.0003);
 412         slon=g_random_double_range(-0.0003, 0.0003);
 413 }
 414 plat=gps->data.lat;
 415 plon=gps->data.lon;
 416 gps->data.lat+=slat;
 417 gps->data.lon+=slon;
 418 BOUND(gps->data.lat, -80.0, 80.0);
 419 BOUND(gps->data.lon, -80.0, 80.0);
 420
 421 g_debug("Sim: %f %f\n", gps->data.lat, gps->data.lon);
 422
 423 gps->data.speed=1+g_random_double_range(0.1, 10.0);
 424 h=calculate_course(plat, plon, gps->data.lat, gps->data.lon);
 425 gps->data.heading=(h<0) ? 360+h : h;
 426 gps->data.time=time(NULL);
 427 gps_data_integerize(&gps->data);
 428 if (gps->update_location!=NULL) {
 429         gps->update_location(gps);
 430 }
 431 gps->data.lheading=gps->data.heading;
 432
 433 return gps->io.conn==RCVR_FIXED ? TRUE : FALSE;
 434 }
 435
 436 /**
 437  * Reset GPS read buffer
 438  */
 439 static void
 440 gps_read_buffer_prepare(Gps *gps)
 441 {
 442 gps->io.curr=gps->io.buffer;
 443 *gps->io.curr = '\0';
 444 gps->io.last=gps->io.buffer+GPS_READ_BUF_SIZE-1;
 445 }
 446
 447 /**
 448  * Connect file descriptor to channel and add watches.
 449  */
 450 static void
 451 gps_connect_channel_connect(Gps *gps)
 452 {
 453 g_assert(gps->io.channel==NULL);
 454 gps->io.channel=g_io_channel_unix_new(gps->io.fd);
 455 g_io_channel_set_encoding(gps->io.channel, NULL, NULL);
 456 g_io_channel_set_flags(gps->io.channel, G_IO_FLAG_NONBLOCK, NULL);
 457 g_io_channel_set_buffered(gps->io.channel, FALSE);
 458 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);
 459 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);
 460 gps->io.clater_sid=0;
 461 }
 462
 463 /**
 464  * Connect to the receiver.
 465  * This method assumes that fd is -1 and _channel is NULL.  If unsure, call
 466  * gps_disconnect() first.
 467  * Since this is an idle function, this function returns whether or not it
 468  * should be called again, which is always FALSE.
 469  *
 470  * This function prepares for the connection, the gps_connect does the "connecting" if needed.
 471  */
 472 gboolean
 473 gps_connect_now(Gps *gps)
 474 {
 475 GError *error = NULL;
 476
 477 g_assert(gps);
 478 g_debug("GPS: Connecting GPS type %d to %s:%d\n", gps->io.type, gps->io.address, gps->io.port);
 479
 480 switch (gps->io.type) {
 481         case GPS_IO_FILE:
 482                 if (!gps->io.address)
 483                         return FALSE;
 484                 gps_connect_file(gps, gps->io.address);
 485                 return FALSE;
 486         break;
 487         case GPS_IO_TCP:
 488         case GPS_IO_GPSD:
 489                 if (gps->io.conn<=RCVR_DOWN && gps->io.address) {
 490                         gps->io.fd=socket(AF_INET, SOCK_STREAM, 0);
 491                         if (gps->io.fd==-1) {
 492                                 g_debug("Failed to create socket\n");
 493                                 gps_connect_later(gps);
 494                                 return FALSE;
 495                         }
 496                         g_debug("TCP: Preparing to connect to %s:%d", gps->io.address, gps->io.port);
 497                         gps->io.rcvr_addr_in.sin_family = AF_INET;
 498                         gps->io.rcvr_addr_in.sin_port = htons(gps->io.port);
 499                         if (inet_pton(AF_INET, gps->io.address, &gps->io.rcvr_addr_in.sin_addr.s_addr)<1) {
 500                                 perror("TCP:");
 501                                 return FALSE;
 502                         }
 503                 } else {
 504                         return FALSE;
 505                 }
 506 #ifdef WITH_GPSBT
 507                 if (gps->io.type==GPS_IO_GPSD) {
 508                         gint r;
 509                         gchar ebuf[128];
 510                         r=gpsbt_start(NULL, 0, 0, 0, ebuf, 128, 0, &ctx);
 511                         if (r!=0) {
 512                                 g_warning("Failed to start gpsd");
 513                                 return FALSE;
 514                         } else {
 515                                 /* */
 516                         }
 517                 }
 518 #endif
 519         break;
 520 #ifdef WITH_HILDON_DBUS_BT
 521         case GPS_IO_HILDON_DBUS:
 522                 if (NULL == (gps->io.dbus_conn=dbus_g_bus_get(DBUS_BUS_SYSTEM, &error))) {
 523                         g_printerr("Failed to get D-Bus connection.");
 524                         return FALSE;
 525                 }
 526                 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))) {
 527                         g_printerr("Failed to open connection to %s.\n", BTCOND_REQ_INTERFACE);
 528                         return FALSE;
 529                 }
 530
 531                 if (gps->io.rfcomm_req_proxy) {
 532                         gboolean mybool = TRUE;
 533                         dbus_g_proxy_begin_call(gps->io.rfcomm_req_proxy,
 534                                 BTCOND_RFCOMM_CONNECT_REQ, (DBusGProxyCallNotify)gps_connect_response, gps, NULL,
 535                                 G_TYPE_STRING, gps->io.address,
 536                                 G_TYPE_STRING, "SPP",
 537                                 G_TYPE_BOOLEAN, &mybool,
 538                                 G_TYPE_INVALID);
 539                 }
 540         break;
 541 #endif
 542 #ifdef WITH_BLUEZ
 543         case GPS_IO_RFCOMM:
 544                 if (!gps->io.address)
 545                         return FALSE;
 546
 547                 if (gps->io.conn<=RCVR_DOWN && gps->io.address) {
 548                         gps->io.fd=socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
 549
 550                         /* If file descriptor creation failed, try again later.  Note that
 551                          * there is no need to call rcvr_disconnect() because the file
 552                          * descriptor creation is the first step, so if it fails, there's
 553                          * nothing to clean up. */
 554                         if (gps->io.fd==-1) {
 555                                 g_debug("Failed to create socket\n");
 556                                 gps_connect_later(gps);
 557                                 return FALSE;
 558                         }
 559                         gps->io.rcvr_addr_rc.rc_family=AF_BLUETOOTH;
 560                         gps->io.rcvr_addr_rc.rc_channel=1;
 561                         str2ba(gps->io.address, &gps->io.rcvr_addr_rc.rc_bdaddr);
 562                 }
 563 #endif
 564         break;
 565         case GPS_IO_SIMULATION:
 566                 /* Set a periodic cb to generate random movement */
 567                 gps_simulate_start(gps);
 568                 gps->io.input_sid=g_timeout_add(1000, (GSourceFunc)gps_simulate_move, gps);
 569                 return FALSE;
 570         break;
 571         default:
 572                 g_printerr("Unknown GPS connection type\n");
 573                 return FALSE;
 574         break;
 575 }
 576
 577 gps_read_buffer_prepare(gps);
 578 gps_connect_channel_connect(gps);
 579 gps_connect(gps);
 580 return FALSE;
 581 }
 582
 583 /**
 584  * Connection callback
 585  */
 586 static gboolean
 587 gps_channel_cb_connect(GIOChannel *src, GIOCondition condition, gpointer data)
 588 {
 589 gint error, size = sizeof(error);
 590 Gps *gps=(Gps *)data;
 591
 592 g_assert(data);
 593 g_assert(gps->io.channel);
 594
 595 gps->io.curr=gps->io.buffer;
 596 gps->io.last=gps->io.buffer+GPS_READ_BUF_SIZE-1;
 597
 598 switch (gps->io.type) {
 599         case GPS_IO_TCP:
 600         case GPS_IO_RFCOMM:
 601         case GPS_IO_GPSD:
 602                 if ((getsockopt(gps->io.fd, SOL_SOCKET, SO_ERROR, &error, &size) || error)) {
 603                         g_printerr("Error connecting to receiver; retrying...\n");
 604                         gps_disconnect(gps);
 605                         gps_connect_later(gps);
 606                         gps->io.connect_sid=0;
 607                         gps->io.errors++;
 608                         return FALSE;
 609                 }
 610                 /* Set gpsd to NMEA mode */
 611                 if (gps->io.type==GPS_IO_GPSD) {
 612                         GIOStatus status;
 613                         gsize written;
 614                         GError *error = NULL;
 615
 616                         g_debug("GPS: Requesting NMEA mode from GPSD using cmd %s", GPSD_NMEA);
 617                         status=g_io_channel_write_chars(gps->io.channel, GPSD_NMEA, strlen(GPSD_NMEA), &written, &error);
 618                         if (status==G_IO_STATUS_NORMAL && written==strlen(GPSD_NMEA)) {
 619                                 g_debug("GPS: NMEA mode set (%d %d)", (gint)written, (gint)status);
 620                         } else {
 621                                 g_printerr("Failed to set gpsd to NMEA mode: %d %d %s\n",(gint)status,(gint)written, error->message);
 622                                 g_error_free(error);
 623                                 gps_disconnect(gps);
 624                                 gps_connect_later(gps);
 625                                 gps->io.connect_sid=0;
 626                                 gps->io.errors++;
 627                                 return FALSE;
 628                         }
 629                 }
 630         break;
 631         case GPS_IO_FILE:
 632
 633         break;
 634         default:
 635                 g_warning("Connection from non-connecting source\n");
 636         break;
 637 }
 638
 639 g_printf("Connected to receiver!\n");
 640 gps_conn_set_state(gps, RCVR_UP);
 641 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);
 642 gps->io.errors=0;
 643 gps->io.connect_sid=0;
 644 return FALSE;
 645 }
 646
 647 static gboolean
 648 gps_channel_cb_error(GIOChannel *src, GIOCondition condition, gpointer data)
 649 {
 650 Gps *gps=(Gps *)data;
 651
 652 g_assert(data);
 653 g_debug("GPS: Channel error %d", condition);
 654
 655 /* An error has occurred - re-connect(). */
 656 gps_disconnect(gps);
 657
 658 /* Do error cb */
 659 if (gps->connection_error!=NULL)
 660         gps->connection_error(gps, "GPS Connection error");
 661
 662 if (gps->io.conn > RCVR_OFF) {
 663         gps_conn_set_state(gps, RCVR_DOWN);
 664         gps_connect_later(gps);
 665 }
 666
 667 return FALSE;
 668 }
 669
 670 static gboolean
 671 gps_channel_cb_input(GIOChannel *src, GIOCondition condition, gpointer data)
 672 {
 673 gsize bytes_read;
 674 GIOStatus status;
 675 gchar *eol;
 676 Gps *gps=(Gps *)data;
 677
 678 g_debug("%s(%d)\n", __PRETTY_FUNCTION__, condition);
 679 g_assert(data);
 680
 681 status=g_io_channel_read_chars(gps->io.channel, gps->io.curr, gps->io.last-gps->io.curr, &bytes_read, NULL);
 682 switch (status) {
 683         case G_IO_STATUS_NORMAL:
 684         gps->io.curr += bytes_read;
 685         *gps->io.curr = '\0';   /* append a \0 so we can read as string */
 686         while ((eol = strchr(gps->io.buffer, '\n'))) {
 687                 gchar *sptr = gps->io.buffer + 1;       /* Skip the $ */
 688                 guint csum = 0;
 689                 if (*gps->io.buffer=='$') {
 690                         /* This is the beginning of a sentence; okay to parse. */
 691                         *eol = '\0';    /* overwrite \n with \0 */
 692                         while (*sptr && *sptr != '*')
 693                                 csum ^= *sptr++;
 694
 695                         /* If we're at a \0 (meaning there is no checksum), or if the
 696                          * checksum is good, then parse the sentence. */
 697                         if (!*sptr || csum == strtol(sptr + 1, NULL, 16)) {
 698                                 gchar *data;
 699
 700                                 if (*sptr)
 701                                         *sptr = '\0';   /* take checksum out of the buffer. */
 702
 703                                 gps->io.nmea_cnt++;
 704                                 gps->io.nmea=gps->io.buffer;
 705
 706                                 g_assert(gps->io.nmea);
 707                                 g_debug("NMEA1 %d: (%s)", gps->io.nmea_cnt, gps->io.nmea);
 708
 709                                 gps_nmea_parse(gps);
 710                         } else {
 711                                 /* There was a checksum, and it was bad. */
 712                                 g_printerr("%s: Bad checksum in NMEA sentence:\n%s\n", __PRETTY_FUNCTION__, gps->io.buffer);
 713                         }
 714                 }
 715
 716                 /* If eol is at or after (_gps_read_buf_curr - 1) */
 717                 if (eol >= (gps->io.curr - 1)) {
 718                         /* Last read was a newline - reset read buffer */
 719                         gps->io.curr = gps->io.buffer;
 720                         *gps->io.curr = '\0';
 721                 } else {
 722                         /* Move the next line to the front of the buffer. */
 723                         memmove(gps->io.buffer, eol + 1, gps->io.curr - eol);   /* include terminating 0 */
 724                         /* Subtract _curr so that it's pointing at the new \0. */
 725                         gps->io.curr -= (eol - gps->io.buffer + 1);
 726                 }
 727         }
 728         gps->io.errors=0;
 729         gps->io.again=0;
 730         break;
 731         case G_IO_STATUS_ERROR:
 732         case G_IO_STATUS_EOF:
 733                 gps_disconnect(gps);
 734                 gps_connect_later(gps);
 735                 gps->io.errors++;
 736                 if (gps->io.errors>10) {
 737                         if (gps->connection_error==NULL)
 738                                 return FALSE;
 739                         else {
 740                                 gps->connection_error(gps, "GPS data read error.");
 741                         }
 742                 }
 743                 return FALSE;
 744         break;
 745         case G_IO_STATUS_AGAIN:
 746                 gps->io.again++;
 747                 if (gps->io.again>20) {
 748                         gps_disconnect(gps);
 749                         gps_connect_later(gps);
 750                         if (gps->connection_error==NULL)
 751                                 return FALSE;
 752                         else {
 753                                 gps->connection_error(gps, "GPS connection deadlock.");
 754                         }
 755                 }
 756                 return TRUE;
 757         break;
 758         default:
 759                 g_assert_not_reached();
 760         break;
 761 }
 762
 763 return TRUE;
 764 }
 765