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 #ifdef _WIN32
   8     #include <serial_io.h>
   9 #else
  10 #include <termios.h>
  11 #endif
  12 #include <math.h>
  13 #include "config.h"
  14 #include "debug.h"
  15 #include "callback.h"
  16 #include "plugin.h"
  17 #include "coord.h"
  18 #include "item.h"
  19 #include "vehicle.h"
  20
  21 static void vehicle_file_disable_watch(struct vehicle_priv *priv);
  22 static void vehicle_file_enable_watch(struct vehicle_priv *priv);
  23 static void vehicle_file_parse(struct vehicle_priv *priv, char *buffer);
  24 static int vehicle_file_open(struct vehicle_priv *priv);
  25 static void vehicle_file_close(struct vehicle_priv *priv);
  26
  27
  28 enum file_type {
  29         file_type_pipe = 1, file_type_device, file_type_file
  30 };
  31
  32 static int buffer_size = 256;
  33
  34 struct vehicle_priv {
  35         char *source;
  36         enum file_type file_type;
  37         struct callback_list *cbl;
  38         int fd;
  39         FILE *file;
  40         guint watch;
  41         GIOChannel *iochan;
  42         char *buffer;
  43         int buffer_pos;
  44         char *nmea_data;
  45         char *nmea_data_buf;
  46
  47         struct coord_geo geo;
  48         double speed;
  49         double direction;
  50         double height;
  51         int status;
  52         int sats_used;
  53         int time;
  54         int on_eof;
  55 #ifdef _WIN32
  56         int no_data_count;
  57 #endif
  58 };
  59
  60 #ifdef _WIN32
  61 static int vehicle_win32_serial_track(struct vehicle_priv *priv)
  62 {
  63
  64     static char buffer[2048] = {0,};
  65     static int current_index = 0;
  66     const int chunk_size = 1024;
  67
  68     if ( priv->no_data_count > 5 )
  69     {
  70         vehicle_file_close( priv );
  71         priv->no_data_count = 0;
  72     }
  73
  74     if ( priv->fd <= 0 )
  75     {
  76         vehicle_file_open( priv );
  77     }
  78
  79     if ( current_index >= ( sizeof( buffer ) - chunk_size ) )
  80     {
  81         // discard
  82         current_index = 0;
  83         memset( buffer, 0 , sizeof( buffer ) );
  84     }
  85
  86     int dwBytes = serial_io_read( priv->fd, &buffer[ current_index ], chunk_size );
  87     if ( dwBytes > 0 )
  88     {
  89         current_index += dwBytes;
  90
  91         char* return_pos = NULL;
  92         while ( ( return_pos = strchr( buffer, '\n' ) ) != NULL )
  93         {
  94             char return_buffer[1024];
  95             int bytes_to_copy = return_pos - buffer + 1;
  96             memcpy( return_buffer, buffer, bytes_to_copy );
  97             return_buffer[ bytes_to_copy + 1 ] = '\0';
  98
  99             // printf( "received %d : '%s' bytes to copy\n", bytes_to_copy, return_buffer );
 100             vehicle_file_parse( priv, return_buffer );
 101
 102             current_index -= bytes_to_copy;
 103             memmove( buffer, &buffer[ bytes_to_copy ] , sizeof( buffer ) - bytes_to_copy );
 104         }
 105
 106     }
 107     else
 108     {
 109         priv->no_data_count++;
 110     }
 111     return 1;
 112 }
 113 #endif
 114
 115 static int
 116 vehicle_file_open(struct vehicle_priv *priv)
 117 {
 118 #ifdef _WIN32
 119     dbg(1, "enter vehicle_file_open, priv->source='%s'\n", priv->source);
 120
 121     if ( priv->source )
 122     {
 123         char* raw_setting_str = g_strdup( priv->source );
 124
 125         char* strport = strchr(raw_setting_str, ':' );
 126         char* strsettings = strchr(raw_setting_str, ' ' );
 127
 128         if ( strport && strsettings )
 129         {
 130             strport++;
 131             *strsettings = '\0';
 132             strsettings++;
 133
 134             dbg(1, "calling serial_io_init('%s', '%s')\n", strport, strsettings );
 135             priv->fd=serial_io_init( strport, strsettings );
 136         }
 137         g_free( raw_setting_str );
 138     }
 139 #else
 140         char *name;
 141         struct stat st;
 142         struct termios tio;
 143
 144         name = priv->source + 5;
 145         if (!strncmp(priv->source, "file:", 5)) {
 146                 priv->fd = open(name, O_RDONLY | O_NDELAY);
 147                 if (priv->fd < 0)
 148                         return 0;
 149                 stat(name, &st);
 150                 if (S_ISREG(st.st_mode)) {
 151                         priv->file_type = file_type_file;
 152                 } else {
 153                         tcgetattr(priv->fd, &tio);
 154                         cfmakeraw(&tio);
 155                         cfsetispeed(&tio, B4800);
 156                         cfsetospeed(&tio, B4800);
 157                         tio.c_cc[VMIN] = 16;
 158                         tio.c_cc[VTIME] = 1;
 159                         tcsetattr(priv->fd, TCSANOW, &tio);
 160                         priv->file_type = file_type_device;
 161                 }
 162         } else {
 163                 priv->file = popen(name, "r");
 164                 if (!priv->file)
 165                         return 0;
 166                 priv->fd = fileno(priv->file);
 167                 priv->file_type = file_type_pipe;
 168         }
 169         priv->iochan = g_io_channel_unix_new(priv->fd);
 170 #endif
 171         return 1;
 172 }
 173
 174 static void
 175 vehicle_file_close(struct vehicle_priv *priv)
 176 {
 177 #ifdef _WIN32
 178     serial_io_shutdown( priv->fd );
 179 #else
 180         GError *error = NULL;
 181         if (priv->iochan) {
 182                 g_io_channel_shutdown(priv->iochan, 0, &error);
 183                 priv->iochan = NULL;
 184         }
 185         if (priv->file)
 186                 pclose(priv->file);
 187         else if (priv->fd >= 0)
 188                 close(priv->fd);
 189 #endif
 190         priv->file = NULL;
 191         priv->fd = -1;
 192 }
 193
 194 static int
 195 vehicle_file_enable_watch_timer(gpointer t)
 196 {
 197         struct vehicle_priv *priv = t;
 198         vehicle_file_enable_watch(priv);
 199         dbg(1, "enter\n");
 200
 201         return FALSE;
 202 }
 203
 204
 205 static void
 206 vehicle_file_parse(struct vehicle_priv *priv, char *buffer)
 207 {
 208         char *nmea_data_buf, *p, *item[16];
 209         double lat, lng;
 210         int i, bcsum;
 211         int len = strlen(buffer);
 212         unsigned char csum = 0;
 213
 214         dbg(1, "buffer='%s'\n", buffer);
 215         for (;;) {
 216                 if (len < 4) {
 217                         dbg(0, "'%s' too short\n", buffer);
 218                         return;
 219                 }
 220                 if (buffer[len - 1] == '\r' || buffer[len - 1] == '\n')
 221                         buffer[--len] = '\0';
 222                 else
 223                         break;
 224         }
 225         if (buffer[0] != '$') {
 226                 dbg(0, "no leading $ in '%s'\n", buffer);
 227                 return;
 228         }
 229         if (buffer[len - 3] != '*') {
 230                 dbg(0, "no *XX in '%s'\n", buffer);
 231                 return;
 232         }
 233         for (i = 1; i < len - 3; i++) {
 234                 csum ^= (unsigned char) (buffer[i]);
 235         }
 236         if (!sscanf(buffer + len - 2, "%x", &bcsum)) {
 237                 dbg(0, "no checksum in '%s'\n", buffer);
 238                 return;
 239         }
 240         if (bcsum != csum) {
 241                 dbg(0, "wrong checksum in '%s'\n", buffer);
 242                 return;
 243         }
 244
 245         if (!priv->nmea_data_buf || strlen(priv->nmea_data_buf) < 65536) {
 246                 nmea_data_buf=g_strconcat(priv->nmea_data_buf ? priv->nmea_data_buf : "", buffer, "\n", NULL);
 247                 g_free(priv->nmea_data_buf);
 248                 priv->nmea_data_buf=nmea_data_buf;
 249         } else {
 250                 dbg(0, "nmea buffer overflow, discarding '%s'\n", buffer);
 251         }
 252         i = 0;
 253         p = buffer;
 254         while (i < 16) {
 255                 item[i++] = p;
 256                 while (*p && *p != ',')
 257                         p++;
 258                 if (!*p)
 259                         break;
 260                 *p++ = '\0';
 261         }
 262
 263         if (!strncmp(buffer, "$GPGGA", 6)) {
 264                 /*                                                           1 1111
 265                    0      1          2         3 4          5 6 7  8   9     0 1234
 266                    $GPGGA,184424.505,4924.2811,N,01107.8846,E,1,05,2.5,408.6,M,,,,0000*0C
 267                    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],
 268                    HDOP[8],Altitude[9],"M"[10],height of geoid[11], "M"[12], time since dgps update[13], dgps ref station [14]
 269                  */
 270                 lat = g_ascii_strtod(item[2], NULL);
 271                 priv->geo.lat = floor(lat / 100);
 272                 lat -= priv->geo.lat * 100;
 273                 priv->geo.lat += lat / 60;
 274
 275                 if (!strcasecmp(item[3],"S"))
 276                         priv->geo.lat=-priv->geo.lat;
 277
 278                 lng = g_ascii_strtod(item[4], NULL);
 279                 priv->geo.lng = floor(lng / 100);
 280                 lng -= priv->geo.lng * 100;
 281                 priv->geo.lng += lng / 60;
 282
 283                 if (!strcasecmp(item[5],"W"))
 284                         priv->geo.lng=-priv->geo.lng;
 285
 286                 sscanf(item[6], "%d", &priv->status);
 287                 sscanf(item[7], "%d", &priv->sats_used);
 288                 priv->height = g_ascii_strtod(item[9], NULL);
 289                 g_free(priv->nmea_data);
 290                 priv->nmea_data=priv->nmea_data_buf;
 291                 priv->nmea_data_buf=NULL;
 292
 293                 callback_list_call_0(priv->cbl);
 294
 295 #ifndef _WIN32
 296                 if (priv->file_type == file_type_file) {
 297                         vehicle_file_disable_watch(priv);
 298                         g_timeout_add(priv->time,
 299                                       vehicle_file_enable_watch_timer,
 300                                       priv);
 301                 }
 302 #endif
 303         }
 304         if (!strncmp(buffer, "$GPVTG", 6)) {
 305                 /* 0      1      2 34 5    6 7   8
 306                    $GPVTG,143.58,T,,M,0.26,N,0.5,K*6A
 307                    Course Over Ground Degrees True[1],"T"[2],Course Over Ground Degrees Magnetic[3],"M"[4],
 308                    Speed in Knots[5],"N"[6],"Speed in KM/H"[7],"K"[8]
 309                  */
 310                 priv->direction = g_ascii_strtod( item[1], NULL );
 311                 priv->speed = g_ascii_strtod( item[7], NULL );
 312         }
 313         if (!strncmp(buffer, "$GPRMC", 6)) {
 314                 /*                                                           1     1
 315                    0      1      2 3        4 5         6 7     8     9      0     1
 316                    $GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A
 317                    Time[1],Active/Void[2],lat[3],N/S[4],long[5],W/E[6],speed in knots[7],track angle[8],date[9],
 318                    magnetic variation[10],magnetic variation direction[11]
 319                  */
 320                 priv->direction = g_ascii_strtod( item[8], NULL );
 321                 priv->speed = g_ascii_strtod( item[7], NULL );
 322                 priv->speed *= 1.852;
 323         }
 324 }
 325
 326 #ifndef _WIN32
 327 static gboolean
 328 vehicle_file_io(GIOChannel * iochan, GIOCondition condition, gpointer t)
 329 {
 330         struct vehicle_priv *priv = t;
 331         int size;
 332         char *str, *tok;
 333
 334         dbg(1, "enter condition=%d\n", condition);
 335         if (condition == G_IO_IN) {
 336                 size =
 337                     read(g_io_channel_unix_get_fd(iochan),
 338                          priv->buffer + priv->buffer_pos,
 339                          buffer_size - priv->buffer_pos - 1);
 340                 if (size <= 0) {
 341                         switch (priv->on_eof) {
 342                         case 0:
 343                                 vehicle_file_close(priv);
 344                                 vehicle_file_open(priv);
 345                                 break;
 346                         case 1:
 347                                 break;
 348                         case 2:
 349                                 exit(0);
 350                                 break;
 351                         }
 352                         return TRUE;
 353                 }
 354                 priv->buffer_pos += size;
 355                 priv->buffer[priv->buffer_pos] = '\0';
 356                 dbg(1, "size=%d pos=%d buffer='%s'\n", size,
 357                     priv->buffer_pos, priv->buffer);
 358                 str = priv->buffer;
 359                 while ((tok = index(str, '\n'))) {
 360                         *tok++ = '\0';
 361                         dbg(1, "line='%s'\n", str);
 362                         vehicle_file_parse(priv, str);
 363                         str = tok;
 364                 }
 365                 if (str != priv->buffer) {
 366                         size = priv->buffer + priv->buffer_pos - str;
 367                         memmove(priv->buffer, str, size + 1);
 368                         priv->buffer_pos = size;
 369                         dbg(1, "now pos=%d buffer='%s'\n",
 370                             priv->buffer_pos, priv->buffer);
 371                 } else if (priv->buffer_pos == buffer_size - 1) {
 372                         dbg(0,
 373                             "Overflow. Most likely wrong baud rate or no nmea protocol\n");
 374                         priv->buffer_pos = 0;
 375                 }
 376                 return TRUE;
 377         }
 378         return FALSE;
 379 }
 380 #endif
 381
 382 static void
 383 vehicle_file_enable_watch(struct vehicle_priv *priv)
 384 {
 385 #ifdef _WIN32
 386         g_timeout_add(500, vehicle_win32_serial_track, priv);
 387 #else
 388         priv->watch =
 389             g_io_add_watch(priv->iochan, G_IO_IN | G_IO_ERR | G_IO_HUP,
 390                            vehicle_file_io, priv);
 391 #endif
 392 }
 393
 394 static void
 395 vehicle_file_disable_watch(struct vehicle_priv *priv)
 396 {
 397 #ifndef _WIN32
 398         if (priv->watch)
 399                 g_source_remove(priv->watch);
 400         priv->watch = 0;
 401 #endif
 402 }
 403
 404
 405 static void
 406 vehicle_file_destroy(struct vehicle_priv *priv)
 407 {
 408         vehicle_file_close(priv);
 409         if (priv->source)
 410                 g_free(priv->source);
 411         if (priv->buffer)
 412                 g_free(priv->buffer);
 413         g_free(priv);
 414 }
 415
 416 static int
 417 vehicle_file_position_attr_get(struct vehicle_priv *priv,
 418                                enum attr_type type, struct attr *attr)
 419 {
 420         switch (type) {
 421         case attr_position_height:
 422                 attr->u.numd = &priv->height;
 423                 break;
 424         case attr_position_speed:
 425                 attr->u.numd = &priv->speed;
 426                 break;
 427         case attr_position_direction:
 428                 attr->u.numd = &priv->direction;
 429                 break;
 430         case attr_position_sats_used:
 431                 attr->u.num = priv->sats_used;
 432                 break;
 433         case attr_position_coord_geo:
 434                 attr->u.coord_geo = &priv->geo;
 435                 break;
 436         case attr_position_nmea:
 437                 attr->u.str=priv->nmea_data;
 438                 if (! attr->u.str)
 439                         return 0;
 440                 break;
 441         default:
 442                 return 0;
 443         }
 444         attr->type = type;
 445         return 1;
 446 }
 447
 448 struct vehicle_methods vehicle_file_methods = {
 449         vehicle_file_destroy,
 450         vehicle_file_position_attr_get,
 451 };
 452
 453 static struct vehicle_priv *
 454 vehicle_file_new_file(struct vehicle_methods
 455                       *meth, struct callback_list
 456                       *cbl, struct attr **attrs)
 457 {
 458         struct vehicle_priv *ret;
 459         struct attr *source;
 460         struct attr *time;
 461         struct attr *on_eof;
 462
 463         dbg(1, "enter\n");
 464         source = attr_search(attrs, NULL, attr_source);
 465         ret = g_new0(struct vehicle_priv, 1);
 466         ret->fd = -1;
 467         ret->cbl = cbl;
 468         ret->source = g_strdup(source->u.str);
 469         ret->buffer = g_malloc(buffer_size);
 470         ret->time=1000;
 471         time = attr_search(attrs, NULL, attr_time);
 472         if (time)
 473                 ret->time=time->u.num;
 474         on_eof = attr_search(attrs, NULL, attr_on_eof);
 475         if (on_eof && !strcasecmp(on_eof->u.str, "stop"))
 476                 ret->on_eof=1;
 477         if (on_eof && !strcasecmp(on_eof->u.str, "exit"))
 478                 ret->on_eof=2;
 479         dbg(0,"on_eof=%d\n", ret->on_eof);
 480         *meth = vehicle_file_methods;
 481         if (vehicle_file_open(ret)) {
 482                 vehicle_file_enable_watch(ret);
 483                 return ret;
 484         }
 485
 486 #ifdef _WIN32
 487     ret->no_data_count = 0;
 488 #endif
 489         dbg(0, "Failed to open '%s'\n", ret->source);
 490         vehicle_file_destroy(ret);
 491         return NULL;
 492 }
 493
 494 void
 495 plugin_init(void)
 496 {
 497         dbg(1, "enter\n");
 498         plugin_register_vehicle_type("file", vehicle_file_new_file);
 499         plugin_register_vehicle_type("pipe", vehicle_file_new_file);
 500 }