git.maemo.org Git - navit-package/blob - navit/vehicle/gpsd/vehicle_gpsd.c

   1 /**
   2  * Navit, a modular navigation system.
   3  * Copyright (C) 2005-2008 Navit Team
   4  *
   5  * This program is free software; you can redistribute it and/or
   6  * modify it under the terms of the GNU General Public License
   7  * version 2 as published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with this program; if not, write to the
  16  * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
  17  * Boston, MA  02110-1301, USA.
  18  */
  19
  20 #include <config.h>
  21 #include <gps.h>
  22 #include <string.h>
  23 #include <glib.h>
  24 #include <math.h>
  25 #ifdef HAVE_GPSBT
  26 #include <gpsbt.h>
  27 #include <errno.h>
  28 #endif
  29 #include "debug.h"
  30 #include "callback.h"
  31 #include "plugin.h"
  32 #include "coord.h"
  33 #include "item.h"
  34 #include "vehicle.h"
  35 #include "event.h"
  36
  37 static struct vehicle_priv {
  38         char *source;
  39 #ifndef HAVE_LIBGPS19
  40         char *gpsd_query;
  41 #endif
  42         struct callback_list *cbl;
  43         struct callback *cb;
  44         struct event_watch *evwatch;
  45         guint retry_interval;
  46         struct gps_data_t *gps;
  47         struct coord_geo geo;
  48         double speed;
  49         double direction;
  50         double height;
  51         double hdop;
  52         int status;
  53         int fix_type;
  54         time_t fix_time;
  55         int sats;
  56         int sats_signal;
  57         int sats_used;
  58         char *nmea_data;
  59         char *nmea_data_buf;
  60         guint retry_timer;
  61         struct attr ** attrs;
  62         char fixiso8601[128];
  63 #ifdef HAVE_GPSBT
  64         gpsbt_t context;
  65 #endif
  66 } *vehicle_last;
  67
  68 #define DEFAULT_RETRY_INTERVAL 10 // seconds
  69 #define MIN_RETRY_INTERVAL 1 // seconds
  70
  71 static void vehicle_gpsd_io(struct vehicle_priv *priv);
  72
  73 static void
  74 #ifdef HAVE_LIBGPS19
  75 vehicle_gpsd_callback(struct gps_data_t *data, char *buf, size_t len)
  76 #else
  77 vehicle_gpsd_callback(struct gps_data_t *data, char *buf, size_t len,
  78                       int level)
  79 #endif
  80 {
  81         char *pos,*nmea_data_buf;
  82         int i=0,sats_signal=0;
  83
  84         struct vehicle_priv *priv = vehicle_last;
  85         if( len > 0 && buf[0] == '$' ) {
  86                 char buffer[len+2];
  87                 buffer[len+1]='\0';
  88                 memcpy(buffer, buf, len);
  89                 pos=strchr(buffer,'\n');
  90                 if (pos) {
  91                         *++pos='\0';
  92                         if (!priv->nmea_data_buf || strlen(priv->nmea_data_buf) < 65536) {
  93                                 nmea_data_buf=g_strconcat(priv->nmea_data_buf ? priv->nmea_data_buf : "", buffer, NULL);
  94                                 g_free(priv->nmea_data_buf);
  95                                 priv->nmea_data_buf=nmea_data_buf;
  96                         } else {
  97                                 dbg(0, "nmea buffer overflow, discarding '%s'\n", buffer);
  98                         }
  99                 }
 100         }
 101         dbg(1,"data->set=0x%x\n", data->set);
 102         if (data->set & SPEED_SET) {
 103                 priv->speed = data->fix.speed * 3.6;
 104                 if(!isnan(data->fix.speed))
 105                         callback_list_call_attr_0(priv->cbl, attr_position_speed);
 106                 data->set &= ~SPEED_SET;
 107         }
 108         if (data->set & TRACK_SET) {
 109                 priv->direction = data->fix.track;
 110                 data->set &= ~TRACK_SET;
 111         }
 112         if (data->set & ALTITUDE_SET) {
 113                 priv->height = data->fix.altitude;
 114                 data->set &= ~ALTITUDE_SET;
 115         }
 116         if (data->set & SATELLITE_SET) {
 117 #ifdef HAVE_LIBGPS19
 118                 if(data->satellites_visible > 0) {
 119 #else
 120                 if(data->satellites > 0) {
 121 #endif
 122                         sats_signal=0;
 123 #ifdef HAVE_LIBGPS19
 124                         for( i=0;i<data->satellites_visible;i++) {
 125 #else
 126                         for( i=0;i<data->satellites;i++) {
 127 #endif
 128                                if (data->ss[i] > 0)
 129                                         sats_signal++;
 130                         }
 131                 }
 132 #ifdef HAVE_LIBGPS19
 133                 if (priv->sats_used != data->satellites_used || priv->sats != data->satellites_visible || priv->sats_signal != sats_signal ) {
 134 #else
 135                 if (priv->sats_used != data->satellites_used || priv->sats != data->satellites || priv->sats_signal != sats_signal ) {
 136 #endif
 137                         priv->sats_used = data->satellites_used;
 138 #ifdef HAVE_LIBGPS19
 139                         priv->sats = data->satellites_visible;
 140 #else
 141                         priv->sats = data->satellites;
 142 #endif
 143                         priv->sats_signal = sats_signal;
 144                         callback_list_call_attr_0(priv->cbl, attr_position_sats);
 145                 }
 146                 data->set &= ~SATELLITE_SET;
 147         }
 148         if (data->set & STATUS_SET) {
 149                 priv->status = data->status;
 150                 data->set &= ~STATUS_SET;
 151         }
 152         if (data->set & MODE_SET) {
 153                 priv->fix_type = data->fix.mode - 1;
 154                 data->set &= ~MODE_SET;
 155         }
 156         if (data->set & TIME_SET) {
 157                 priv->fix_time = data->fix.time;
 158                 data->set &= ~TIME_SET;
 159         }
 160 #ifdef HAVE_LIBGPS19
 161         if (data->set & DOP_SET) {
 162                 dbg(1, "pdop : %g\n", data->dop.pdop);
 163                 priv->hdop = data->dop.pdop;
 164                 data->set &= ~DOP_SET;
 165 #else
 166         if (data->set & PDOP_SET) {
 167                 dbg(1, "pdop : %g\n", data->pdop);
 168                 priv->hdop = data->hdop;
 169                 data->set &= ~PDOP_SET;
 170 #endif
 171         }
 172         if (data->set & LATLON_SET) {
 173                 priv->geo.lat = data->fix.latitude;
 174                 priv->geo.lng = data->fix.longitude;
 175                 dbg(1,"lat=%f lng=%f\n", priv->geo.lat, priv->geo.lng);
 176                 g_free(priv->nmea_data);
 177                 priv->nmea_data=priv->nmea_data_buf;
 178                 priv->nmea_data_buf=NULL;
 179                 data->set &= ~LATLON_SET;
 180         }
 181         // If data->fix.speed is NAN, then the drawing gets jumpy.
 182         if (! isnan(data->fix.speed) && priv->fix_type > 0) {
 183                 callback_list_call_attr_0(priv->cbl, attr_position_coord_geo);
 184         }
 185         dbg(2,"speed ok\n");
 186 }
 187
 188 /**
 189  * Attempt to open the gps device.
 190  * Return FALSE if retry not required
 191  * Return TRUE to try again
 192  */
 193 static gboolean
 194 vehicle_gpsd_try_open(gpointer *data)
 195 {
 196         struct vehicle_priv *priv = (struct vehicle_priv *)data;
 197         char *source = g_strdup(priv->source);
 198         char *colon = index(source + 7, ':');
 199         char *port=NULL;
 200         if (colon) {
 201                 *colon = '\0';
 202                 port=colon+1;
 203         }
 204         dbg(0,"Trying to connect to %s:%s\n",source+7,port?port:"default");
 205         priv->gps = gps_open(source + 7, port);
 206         g_free(source);
 207
 208         if (!priv->gps){
 209                 dbg(0,"gps_open failed for '%s'. Retrying in %d seconds. Have you started gpsd?\n", priv->source, priv->retry_interval);
 210                 return TRUE;
 211         }
 212 #ifdef HAVE_LIBGPS19
 213         gps_stream(priv->gps, WATCH_ENABLE, NULL);
 214 #else
 215         gps_query(priv->gps, priv->gpsd_query);
 216 #endif
 217         gps_set_raw_hook(priv->gps, vehicle_gpsd_callback);
 218         priv->cb = callback_new_1(callback_cast(vehicle_gpsd_io), priv);
 219         priv->evwatch = event_add_watch((void *)priv->gps->gps_fd, event_watch_cond_read, priv->cb);
 220         if (!priv->gps->gps_fd) {
 221                 dbg(0,"Warning: gps_fd is 0, most likely you have used a gps.h incompatible to libgps");
 222         }
 223         dbg(0,"Connected to gpsd fd=%d evwatch=%p\n", priv->gps->gps_fd, priv->evwatch);
 224         return FALSE;
 225 }
 226
 227 /**
 228  * Open a connection to gpsd. Will re-try the connection if it fails
 229  */
 230 static void
 231 vehicle_gpsd_open(struct vehicle_priv *priv)
 232 {
 233 #ifdef HAVE_GPSBT
 234         char errstr[256] = "";
 235         /* We need to start gpsd (via gpsbt) first. */
 236         errno = 0;
 237         memset(&priv->context, 0, sizeof(gpsbt_t));
 238         if(gpsbt_start(NULL, 0, 0, 0, errstr, sizeof(errstr),
 239                 0, &priv->context) < 0) {
 240                dbg(0,"Error connecting to GPS with gpsbt: (%d) %s (%s)\n",
 241                   errno, strerror(errno), errstr);
 242         }
 243         sleep(1);       /* give gpsd time to start */
 244         dbg(1,"gpsbt_start: completed\n");
 245 #endif
 246         priv->retry_timer=0;
 247         if (vehicle_gpsd_try_open((gpointer *)priv)) {
 248                 priv->retry_timer = g_timeout_add(priv->retry_interval*1000, (GSourceFunc)vehicle_gpsd_try_open, (gpointer *)priv);
 249         }
 250 }
 251
 252 static void
 253 vehicle_gpsd_close(struct vehicle_priv *priv)
 254 {
 255 #ifdef HAVE_GPSBT
 256         int err;
 257 #endif
 258
 259         if (priv->retry_timer) {
 260                 g_source_remove(priv->retry_timer);
 261                 priv->retry_timer=0;
 262         }
 263         if (priv->evwatch) {
 264                 event_remove_watch(priv->evwatch);
 265                 priv->evwatch = NULL;
 266         }
 267         if (priv->cb) {
 268                 callback_destroy(priv->cb);
 269                 priv->cb = NULL;
 270         }
 271         if (priv->gps) {
 272                 gps_close(priv->gps);
 273                 priv->gps = NULL;
 274         }
 275 #ifdef HAVE_GPSBT
 276         err = gpsbt_stop(&priv->context);
 277         if (err < 0) {
 278                 dbg(0,"Error %d while gpsbt_stop", err);
 279         }
 280         dbg(1,"gpsbt_stop: completed, (%d)",err);
 281 #endif
 282 }
 283
 284 static void
 285 vehicle_gpsd_io(struct vehicle_priv *priv)
 286 {
 287         dbg(1, "enter\n");
 288         if (priv->gps) {
 289                 vehicle_last = priv;
 290                 if (gps_poll(priv->gps)) {
 291                         g_warning("gps_poll failed\n");
 292                         vehicle_gpsd_close(priv);
 293                         vehicle_gpsd_open(priv);
 294                 }
 295         }
 296 }
 297
 298 static void
 299 vehicle_gpsd_destroy(struct vehicle_priv *priv)
 300 {
 301         vehicle_gpsd_close(priv);
 302         if (priv->source)
 303                 g_free(priv->source);
 304 #ifndef HAVE_LIBGPS19
 305         if (priv->gpsd_query)
 306                 g_free(priv->gpsd_query);
 307 #endif
 308         g_free(priv);
 309 }
 310
 311 static int
 312 vehicle_gpsd_position_attr_get(struct vehicle_priv *priv,
 313                                enum attr_type type, struct attr *attr)
 314 {
 315         struct attr * active=NULL;
 316         switch (type) {
 317         case attr_position_fix_type:
 318                 attr->u.num = priv->fix_type;
 319                 break;
 320         case attr_position_height:
 321                 attr->u.numd = &priv->height;
 322                 break;
 323         case attr_position_speed:
 324                 attr->u.numd = &priv->speed;
 325                 break;
 326         case attr_position_direction:
 327                 attr->u.numd = &priv->direction;
 328                 break;
 329         case attr_position_hdop:
 330                 attr->u.numd = &priv->hdop;
 331                 break;
 332         case attr_position_qual:
 333                 attr->u.num = priv->sats;
 334                 break;
 335         case attr_position_sats_signal:
 336                 attr->u.num = priv->sats_signal;
 337                 break;
 338         case attr_position_sats_used:
 339                 attr->u.num = priv->sats_used;
 340                 break;
 341         case attr_position_coord_geo:
 342                 attr->u.coord_geo = &priv->geo;
 343                 break;
 344         case attr_position_nmea:
 345                 attr->u.str=priv->nmea_data;
 346                 if (! attr->u.str)
 347                         return 0;
 348                 break;
 349         case attr_position_time_iso8601:
 350                 {
 351                 struct tm tm;
 352                 if (!priv->fix_time)
 353                         return 0;
 354                 if (gmtime_r(&priv->fix_time, &tm)) {
 355                         strftime(priv->fixiso8601, sizeof(priv->fixiso8601),
 356                                 "%Y-%m-%dT%TZ", &tm);
 357                         attr->u.str=priv->fixiso8601;
 358                 } else
 359                         return 0;
 360                 }
 361                 break;
 362         case attr_active:
 363           active = attr_search(priv->attrs,NULL,attr_active);
 364           if(active != NULL) {
 365                 attr->u.num=active->u.num;
 366             return 1;
 367           } else
 368             return 0;
 369                break;
 370         default:
 371                 return 0;
 372         }
 373         attr->type = type;
 374         return 1;
 375 }
 376
 377 struct vehicle_methods vehicle_gpsd_methods = {
 378         vehicle_gpsd_destroy,
 379         vehicle_gpsd_position_attr_get,
 380 };
 381
 382 static struct vehicle_priv *
 383 vehicle_gpsd_new_gpsd(struct vehicle_methods
 384                       *meth, struct callback_list
 385                       *cbl, struct attr **attrs)
 386 {
 387         struct vehicle_priv *ret;
 388 #ifdef HAVE_LIBGPS19
 389         struct attr *source, *retry_int;
 390 #else
 391         struct attr *source, *query, *retry_int;
 392 #endif
 393
 394         dbg(1, "enter\n");
 395         source = attr_search(attrs, NULL, attr_source);
 396         ret = g_new0(struct vehicle_priv, 1);
 397         ret->source = g_strdup(source->u.str);
 398 #ifndef HAVE_LIBGPS19
 399         query = attr_search(attrs, NULL, attr_gpsd_query);
 400         if (query) {
 401                 ret->gpsd_query = g_strconcat(query->u.str, "\n", NULL);
 402         } else {
 403                 ret->gpsd_query = g_strdup("w+x\n");
 404         }
 405         dbg(1,"Format string for gpsd_query: %s\n",ret->gpsd_query);
 406 #endif
 407         retry_int = attr_search(attrs, NULL, attr_retry_interval);
 408         if (retry_int) {
 409                 ret->retry_interval = retry_int->u.num;
 410                 if (ret->retry_interval < MIN_RETRY_INTERVAL) {
 411                         dbg(0, "Retry interval %d too small, setting to %d\n", ret->retry_interval, MIN_RETRY_INTERVAL);
 412                         ret->retry_interval = MIN_RETRY_INTERVAL;
 413                 }
 414         } else {
 415                 dbg(1, "Retry interval not defined, setting to %d\n", DEFAULT_RETRY_INTERVAL);
 416                 ret->retry_interval = DEFAULT_RETRY_INTERVAL;
 417         }
 418         ret->cbl = cbl;
 419         *meth = vehicle_gpsd_methods;
 420         ret->attrs = attrs;
 421         vehicle_gpsd_open(ret);
 422         return ret;
 423 }
 424
 425 void
 426 plugin_init(void)
 427 {
 428         dbg(1, "enter\n");
 429         plugin_register_vehicle_type("gpsd", vehicle_gpsd_new_gpsd);
 430 }