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

   1 #include <config.h>
   2 #include <gps.h>
   3 #include <string.h>
   4 #include <glib.h>
   5 #include <math.h>
   6 #include "debug.h"
   7 #include "callback.h"
   8 #include "plugin.h"
   9 #include "coord.h"
  10 #include "item.h"
  11 #include "vehicle.h"
  12
  13 static struct vehicle_priv {
  14         char *source;
  15         struct callback_list *cbl;
  16         GIOChannel *iochan;
  17         guint watch;
  18         struct gps_data_t *gps;
  19         struct coord_geo geo;
  20         double speed;
  21         double direction;
  22         double height;
  23         int status;
  24         int sats;
  25         int sats_used;
  26 } *vehicle_last;
  27
  28 static gboolean vehicle_gpsd_io(GIOChannel * iochan,
  29                                 GIOCondition condition, gpointer t);
  30
  31
  32
  33 static void
  34 vehicle_gpsd_callback(struct gps_data_t *data, char *buf, size_t len,
  35                       int level)
  36 {
  37         struct vehicle_priv *priv = vehicle_last;
  38         // If data->fix.speed is NAN, then the drawing gets jumpy.
  39         if (isnan(data->fix.speed)) {
  40                 return;
  41         }
  42
  43         if (data->set & SPEED_SET) {
  44                 priv->speed = data->fix.speed * 3.6;
  45                 data->set &= ~SPEED_SET;
  46         }
  47         if (data->set & TRACK_SET) {
  48                 priv->direction = data->fix.track;
  49                 data->set &= ~TRACK_SET;
  50         }
  51         if (data->set & ALTITUDE_SET) {
  52                 priv->height = data->fix.altitude;
  53                 data->set &= ~ALTITUDE_SET;
  54         }
  55         if (data->set & SATELLITE_SET) {
  56                 priv->sats_used = data->satellites_used;
  57                 priv->sats = data->satellites;
  58                 data->set &= ~SATELLITE_SET;
  59         }
  60         if (data->set & STATUS_SET) {
  61                 priv->status = data->status;
  62                 data->set &= ~STATUS_SET;
  63         }
  64         if (data->set & PDOP_SET) {
  65                 dbg(0, "pdop : %g\n", data->pdop);
  66                 data->set &= ~PDOP_SET;
  67         }
  68         if (data->set & LATLON_SET) {
  69                 priv->geo.lat = data->fix.latitude;
  70                 priv->geo.lng = data->fix.longitude;
  71                 dbg(1,"lat=%f lng=%f\n", priv->geo.lat, priv->geo.lng);
  72                 callback_list_call_0(priv->cbl);
  73                 data->set &= ~LATLON_SET;
  74         }
  75 }
  76
  77 static int
  78 vehicle_gpsd_open(struct vehicle_priv *priv)
  79 {
  80         char *source = g_strdup(priv->source);
  81         char *colon = index(source + 7, ':');
  82         if (colon) {
  83                 *colon = '\0';
  84                 priv->gps = gps_open(source + 7, colon + 1);
  85         } else
  86                 priv->gps = gps_open(source + 7, NULL);
  87         g_free(source);
  88         if (!priv->gps)
  89                 return 0;
  90         gps_query(priv->gps, "w+x\n");
  91         gps_set_raw_hook(priv->gps, vehicle_gpsd_callback);
  92         priv->iochan = g_io_channel_unix_new(priv->gps->gps_fd);
  93         priv->watch =
  94             g_io_add_watch(priv->iochan, G_IO_IN | G_IO_ERR | G_IO_HUP,
  95                            vehicle_gpsd_io, priv);
  96         return 1;
  97 }
  98
  99 static void
 100 vehicle_gpsd_close(struct vehicle_priv *priv)
 101 {
 102         GError *error = NULL;
 103
 104         if (priv->watch) {
 105                 g_source_remove(priv->watch);
 106                 priv->watch = 0;
 107         }
 108         if (priv->iochan) {
 109                 g_io_channel_shutdown(priv->iochan, 0, &error);
 110                 priv->iochan = NULL;
 111         }
 112         if (priv->gps) {
 113                 gps_close(priv->gps);
 114                 priv->gps = NULL;
 115         }
 116 }
 117
 118 static gboolean
 119 vehicle_gpsd_io(GIOChannel * iochan, GIOCondition condition, gpointer t)
 120 {
 121         struct vehicle_priv *priv = t;
 122
 123         dbg(1, "enter condition=%d\n", condition);
 124         if (condition == G_IO_IN) {
 125                 if (priv->gps) {
 126                         vehicle_last = priv;
 127                         gps_poll(priv->gps);
 128                 }
 129                 return TRUE;
 130         }
 131         return FALSE;
 132 }
 133
 134 static void
 135 vehicle_gpsd_destroy(struct vehicle_priv *priv)
 136 {
 137         vehicle_gpsd_close(priv);
 138         if (priv->source)
 139                 g_free(priv->source);
 140         g_free(priv);
 141 }
 142
 143 static int
 144 vehicle_gpsd_position_attr_get(struct vehicle_priv *priv,
 145                                enum attr_type type, struct attr *attr)
 146 {
 147         switch (type) {
 148         case attr_position_height:
 149                 attr->u.numd = &priv->height;
 150                 break;
 151         case attr_position_speed:
 152                 attr->u.numd = &priv->speed;
 153                 break;
 154         case attr_position_direction:
 155                 attr->u.numd = &priv->direction;
 156                 break;
 157         case attr_position_sats:
 158                 attr->u.num = priv->sats;
 159                 break;
 160         case attr_position_sats_used:
 161                 attr->u.num = priv->sats_used;
 162                 break;
 163         case attr_position_coord_geo:
 164                 attr->u.coord_geo = &priv->geo;
 165                 break;
 166         default:
 167                 return 0;
 168         }
 169         attr->type = type;
 170         return 1;
 171 }
 172
 173 struct vehicle_methods vehicle_gpsd_methods = {
 174         vehicle_gpsd_destroy,
 175         vehicle_gpsd_position_attr_get,
 176 };
 177
 178 static struct vehicle_priv *
 179 vehicle_gpsd_new_gpsd(struct vehicle_methods
 180                       *meth, struct callback_list
 181                       *cbl, struct attr **attrs)
 182 {
 183         struct vehicle_priv *ret;
 184         struct attr *source;
 185
 186         dbg(1, "enter\n");
 187         source = attr_search(attrs, NULL, attr_source);
 188         ret = g_new0(struct vehicle_priv, 1);
 189         ret->source = g_strdup(source->u.str);
 190         ret->cbl = cbl;
 191         *meth = vehicle_gpsd_methods;
 192         if (vehicle_gpsd_open(ret))
 193                 return ret;
 194         dbg(0, "Failed to open '%s'\n", ret->source);
 195         vehicle_gpsd_destroy(ret);
 196         return NULL;
 197 }
 198
 199 void
 200 plugin_init(void)
 201 {
 202         dbg(1, "enter\n");
 203         plugin_register_vehicle_type("gpsd", vehicle_gpsd_new_gpsd);
 204 }