vcs.maemo.org Git - navit-package/blob - src/vehicle/file/vehicle_file.c

   1 #include <stdio.h>
   2 #include <unistd.h>
   3 #include <fcntl.h>
   4 #include <string.h>
   5 #include <glib.h>
   6 #include <sys/stat.h>
   7 #include <termios.h>
   8 #include <math.h>
   9 #include "debug.h"
  10 #include "callback.h"
  11 #include "plugin.h"
  12 #include "coord.h"
  13 #include "item.h"
  14 #include "vehicle.h"
  15
  16 static void vehicle_file_disable_watch(struct vehicle_priv *priv);
  17 static void vehicle_file_enable_watch(struct vehicle_priv *priv);
  18
  19 enum file_type {
  20         file_type_pipe = 1, file_type_device, file_type_file
  21 };
  22
  23 static int buffer_size = 256;
  24
  25 struct vehicle_priv {
  26         char *source;
  27         enum file_type file_type;
  28         struct callback_list *cbl;
  29         int fd;
  30         FILE *file;
  31         guint watch;
  32         GIOChannel *iochan;
  33         char *buffer;
  34         int buffer_pos;
  35
  36         struct coord_geo geo;
  37         double speed;
  38         double direction;
  39         double height;
  40         int status;
  41         int sats_used;
  42 };
  43
  44 static int
  45 vehicle_file_open(struct vehicle_priv *priv)
  46 {
  47         char *name;
  48         struct stat st;
  49         struct termios tio;
  50
  51         name = priv->source + 5;
  52         if (!strncmp(priv->source, "file:", 5)) {
  53                 priv->fd = open(name, O_RDONLY | O_NDELAY);
  54                 if (priv->fd < 0)
  55                         return 0;
  56                 stat(name, &st);
  57                 if (S_ISREG(st.st_mode)) {
  58                         priv->file_type = file_type_file;
  59                 } else {
  60                         tcgetattr(priv->fd, &tio);
  61                         cfmakeraw(&tio);
  62                         cfsetispeed(&tio, B4800);
  63                         cfsetospeed(&tio, B4800);
  64                         tio.c_cc[VMIN] = 16;
  65                         tio.c_cc[VTIME] = 1;
  66                         tcsetattr(priv->fd, TCSANOW, &tio);
  67                         priv->file_type = file_type_device;
  68                 }
  69         } else {
  70                 priv->file = popen(name, "r");
  71                 if (!priv->file)
  72                         return 0;
  73                 priv->fd = fileno(priv->file);
  74                 priv->file_type = file_type_pipe;
  75         }
  76         priv->iochan = g_io_channel_unix_new(priv->fd);
  77         return 1;
  78 }
  79
  80 static void
  81 vehicle_file_close(struct vehicle_priv *priv)
  82 {
  83         GError *error = NULL;
  84         if (priv->iochan) {
  85                 g_io_channel_shutdown(priv->iochan, 0, &error);
  86                 priv->iochan = NULL;
  87         }
  88         if (priv->file)
  89                 pclose(priv->file);
  90         else if (priv->fd >= 0)
  91                 close(priv->fd);
  92         priv->file = NULL;
  93         priv->fd = -1;
  94 }
  95
  96 static int
  97 vehicle_file_enable_watch_timer(gpointer t)
  98 {
  99         struct vehicle_priv *priv = t;
 100         vehicle_file_enable_watch(priv);
 101         dbg(1, "enter\n");
 102
 103         return FALSE;
 104 }
 105
 106
 107 static void
 108 vehicle_file_parse(struct vehicle_priv *priv, char *buffer)
 109 {
 110         char *p, *item[16];
 111         double lat, lng;
 112         int i, bcsum;
 113         int len = strlen(buffer);
 114         unsigned char csum = 0;
 115
 116         dbg(1, "buffer='%s'\n", buffer);
 117         for (;;) {
 118                 if (len < 4) {
 119                         dbg(0, "too short\n");
 120                         return;
 121                 }
 122                 if (buffer[len - 1] == '\r' || buffer[len - 1] == '\n')
 123                         buffer[--len] = '\0';
 124                 else
 125                         break;
 126         }
 127         if (buffer[0] != '$') {
 128                 dbg(0, "no leading $\n");
 129                 return;
 130         }
 131         if (buffer[len - 3] != '*') {
 132                 dbg(0, "no *XX\n");
 133                 return;
 134         }
 135         for (i = 1; i < len - 3; i++) {
 136                 csum ^= (unsigned char) (buffer[i]);
 137         }
 138         if (!sscanf(buffer + len - 2, "%x", &bcsum)) {
 139                 dbg(0, "no checksum\n");
 140                 return;
 141         }
 142         if (bcsum != csum) {
 143                 dbg(0, "wrong checksum\n");
 144                 return;
 145         }
 146
 147         i = 0;
 148         p = buffer;
 149         while (i < 16) {
 150                 item[i++] = p;
 151                 while (*p && *p != ',')
 152                         p++;
 153                 if (!*p)
 154                         break;
 155                 *p++ = '\0';
 156         }
 157
 158         if (!strncmp(buffer, "$GPGGA", 6)) {
 159                 /*                                                           1 1111
 160                    0      1          2         3 4          5 6 7  8   9     0 1234
 161                    $GPGGA,184424.505,4924.2811,N,01107.8846,E,1,05,2.5,408.6,M,,,,0000*0C
 162                    UTC of Fix[1],Latitude[2],N/S[3],Longitude[4],E/W[5],Quality(0=inv,1=gps,2=dgps)[6],Satelites used[7],
 163                    HDOP[8],Altitude[9],"M"[10],height of geoid[11], "M"[12], time since dgps update[13], dgps ref station [14]
 164                  */
 165                 sscanf(item[2], "%lf", &lat);
 166                 priv->geo.lat = floor(lat / 100);
 167                 lat -= priv->geo.lat * 100;
 168                 priv->geo.lat += lat / 60;
 169
 170                 sscanf(item[4], "%lf", &lng);
 171                 priv->geo.lng = floor(lng / 100);
 172                 lng -= priv->geo.lng * 100;
 173                 priv->geo.lng += lng / 60;
 174
 175                 sscanf(item[6], "%d", &priv->status);
 176                 sscanf(item[7], "%d", &priv->sats_used);
 177                 sscanf(item[9], "%lf", &priv->height);
 178
 179                 callback_list_call_0(priv->cbl);
 180                 if (priv->file_type == file_type_file) {
 181                         vehicle_file_disable_watch(priv);
 182                         g_timeout_add(1000,
 183                                       vehicle_file_enable_watch_timer,
 184                                       priv);
 185                 }
 186         }
 187         if (!strncmp(buffer, "$GPVTG", 6)) {
 188                 /* 0      1      2 34 5    6 7   8
 189                    $GPVTG,143.58,T,,M,0.26,N,0.5,K*6A
 190                    Course Over Ground Degrees True[1],"T"[2],Course Over Ground Degrees Magnetic[3],"M"[4],
 191                    Speed in Knots[5],"N"[6],"Speed in KM/H"[7],"K"[8]
 192                  */
 193                 sscanf(item[1], "%lf", &priv->direction);
 194                 sscanf(item[7], "%lf", &priv->speed);
 195         }
 196         if (!strncmp(buffer, "$GPRMC", 6)) {
 197                 /*                                                           1     1
 198                    0      1      2 3        4 5         6 7     8     9      0     1
 199                    $GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A
 200                    Time[1],Active/Void[2],lat[3],N/S[4],long[5],W/E[6],speed in knots[7],track angle[8],date[9],
 201                    magnetic variation[10],magnetic variation direction[11]
 202                  */
 203                 sscanf(item[8], "%lf", &priv->direction);
 204                 sscanf(item[7], "%lf", &priv->speed);
 205                 priv->speed *= 1.852;
 206         }
 207 }
 208
 209 static gboolean
 210 vehicle_file_io(GIOChannel * iochan, GIOCondition condition, gpointer t)
 211 {
 212         struct vehicle_priv *priv = t;
 213         int size;
 214         char *str, *tok;
 215
 216         dbg(1, "enter condition=%d\n", condition);
 217         if (condition == G_IO_IN) {
 218                 size =
 219                     read(g_io_channel_unix_get_fd(iochan),
 220                          priv->buffer + priv->buffer_pos,
 221                          buffer_size - priv->buffer_pos - 1);
 222                 if (size <= 0) {
 223                         vehicle_file_close(priv);
 224                         vehicle_file_open(priv);
 225                         return TRUE;
 226                 }
 227                 priv->buffer_pos += size;
 228                 priv->buffer[priv->buffer_pos] = '\0';
 229                 dbg(1, "size=%d pos=%d buffer='%s'\n", size,
 230                     priv->buffer_pos, priv->buffer);
 231                 str = priv->buffer;
 232                 while ((tok = index(str, '\n'))) {
 233                         *tok++ = '\0';
 234                         dbg(1, "line='%s'\n", str);
 235                         vehicle_file_parse(priv, str);
 236                         str = tok;
 237                 }
 238                 if (str != priv->buffer) {
 239                         size = priv->buffer + priv->buffer_pos - str;
 240                         memmove(priv->buffer, str, size + 1);
 241                         priv->buffer_pos = size;
 242                         dbg(1, "now pos=%d buffer='%s'\n",
 243                             priv->buffer_pos, priv->buffer);
 244                 } else if (priv->buffer_pos == buffer_size - 1) {
 245                         dbg(0,
 246                             "Overflow. Most likely wrong baud rate or no nmea protocol\n");
 247                         priv->buffer_pos = 0;
 248                 }
 249                 return TRUE;
 250         }
 251         return FALSE;
 252 }
 253
 254 static void
 255 vehicle_file_enable_watch(struct vehicle_priv *priv)
 256 {
 257         priv->watch =
 258             g_io_add_watch(priv->iochan, G_IO_IN | G_IO_ERR | G_IO_HUP,
 259                            vehicle_file_io, priv);
 260 }
 261
 262 static void
 263 vehicle_file_disable_watch(struct vehicle_priv *priv)
 264 {
 265         if (priv->watch)
 266                 g_source_remove(priv->watch);
 267         priv->watch = 0;
 268 }
 269
 270
 271 static void
 272 vehicle_file_destroy(struct vehicle_priv *priv)
 273 {
 274         vehicle_file_close(priv);
 275         if (priv->source)
 276                 g_free(priv->source);
 277         if (priv->buffer)
 278                 g_free(priv->buffer);
 279         g_free(priv);
 280 }
 281
 282 static int
 283 vehicle_file_position_attr_get(struct vehicle_priv *priv,
 284                                enum attr_type type, struct attr *attr)
 285 {
 286         switch (type) {
 287         case attr_position_height:
 288                 attr->u.numd = &priv->height;
 289                 break;
 290         case attr_position_speed:
 291                 attr->u.numd = &priv->speed;
 292                 break;
 293         case attr_position_direction:
 294                 attr->u.numd = &priv->direction;
 295                 break;
 296         case attr_position_sats_used:
 297                 attr->u.num = priv->sats_used;
 298                 break;
 299         case attr_position_coord_geo:
 300                 attr->u.coord_geo = &priv->geo;
 301                 break;
 302         default:
 303                 return 0;
 304         }
 305         attr->type = type;
 306         return 1;
 307 }
 308
 309 struct vehicle_methods vehicle_file_methods = {
 310         vehicle_file_destroy,
 311         vehicle_file_position_attr_get,
 312 };
 313
 314 static struct vehicle_priv *
 315 vehicle_file_new_file(struct vehicle_methods
 316                       *meth, struct callback_list
 317                       *cbl, struct attr **attrs)
 318 {
 319         struct vehicle_priv *ret;
 320         struct attr *source;
 321
 322         dbg(1, "enter\n");
 323         source = attr_search(attrs, NULL, attr_source);
 324         ret = g_new0(struct vehicle_priv, 1);
 325         ret->fd = -1;
 326         ret->cbl = cbl;
 327         ret->source = g_strdup(source->u.str);
 328         ret->buffer = g_malloc(buffer_size);
 329         *meth = vehicle_file_methods;
 330         if (vehicle_file_open(ret)) {
 331                 vehicle_file_enable_watch(ret);
 332                 return ret;
 333         }
 334         dbg(0, "Failed to open '%s'\n", ret->source);
 335         vehicle_file_destroy(ret);
 336         return NULL;
 337 }
 338
 339 void
 340 plugin_init(void)
 341 {
 342         dbg(1, "enter\n");
 343         plugin_register_vehicle_type("file", vehicle_file_new_file);
 344         plugin_register_vehicle_type("pipe", vehicle_file_new_file);
 345 }