vcs.maemo.org Git - navit-package/blob - navit/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         char *gpsd_query;
  16         struct callback_list *cbl;
  17         GIOChannel *iochan;
  18         guint retry_interval;
  19         guint watch;
  20         struct gps_data_t *gps;
  21         struct coord_geo geo;
  22         double speed;
  23         double direction;
  24         double height;
  25         int status;
  26         int sats;
  27         int sats_used;
  28         char *nmea_data;
  29         char *nmea_data_buf;
  30         guint retry_timer;
  31 } *vehicle_last;
  32
  33 #define DEFAULT_RETRY_INTERVAL 10 // seconds
  34 #define MIN_RETRY_INTERVAL 1 // seconds
  35
  36 static gboolean vehicle_gpsd_io(GIOChannel * iochan,
  37                                 GIOCondition condition, gpointer t);
  38
  39
  40
  41 static void
  42 vehicle_gpsd_callback(struct gps_data_t *data, char *buf, size_t len,
  43                       int level)
  44 {
  45         char *pos,*nmea_data_buf;
  46         struct vehicle_priv *priv = vehicle_last;
  47         if (buf[0] == '$' && len > 0) {
  48                 char buffer[len+2];
  49                 buffer[len+1]='\0';
  50                 memcpy(buffer, buf, len);
  51                 pos=strchr(buffer,'\n');
  52                 if (pos) {
  53                         *++pos='\0';
  54                         if (!priv->nmea_data_buf || strlen(priv->nmea_data_buf) < 65536) {
  55                                 nmea_data_buf=g_strconcat(priv->nmea_data_buf ? priv->nmea_data_buf : "", buffer, NULL);
  56                                 g_free(priv->nmea_data_buf);
  57                                 priv->nmea_data_buf=nmea_data_buf;
  58                         } else {
  59                                 dbg(0, "nmea buffer overflow, discarding '%s'\n", buffer);
  60                         }
  61                 }
  62         }
  63         dbg(1,"data->set=0x%x\n", data->set);
  64         // If data->fix.speed is NAN, then the drawing gets jumpy.
  65         if (isnan(data->fix.speed)) {
  66                 return;
  67         }
  68         dbg(2,"speed ok\n");
  69         if (data->set & SPEED_SET) {
  70                 priv->speed = data->fix.speed * 3.6;
  71                 data->set &= ~SPEED_SET;
  72         }
  73         if (data->set & TRACK_SET) {
  74                 priv->direction = data->fix.track;
  75                 data->set &= ~TRACK_SET;
  76         }
  77         if (data->set & ALTITUDE_SET) {
  78                 priv->height = data->fix.altitude;
  79                 data->set &= ~ALTITUDE_SET;
  80         }
  81         if (data->set & SATELLITE_SET) {
  82                 priv->sats_used = data->satellites_used;
  83                 priv->sats = data->satellites;
  84                 data->set &= ~SATELLITE_SET;
  85         }
  86         if (data->set & STATUS_SET) {
  87                 priv->status = data->status;
  88                 data->set &= ~STATUS_SET;
  89         }
  90         if (data->set & PDOP_SET) {
  91                 dbg(0, "pdop : %g\n", data->pdop);
  92                 data->set &= ~PDOP_SET;
  93         }
  94         if (data->set & LATLON_SET) {
  95                 priv->geo.lat = data->fix.latitude;
  96                 priv->geo.lng = data->fix.longitude;
  97                 dbg(1,"lat=%f lng=%f\n", priv->geo.lat, priv->geo.lng);
  98                 g_free(priv->nmea_data);
  99                 priv->nmea_data=priv->nmea_data_buf;
 100                 priv->nmea_data_buf=NULL;
 101                 callback_list_call_0(priv->cbl);
 102                 data->set &= ~LATLON_SET;
 103         }
 104 }
 105
 106 /**
 107  * Attempt to open the gps device.
 108  * Return FALSE if retry not required
 109  * Return TRUE to try again
 110  */
 111 static gboolean
 112 vehicle_gpsd_try_open(gpointer *data)
 113 {
 114         struct vehicle_priv *priv = (struct vehicle_priv *)data;
 115         char *source = g_strdup(priv->source);
 116         char *colon = index(source + 7, ':');
 117         if (colon) {
 118                 *colon = '\0';
 119                 priv->gps = gps_open(source + 7, colon + 1);
 120         } else
 121                 priv->gps = gps_open(source + 7, NULL);
 122         g_free(source);
 123
 124         if (!priv->gps){
 125                 g_warning("gps_open failed for '%s'. Retrying in %d seconds. Have you started gpsd?\n", priv->source, priv->retry_interval);
 126                 return TRUE;
 127         }
 128         gps_query(priv->gps, priv->gpsd_query);
 129         gps_set_raw_hook(priv->gps, vehicle_gpsd_callback);
 130         priv->iochan = g_io_channel_unix_new(priv->gps->gps_fd);
 131         priv->watch =
 132             g_io_add_watch(priv->iochan, G_IO_IN | G_IO_ERR | G_IO_HUP,
 133                            vehicle_gpsd_io, priv);
 134         dbg(0,"Connected to gpsd fd=%d iochan=%p watch=%p\n", priv->gps->gps_fd, priv->iochan, priv->watch);
 135         return FALSE;
 136 }
 137
 138 /**
 139  * Open a connection to gpsd. Will re-try the connection if it fails
 140  */
 141 static void
 142 vehicle_gpsd_open(struct vehicle_priv *priv)
 143 {
 144         priv->retry_timer=0;
 145         if (vehicle_gpsd_try_open((gpointer *)priv)) {
 146                 priv->retry_timer = g_timeout_add(priv->retry_interval*1000, (GSourceFunc)vehicle_gpsd_try_open, (gpointer *)priv);
 147         }
 148 }
 149
 150 static void
 151 vehicle_gpsd_close(struct vehicle_priv *priv)
 152 {
 153         GError *error = NULL;
 154
 155         if (priv->watch) {
 156                 g_source_remove(priv->watch);
 157                 priv->watch = 0;
 158         }
 159         if (priv->retry_timer) {
 160                 g_source_remove(priv->retry_timer);
 161                 priv->retry_timer=0;
 162         }
 163         if (priv->iochan) {
 164                 g_io_channel_shutdown(priv->iochan, 0, &error);
 165                 priv->iochan = NULL;
 166         }
 167         if (priv->gps) {
 168                 gps_close(priv->gps);
 169                 priv->gps = NULL;
 170         }
 171 }
 172
 173 static gboolean
 174 vehicle_gpsd_io(GIOChannel * iochan, GIOCondition condition, gpointer t)
 175 {
 176         struct vehicle_priv *priv = t;
 177
 178         dbg(1, "enter condition=%d\n", condition);
 179         if (condition == G_IO_IN) {
 180                 if (priv->gps) {
 181                         vehicle_last = priv;
 182                         if (gps_poll(priv->gps)) {
 183                                g_warning("gps_poll failed\n");
 184                                vehicle_gpsd_close(priv);
 185                                vehicle_gpsd_open(priv);
 186                         }
 187                 }
 188                 return TRUE;
 189         }
 190         return FALSE;
 191 }
 192
 193 static void
 194 vehicle_gpsd_destroy(struct vehicle_priv *priv)
 195 {
 196         vehicle_gpsd_close(priv);
 197         if (priv->source)
 198                 g_free(priv->source);
 199         if (priv->gpsd_query)
 200                 g_free(priv->gpsd_query);
 201         g_free(priv);
 202 }
 203
 204 static int
 205 vehicle_gpsd_position_attr_get(struct vehicle_priv *priv,
 206                                enum attr_type type, struct attr *attr)
 207 {
 208         switch (type) {
 209         case attr_position_height:
 210                 attr->u.numd = &priv->height;
 211                 break;
 212         case attr_position_speed:
 213                 attr->u.numd = &priv->speed;
 214                 break;
 215         case attr_position_direction:
 216                 attr->u.numd = &priv->direction;
 217                 break;
 218         case attr_position_sats:
 219                 attr->u.num = priv->sats;
 220                 break;
 221         case attr_position_sats_used:
 222                 attr->u.num = priv->sats_used;
 223                 break;
 224         case attr_position_coord_geo:
 225                 attr->u.coord_geo = &priv->geo;
 226                 break;
 227         case attr_position_nmea:
 228                 attr->u.str=priv->nmea_data;
 229                 if (! attr->u.str)
 230                         return 0;
 231                 break;
 232         default:
 233                 return 0;
 234         }
 235         attr->type = type;
 236         return 1;
 237 }
 238
 239 struct vehicle_methods vehicle_gpsd_methods = {
 240         vehicle_gpsd_destroy,
 241         vehicle_gpsd_position_attr_get,
 242 };
 243
 244 static struct vehicle_priv *
 245 vehicle_gpsd_new_gpsd(struct vehicle_methods
 246                       *meth, struct callback_list
 247                       *cbl, struct attr **attrs)
 248 {
 249         struct vehicle_priv *ret;
 250         struct attr *source, *query, *retry_int;
 251
 252         dbg(1, "enter\n");
 253         source = attr_search(attrs, NULL, attr_source);
 254         ret = g_new0(struct vehicle_priv, 1);
 255         ret->source = g_strdup(source->u.str);
 256         query = attr_search(attrs, NULL, attr_gpsd_query);
 257         if (query) {
 258                 ret->gpsd_query = g_strconcat(query->u.str, "\n", NULL);
 259         } else {
 260                 ret->gpsd_query = g_strdup("w+x\n");
 261         }
 262         dbg(1,"Format string for gpsd_query: %s\n",ret->gpsd_query);
 263         retry_int = attr_search(attrs, NULL, attr_retry_interval);
 264         if (retry_int) {
 265                 ret->retry_interval = retry_int->u.num;
 266                 if (ret->retry_interval < MIN_RETRY_INTERVAL) {
 267                         dbg(0, "Retry interval %d too small, setting to %d\n", ret->retry_interval, MIN_RETRY_INTERVAL);
 268                         ret->retry_interval = MIN_RETRY_INTERVAL;
 269                 }
 270         } else {
 271                 dbg(0, "Retry interval not defined, setting to %d\n", DEFAULT_RETRY_INTERVAL);
 272                 ret->retry_interval = DEFAULT_RETRY_INTERVAL;
 273         }
 274         ret->cbl = cbl;
 275         *meth = vehicle_gpsd_methods;
 276         vehicle_gpsd_open(ret);
 277         return ret;
 278 }
 279
 280 void
 281 plugin_init(void)
 282 {
 283         dbg(1, "enter\n");
 284         plugin_register_vehicle_type("gpsd", vehicle_gpsd_new_gpsd);
 285 }