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=60.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 void
 297 gps_simulate_start(Gps *gps)
 298 {
 299 gps->data.lat=60.45;
 300 gps->data.lon=22.26;
 301 gps->io.conn=RCVR_FIXED;
 302 gps->data.fix=FIX_2D;
 303 gps_data_integerize(&gps->data);
 304 }
 305
 306 static gboolean
 307 gps_simulate_move(Gps *gps)
 308 {
 309 static gdouble slat=0, slon=0;
 310 gdouble plat, plon;
 311 g_assert(gps);
 312
 313 if (g_random_double()<0.5) {
 314         slat=g_random_double_range(-0.0003, 0.0003);
 315         slon=g_random_double_range(-0.0003, 0.0003);
 316 }
 317 plat=gps->data.lat;
 318 plon=gps->data.lon;
 319 gps->data.lat+=slat;
 320 gps->data.lon+=slon;
 321 BOUND(gps->data.lat, -80.0, 80.0);
 322 BOUND(gps->data.lon, -80.0, 80.0);
 323
 324 g_debug("Sim: %f %f\n", gps->data.lat, gps->data.lon);
 325
 326 gps->data.speed=1+g_random_double_range(0.1, 10.0);
 327 gps->data.heading=calculate_course(plat, plon, gps->data.lat, gps->data.lon);
 328 gps->data.time=time(NULL);
 329 gps_data_integerize(&gps->data);
 330 track_add(&gps->data, FALSE);
 331 gps->data.lheading=gps->data.heading;
 332 map_refresh_mark();
 333
 334 return gps->io.conn==RCVR_FIXED ? TRUE : FALSE;
 335 }
 336
 337 /**
 338  * Reset GPS read buffer
 339  */
 340 static void
 341 gps_read_buffer_prepare(Gps *gps)
 342 {
 343 gps->io.curr=gps->io.buffer;
 344 *gps->io.curr = '\0';
 345 gps->io.last=gps->io.buffer+GPS_READ_BUF_SIZE-1;
 346 }
 347
 348 /**
 349  * Connect file descriptor to channel and add watches.
 350  */
 351 static void
 352 gps_read_channel_connect(Gps *gps)
 353 {
 354 gps->io.channel=g_io_channel_unix_new(gps->io.fd);
 355 g_io_channel_set_encoding(gps->io.channel, NULL, NULL);
 356 g_io_channel_set_flags(gps->io.channel, G_IO_FLAG_NONBLOCK, NULL);
 357 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);
 358 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);
 359 gps->io.clater_sid=0;
 360 }
 361
 362 /**
 363  * Connect to the receiver.
 364  * This method assumes that fd is -1 and _channel is NULL.  If unsure, call
 365  * gps_disconnect() first.
 366  * Since this is an idle function, this function returns whether or not it
 367  * should be called again, which is always FALSE.
 368  *
 369  * This function prepares for the connection, the gps_connect does the "connecting" if needed.
 370  */
 371 gboolean
 372 gps_connect_now(Gps *gps)
 373 {
 374 g_assert(gps);
 375 g_debug("GPS: Connecting GPS type %d to %s:%d\n", gps->io.type, gps->io.address, gps->io.port);
 376
 377 switch (gps->io.type) {
 378         case GPS_IO_FILE:
 379                 if (!gps->io.address)
 380                         return FALSE;
 381                 if (*gps->io.address=='/')
 382                         gps->io.fd=open(gps->io.address, O_RDONLY);
 383                 else
 384                         return FALSE;
 385         break;
 386         case GPS_IO_TCP:
 387         case GPS_IO_GPSD:
 388                 if (gps->io.conn==RCVR_DOWN && gps->io.address) {
 389                         gps->io.fd=socket(AF_INET, SOCK_STREAM, 0);
 390                         if (gps->io.fd==-1) {
 391                                 g_debug("Failed to create socket\n");
 392                                 gps_connect_later(gps);
 393                                 return FALSE;
 394                         }
 395                         memcpy(&gps->io.rcvr_addr_in.sin_addr.s_addr, gps->io.address, strlen(gps->io.address));
 396                         gps->io.rcvr_addr_in.sin_family = AF_INET;
 397                         gps->io.rcvr_addr_in.sin_port = htons(gps->io.port);
 398                 } else {
 399                         return FALSE;
 400                 }
 401 #ifdef WITH_GPSBT
 402                 if (gps->io.type==GPS_IO_GPSD) {
 403                         gint r;
 404                         gchar ebuf[128];
 405                         r=gpsbt_start(NULL, 0, 0, 0, ebuf, 128, 0, &ctx);
 406                         if (r!=0) {
 407                                 g_warning("Failed to start gpsd");
 408                                 return FALSE;
 409                         }
 410                 }
 411 #endif
 412         break;
 413         case GPS_IO_HILDON_DBUS:
 414 #ifdef WITH_HILDON_DBUS_BT
 415                 if (gps->io.rfcomm_req_proxy) {
 416                         gboolean mybool = TRUE;
 417                         dbus_g_proxy_begin_call(_rfcomm_req_proxy,
 418                                 BTCOND_RFCOMM_CONNECT_REQ, (DBusGProxyCallNotify)gps_connect_response, NULL, NULL,
 419                                 G_TYPE_STRING, gps->io.address,
 420                                 G_TYPE_STRING, "SPP",
 421                                 G_TYPE_BOOLEAN, &mybool,
 422                                 G_TYPE_INVALID);
 423                 }
 424 #else
 425                 return FALSE;
 426 #endif
 427         break;
 428         case GPS_IO_RFCOMM:
 429 #ifdef WITH_BLUEZ
 430                 if (!gps->io.address)
 431                         return FALSE;
 432
 433                 if (gps->io.conn==RCVR_DOWN && gps->io.address) {
 434                         gps->io.fd=socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
 435
 436                         /* If file descriptor creation failed, try again later.  Note that
 437                          * there is no need to call rcvr_disconnect() because the file
 438                          * descriptor creation is the first step, so if it fails, there's
 439                          * nothing to clean up. */
 440                         if (gps->io.fd==-1) {
 441                                 g_debug("Failed to create socket\n");
 442                                 gps_connect_later(gps);
 443                                 return FALSE;
 444                         }
 445                         gps->io.rcvr_addr_rc.rc_family=AF_BLUETOOTH;
 446                         gps->io.rcvr_addr_rc.rc_channel=1;
 447                         str2ba(gps->io.address, &gps->io.rcvr_addr_rc.rc_bdaddr);
 448                 }
 449 #endif
 450                 return FALSE;
 451         break;
 452         case GPS_IO_SIMULATION:
 453                 /* Set a periodic cb to generate random movement */
 454                 gps_simulate_start(gps);
 455                 gps->io.input_sid=g_timeout_add(1000, (GSourceFunc)gps_simulate_move, gps);
 456                 return FALSE;
 457         break;
 458         default:
 459                 return FALSE;
 460         break;
 461 }
 462
 463 gps_read_buffer_prepare(gps);
 464 gps_read_channel_connect(gps);
 465 gps_connect(gps);
 466 return FALSE;
 467 }
 468
 469 /**
 470  * Connection callback
 471  */
 472 static gboolean
 473 gps_channel_cb_connect(GIOChannel * src, GIOCondition condition, gpointer data)
 474 {
 475 gint error, size = sizeof(error);
 476 Gps *gps=(Gps *)data;
 477
 478 g_assert(data);
 479
 480 gps->io.curr=gps->io.buffer;
 481 gps->io.last=gps->io.buffer+GPS_READ_BUF_SIZE-1;
 482
 483 switch (gps->io.type) {
 484         case GPS_IO_TCP:
 485         case GPS_IO_RFCOMM:
 486         case GPS_IO_GPSD:
 487                 if ((getsockopt(gps->io.fd, SOL_SOCKET, SO_ERROR, &error, &size) || error)) {
 488                         g_printerr("Error connecting to receiver; retrying...\n");
 489                         gps_disconnect(gps);
 490                         gps_connect_later(gps);
 491                         gps->io.connect_sid=0;
 492                         gps->io.errors++;
 493                         return FALSE;
 494                 }
 495                 /* Set gpsd to NMEA mode */
 496                 if (gps->io.type==GPS_IO_GPSD) {
 497                         GIOStatus status;
 498                         gsize written;
 499                         status=g_io_channel_write_chars(gps->io.channel, GPSD_NMEA, -1, &written, NULL);
 500                         if (status!=G_IO_STATUS_NORMAL || written!=sizeof(GPSD_NMEA)) {
 501                                 g_printerr("Failed to set gpsd to NMEA mode\n");
 502                                 gps_disconnect(gps);
 503                                 gps_connect_later(gps);
 504                                 gps->io.connect_sid=0;
 505                                 gps->io.errors++;
 506                                 return FALSE;
 507                         }
 508                 }
 509         break;
 510         case GPS_IO_FILE:
 511
 512         break;
 513         default:
 514                 g_warning("Connection from non-connecting source\n");
 515         break;
 516 }
 517
 518 g_printf("Connected to receiver!\n");
 519 gps_conn_set_state(gps, RCVR_UP);
 520 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);
 521 gps->io.errors=0;
 522 gps->io.connect_sid=0;
 523 return FALSE;
 524 }
 525
 526 static gboolean
 527 gps_channel_cb_error(GIOChannel *src, GIOCondition condition, gpointer data)
 528 {
 529 Gps *gps=(Gps *)data;
 530
 531 g_assert(data);
 532 /* An error has occurred - re-connect(). */
 533 gps_disconnect(gps);
 534 track_add(NULL, FALSE);
 535 /* _speed_excess = FALSE; */
 536
 537 if (gps->io.conn > RCVR_OFF) {
 538         gps_conn_set_state(gps, RCVR_DOWN);
 539         gps_connect_later(gps);
 540 }
 541 return FALSE;
 542 }
 543
 544 static gboolean
 545 gps_channel_cb_input(GIOChannel *src, GIOCondition condition, gpointer data)
 546 {
 547 gsize bytes_read;
 548 GIOStatus status;
 549 gchar *eol;
 550 Gps *gps=(Gps *)data;
 551
 552 g_debug("%s(%d)\n", __PRETTY_FUNCTION__, condition);
 553 g_assert(data);
 554
 555 status=g_io_channel_read_chars(gps->io.channel, gps->io.curr, gps->io.last-gps->io.curr, &bytes_read, NULL);
 556 switch (status) {
 557         case G_IO_STATUS_NORMAL:
 558         gps->io.curr += bytes_read;
 559         *gps->io.curr = '\0';   /* append a \0 so we can read as string */
 560         while ((eol = strchr(gps->io.buffer, '\n'))) {
 561                 gchar *sptr = gps->io.buffer + 1;       /* Skip the $ */
 562                 guint csum = 0;
 563                 if (*gps->io.buffer=='$') {
 564                         /* This is the beginning of a sentence; okay to parse. */
 565                         *eol = '\0';    /* overwrite \n with \0 */
 566                         while (*sptr && *sptr != '*')
 567                                 csum ^= *sptr++;
 568
 569                         /* If we're at a \0 (meaning there is no checksum), or if the
 570                          * checksum is good, then parse the sentence. */
 571                         if (!*sptr || csum == strtol(sptr + 1, NULL, 16)) {
 572                                 gchar *data;
 573
 574                                 if (*sptr)
 575                                         *sptr = '\0';   /* take checksum out of the buffer. */
 576
 577                                 data=g_strdup(gps->io.buffer);
 578                                 g_idle_add_full(G_PRIORITY_HIGH_IDLE, (GSourceFunc)gps_nmea_parse, data, NULL);
 579                         } else {
 580                                 /* There was a checksum, and it was bad. */
 581                                 g_printerr("%s: Bad checksum in NMEA sentence:\n%s\n", __PRETTY_FUNCTION__, gps->io.buffer);
 582                         }
 583                 }
 584
 585                 /* If eol is at or after (_gps_read_buf_curr - 1) */
 586                 if (eol >= (gps->io.curr - 1)) {
 587                         /* Last read was a newline - reset read buffer */
 588                         gps->io.curr = gps->io.buffer;
 589                         *gps->io.curr = '\0';
 590                 } else {
 591                         /* Move the next line to the front of the buffer. */
 592                         memmove(gps->io.buffer, eol + 1, gps->io.curr - eol);   /* include terminating 0 */
 593                         /* Subtract _curr so that it's pointing at the new \0. */
 594                         gps->io.curr -= (eol - gps->io.buffer + 1);
 595                 }
 596         }
 597         break;
 598         case G_IO_STATUS_ERROR:
 599         case G_IO_STATUS_EOF:
 600                 gps_disconnect(gps);
 601                 gps_connect_later(gps);
 602                 gps->io.errors++;
 603                 return FALSE;
 604         break;
 605         case G_IO_STATUS_AGAIN:
 606                 return TRUE;
 607         break;
 608         default:
 609                 g_assert_not_reached();
 610         break;
 611 }
 612
 613 return TRUE;
 614 }
 615