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

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