"Fossies" - the Fresh Open Source Software Archive

Member "navit-0.5.6/navit/track.c" (6 Mar 2021, 36698 Bytes) of package /linux/privat/navit-0.5.6.tar.gz:


As a special service "Fossies" has tried to format the requested source page into HTML format using (guessed) C and C++ source code syntax highlighting (style: standard) with prefixed line numbers and code folding option. Alternatively you can here view or download the uninterpreted source code file. For more information about "track.c" see the Fossies "Dox" file reference documentation and the latest Fossies "Diffs" side-by-side code changes report: 0.5.5_vs_0.5.6.

    1 /**
    2  * Navit, a modular navigation system.
    3  * Copyright (C) 2005-2018 Navit Team
    4  *
    5  * This program is free software; you can redistribute it and/or
    6  * modify it under the terms of the GNU General Public License
    7  * version 2 as published by the Free Software Foundation.
    8  *
    9  * This program is distributed in the hope that it will be useful,
   10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
   11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   12  * GNU General Public License for more details.
   13  *
   14  * You should have received a copy of the GNU General Public License
   15  * along with this program; if not, write to the
   16  * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
   17  * Boston, MA  02110-1301, USA.
   18  */
   19 
   20 #include <glib.h>
   21 #include <string.h>
   22 #include <time.h>
   23 #include <math.h>
   24 #include "item.h"
   25 #include "attr.h"
   26 #include "track.h"
   27 #include "xmlconfig.h"
   28 #include "debug.h"
   29 #include "transform.h"
   30 #include "coord.h"
   31 #include "route.h"
   32 #include "projection.h"
   33 #include "map.h"
   34 #include "mapset.h"
   35 #include "plugin.h"
   36 #include "vehicleprofile.h"
   37 #include "vehicle.h"
   38 #include "roadprofile.h"
   39 #include "util.h"
   40 #include "config.h"
   41 #include "callback.h"
   42 
   43 struct object_func tracking_func;
   44 
   45 struct tracking_line {
   46     struct street_data *street;
   47     struct tracking_line *next;
   48     int angle[0];
   49 };
   50 
   51 
   52 /**
   53  * @brief Conatins a list of previous speeds
   54  *
   55  * This structure is used to hold a list of previously reported
   56  * speeds. This data is used by the CDF.
   57  */
   58 struct cdf_speed {
   59     struct cdf_speed *next;
   60     int speed;
   61     time_t time;
   62 };
   63 
   64 /**
   65  * @brief Contains data for the CDF
   66  *
   67  * This structure holds all data needed by the
   68  * cumulative displacement filter.
   69  */
   70 struct cdf_data {
   71     int extrapolating;
   72     int available;
   73     int first_pos;
   74     int poscount;
   75     int hist_size;
   76     struct cdf_speed *speed_hist;
   77     struct pcoord *pos_hist;
   78     int *dir_hist;
   79     double last_dist;
   80     struct pcoord last_out;
   81     int last_dir;
   82 };
   83 
   84 struct tracking {
   85     NAVIT_OBJECT
   86     struct callback_list *callback_list;     /**< Callbacks which will be called whenever the position changes */
   87     struct mapset *ms;                       /**< The mapset */
   88     struct route *rt;                        /**< The route */
   89     struct map *map;                         /**< The tracking map which holds our past movements */
   90     struct vehicle *vehicle;                 /**< The vehicle from which we are obtaining location data */
   91     struct vehicleprofile *vehicleprofile;   /**< The current vehicle profile */
   92     struct coord last_updated;
   93     struct tracking_line *lines;
   94     struct tracking_line *curr_line;
   95     int pos;
   96     struct coord curr[2], curr_in, curr_out;
   97     int curr_angle;
   98     struct coord last[2], last_in, last_out;
   99     struct cdf_data cdf;
  100     struct attr *attr;
  101     int valid;                               /**< Whether we have valid location data */
  102     int time;
  103     double direction, direction_matched;
  104     double speed;                            /**< Current speed */
  105     int coord_geo_valid;
  106     struct coord_geo coord_geo;
  107     enum projection pro;
  108     int street_direction;
  109     int no_gps;
  110     int tunnel;                              /**< Whether we are in a tunnel */
  111     int angle_pref;
  112     int connected_pref;
  113     int nostop_pref;
  114     int offroad_limit_pref;
  115     int route_pref;
  116     int overspeed_pref;
  117     int overspeed_percent_pref;
  118     int tunnel_extrapolation;
  119 };
  120 
  121 
  122 
  123 
  124 static void tracking_init_cdf(struct cdf_data *cdf, int hist_size) {
  125     cdf->extrapolating = 0;
  126     cdf->available = 0;
  127     cdf->poscount = 0;
  128     cdf->last_dist = 0;
  129     cdf->hist_size = hist_size;
  130 
  131     cdf->pos_hist = g_new0(struct pcoord, hist_size);
  132     cdf->dir_hist = g_new0(int, hist_size);
  133 }
  134 
  135 // Variables for finetuning the CDF
  136 
  137 // Minimum average speed
  138 #define CDF_MINAVG 1.f
  139 // Maximum average speed
  140 #define CDF_MAXAVG 6.f // only ~ 20 km/h
  141 // We need a low value here because otherwise we would extrapolate whenever we are not accelerating
  142 
  143 // Mininum distance (square of it..), below which we ignore gps updates
  144 #define CDF_MINDIST 49 // 7 meters, I guess this value has to be changed for pedestrians.
  145 
  146 #if 0
  147 static void tracking_process_cdf(struct cdf_data *cdf, struct pcoord *pin, struct pcoord *pout, int dirin, int *dirout,
  148                                  int cur_speed, time_t fixtime) {
  149     struct cdf_speed *speed,*sc,*sl;
  150     double speed_avg;
  151     int speed_num,i;
  152 
  153     if (cdf->hist_size == 0) {
  154         dbg(lvl_warning,"No CDF.");
  155         *pout = *pin;
  156         *dirout = dirin;
  157         return;
  158     }
  159 
  160     speed = g_new0(struct cdf_speed, 1);
  161     speed->speed = cur_speed;
  162     speed->time = fixtime;
  163 
  164     speed->next = cdf->speed_hist;
  165     cdf->speed_hist = speed;
  166 
  167     sc = speed;
  168     sl = NULL;
  169     speed_num = 0;
  170     speed_avg = 0;
  171     while (sc && ((fixtime - speed->time) < 4)) { // FIXME static maxtime
  172         speed_num++;
  173         speed_avg += sc->speed;
  174         sl = sc;
  175         sc = sc->next;
  176     }
  177 
  178     speed_avg /= (double)speed_num;
  179 
  180     if (sl) {
  181         sl->next = NULL;
  182     }
  183 
  184     while (sc) {
  185         sl = sc->next;
  186         g_free(sc);
  187         sc = sl;
  188     }
  189 
  190     if (speed_avg < CDF_MINAVG) {
  191         speed_avg = CDF_MINAVG;
  192     } else if (speed_avg > CDF_MAXAVG) {
  193         speed_avg = CDF_MAXAVG;
  194     }
  195 
  196 
  197     if (cur_speed >= speed_avg) {
  198         if (cdf->extrapolating) {
  199             cdf->poscount = 0;
  200             cdf->extrapolating = 0;
  201         }
  202 
  203         cdf->first_pos--;
  204         if (cdf->first_pos < 0) {
  205             cdf->first_pos = cdf->hist_size - 1;
  206         }
  207 
  208         if (cdf->poscount < cdf->hist_size) {
  209             cdf->poscount++;
  210         }
  211 
  212         cdf->pos_hist[cdf->first_pos] = *pin;
  213         cdf->dir_hist[cdf->first_pos] = dirin;
  214 
  215         *pout = *pin;
  216         *dirout = dirin;
  217     } else if (cdf->poscount > 0) {
  218 
  219         double mx,my; // Average position's x and y values
  220         double sx,sy; // Support vector
  221         double dx,dy; // Difference between average and current position
  222         double len;   // Length of support vector
  223         double dist;
  224 
  225         mx = my = 0;
  226         sx = sy = 0;
  227 
  228         for (i = 0; i < cdf->poscount; i++) {
  229             mx += (double)cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].x / cdf->poscount;
  230             my += (double)cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].y / cdf->poscount;
  231 
  232 
  233             if (i != 0) {
  234                 sx += cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].x - cdf->pos_hist[((cdf->first_pos + i - 1) %
  235                         cdf->hist_size)].x;
  236                 sy += cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].y - cdf->pos_hist[((cdf->first_pos + i - 1) %
  237                         cdf->hist_size)].y;
  238             }
  239 
  240         }
  241 
  242         if (cdf->poscount > 1) {
  243             // Normalize the support vector
  244             len = sqrt(sx * sx + sy * sy);
  245             sx /= len;
  246             sy /= len;
  247 
  248             // Calculate the new direction
  249             *dirout = (int)rint(atan(sx / sy) / M_PI * 180 + 180);
  250         } else {
  251             // If we only have one position, we can't use differences of positions, but we have to use the reported
  252             // direction of that position
  253             sx = sin((double)cdf->dir_hist[cdf->first_pos] / 180 * M_PI);
  254             sy = cos((double)cdf->dir_hist[cdf->first_pos] / 180 * M_PI);
  255             *dirout = cdf->dir_hist[cdf->first_pos];
  256         }
  257 
  258 
  259         dx = pin->x - mx;
  260         dy = pin->y - my;
  261         dist = dx * sx + dy * sy;
  262 
  263         if (cdf->extrapolating && (dist < cdf->last_dist)) {
  264             dist = cdf->last_dist;
  265         }
  266 
  267         cdf->last_dist = dist;
  268         cdf->extrapolating = 1;
  269 
  270         pout->x = (int)rint(mx + sx * dist);
  271         pout->y = (int)rint(my + sy * dist);
  272         pout->pro = pin->pro;
  273 
  274     } else {
  275         // We should extrapolate, but don't have an old position available
  276         *pout = *pin;
  277         *dirout = dirin;
  278     }
  279 
  280     if (cdf->available) {
  281         int dx,dy;
  282 
  283         dx = pout->x - cdf->last_out.x;
  284         dy = pout->y - cdf->last_out.y;
  285 
  286         if ((dx*dx + dy*dy) < CDF_MINDIST) {
  287             *pout = cdf->last_out;
  288             *dirout = cdf->last_dir;
  289         }
  290     }
  291 
  292     cdf->last_out = *pout;
  293     cdf->last_dir = *dirout;
  294 
  295     cdf->available = 1;
  296 }
  297 #endif
  298 
  299 int tracking_get_angle(struct tracking *tr) {
  300     return tr->curr_angle;
  301 }
  302 
  303 struct coord *
  304 tracking_get_pos(struct tracking *tr) {
  305     return &tr->curr_out;
  306 }
  307 
  308 int tracking_get_street_direction(struct tracking *tr) {
  309     return tr->street_direction;
  310 }
  311 
  312 int tracking_get_segment_pos(struct tracking *tr) {
  313     return tr->pos;
  314 }
  315 
  316 struct street_data *
  317 tracking_get_street_data(struct tracking *tr) {
  318     if (tr->curr_line)
  319         return tr->curr_line->street;
  320     return NULL;
  321 }
  322 
  323 int tracking_get_attr(struct tracking *_this, enum attr_type type, struct attr *attr, struct attr_iter *attr_iter) {
  324     struct item *item;
  325     struct map_rect *mr;
  326     struct tracking_line *tl;
  327 
  328     int result=0;
  329     dbg(lvl_debug,"enter %s",attr_to_name(type));
  330     if (_this->attr) {
  331         attr_free(_this->attr);
  332         _this->attr=NULL;
  333     }
  334     attr->type=type;
  335     switch (type) {
  336     case attr_position_valid:
  337         attr->u.num=_this->valid;
  338         return 1;
  339     case attr_position_direction:
  340         attr->u.numd=&_this->direction;
  341         return 1;
  342     case attr_position_direction_matched:
  343         attr->u.numd=&_this->direction_matched;
  344         return 1;
  345     case attr_position_speed:
  346         attr->u.numd=&_this->speed;
  347         return 1;
  348     case attr_directed:
  349         attr->u.num=_this->street_direction;
  350         return 1;
  351     case attr_position_coord_geo:
  352         if (!_this->coord_geo_valid) {
  353             struct coord c;
  354             c.x=_this->curr_out.x;
  355             c.y=_this->curr_out.y;
  356             transform_to_geo(_this->pro, &c, &_this->coord_geo);
  357             _this->coord_geo_valid=1;
  358         }
  359         attr->u.coord_geo=&_this->coord_geo;
  360         return 1;
  361     case attr_current_item:
  362         if (! _this->curr_line || ! _this->curr_line->street)
  363             return 0;
  364         attr->u.item=&_this->curr_line->street->item;
  365         return 1;
  366     case attr_street_count:
  367         attr->u.num=0;
  368         tl=_this->lines;
  369         while (tl) {
  370             attr->u.num++;
  371             tl=tl->next;
  372         }
  373         return 1;
  374     default:
  375         if (! _this->curr_line || ! _this->curr_line->street)
  376             return 0;
  377         item=&_this->curr_line->street->item;
  378         mr=map_rect_new(item->map,NULL);
  379         item=map_rect_get_item_byid(mr, item->id_hi, item->id_lo);
  380         if (item_attr_get(item, type, attr)) {
  381             _this->attr=attr_dup(attr);
  382             *attr=*_this->attr;
  383             result=1;
  384         }
  385         map_rect_destroy(mr);
  386         return result;
  387     }
  388 }
  389 
  390 struct item *
  391 tracking_get_current_item(struct tracking *_this) {
  392     if (! _this->curr_line || ! _this->curr_line->street)
  393         return NULL;
  394     return &_this->curr_line->street->item;
  395 }
  396 
  397 int *tracking_get_current_flags(struct tracking *_this) {
  398     if (! _this->curr_line || ! _this->curr_line->street)
  399         return NULL;
  400     return &_this->curr_line->street->flags;
  401 }
  402 
  403 static void tracking_get_angles(struct tracking_line *tl) {
  404     int i;
  405     struct street_data *sd=tl->street;
  406     for (i = 0 ; i < sd->count-1 ; i++)
  407         tl->angle[i]=transform_get_angle_delta(&sd->c[i], &sd->c[i+1], 0);
  408 }
  409 
  410 static int street_data_within_selection(struct street_data *sd, struct map_selection *sel) {
  411     struct coord_rect r;
  412     struct map_selection *curr;
  413     int i;
  414 
  415     if (!sel)
  416         return 1;
  417     r.lu=sd->c[0];
  418     r.rl=sd->c[0];
  419     for (i = 1 ; i < sd->count ; i++) {
  420         if (r.lu.x > sd->c[i].x)
  421             r.lu.x=sd->c[i].x;
  422         if (r.rl.x < sd->c[i].x)
  423             r.rl.x=sd->c[i].x;
  424         if (r.rl.y > sd->c[i].y)
  425             r.rl.y=sd->c[i].y;
  426         if (r.lu.y < sd->c[i].y)
  427             r.lu.y=sd->c[i].y;
  428     }
  429     curr=sel;
  430     while (curr) {
  431         struct coord_rect *sr=&curr->u.c_rect;
  432         if (r.lu.x <= sr->rl.x && r.rl.x >= sr->lu.x &&
  433                 r.lu.y >= sr->rl.y && r.rl.y <= sr->lu.y)
  434             return 1;
  435         curr=curr->next;
  436     }
  437     return 0;
  438 }
  439 
  440 
  441 static void tracking_doupdate_lines(struct tracking *tr, struct coord *pc, enum projection pro) {
  442     int max_dist=1000;
  443     struct map_selection *sel;
  444     struct mapset_handle *h;
  445     struct map *m;
  446     struct map_rect *mr;
  447     struct item *item;
  448     struct street_data *street;
  449     struct tracking_line *tl;
  450     struct coord_geo g;
  451     struct coord cc;
  452 
  453     dbg(lvl_debug,"enter");
  454     h=mapset_open(tr->ms);
  455     while ((m=mapset_next(h,2))) {
  456         cc.x = pc->x;
  457         cc.y = pc->y;
  458         if (map_projection(m) != pro) {
  459             transform_to_geo(pro, &cc, &g);
  460             transform_from_geo(map_projection(m), &g, &cc);
  461         }
  462         sel = route_rect(18, &cc, &cc, 0, max_dist);
  463         mr=map_rect_new(m, sel);
  464         if (!mr)
  465             continue;
  466         while ((item=map_rect_get_item(mr))) {
  467             if (item_get_default_flags(item->type)) {
  468                 street=street_get_data(item);
  469                 if (street_data_within_selection(street, sel)) {
  470                     tl=g_malloc(sizeof(struct tracking_line)+(street->count-1)*sizeof(int));
  471                     tl->street=street;
  472                     tracking_get_angles(tl);
  473                     tl->next=tr->lines;
  474                     tr->lines=tl;
  475                 } else
  476                     street_data_free(street);
  477             }
  478         }
  479         map_selection_destroy(sel);
  480         map_rect_destroy(mr);
  481     }
  482     mapset_close(h);
  483     dbg(lvl_debug, "exit");
  484 }
  485 
  486 
  487 void tracking_flush(struct tracking *tr) {
  488     struct tracking_line *tl=tr->lines,*next;
  489     dbg(lvl_debug,"enter(tr=%p)", tr);
  490 
  491     while (tl) {
  492         next=tl->next;
  493         street_data_free(tl->street);
  494         g_free(tl);
  495         tl=next;
  496     }
  497     tr->lines=NULL;
  498     tr->curr_line = NULL;
  499 }
  500 
  501 static int tracking_angle_diff(int a1, int a2, int full) {
  502     int ret=(a1-a2)%full;
  503     if (ret > full/2)
  504         ret-=full;
  505     if (ret < -full/2)
  506         ret+=full;
  507     return ret;
  508 }
  509 
  510 static int tracking_angle_abs_diff(int a1, int a2, int full) {
  511     int ret=tracking_angle_diff(a1, a2, full);
  512     if (ret < 0)
  513         ret=-ret;
  514     return ret;
  515 }
  516 
  517 static int tracking_angle_delta(struct tracking *tr, int vehicle_angle, int street_angle, int flags) {
  518     int full=180,ret=360,fwd=0,rev=0;
  519     struct vehicleprofile *profile=tr->vehicleprofile;
  520 
  521     if (profile) {
  522         fwd=((flags & profile->flags_forward_mask) == profile->flags);
  523         rev=((flags & profile->flags_reverse_mask) == profile->flags);
  524     }
  525     if (fwd || rev) {
  526         if (!fwd || !rev) {
  527             full=360;
  528             if (rev)
  529                 street_angle=(street_angle+180)%360;
  530         }
  531         ret=tracking_angle_abs_diff(vehicle_angle, street_angle, full);
  532     }
  533     return ret*ret;
  534 }
  535 
  536 static int tracking_is_connected(struct tracking *tr, struct coord *c1, struct coord *c2) {
  537     if (c1[0].x == c2[0].x && c1[0].y == c2[0].y)
  538         return 0;
  539     if (c1[0].x == c2[1].x && c1[0].y == c2[1].y)
  540         return 0;
  541     if (c1[1].x == c2[0].x && c1[1].y == c2[0].y)
  542         return 0;
  543     if (c1[1].x == c2[1].x && c1[1].y == c2[1].y)
  544         return 0;
  545     return tr->connected_pref;
  546 }
  547 
  548 static int tracking_is_no_stop(struct tracking *tr, struct coord *c1, struct coord *c2) {
  549     if (c1->x == c2->x && c1->y == c2->y)
  550         return tr->nostop_pref;
  551     return 0;
  552 }
  553 
  554 static int tracking_is_on_route(struct tracking *tr, struct route *rt, struct item *item) {
  555 #ifdef USE_ROUTING
  556     if (! rt)
  557         return 0;
  558     if (route_contains(rt, item))
  559         return 0;
  560     return tr->route_pref;
  561 #else
  562     return 0;
  563 #endif
  564 }
  565 
  566 static int tracking_value(struct tracking *tr, struct tracking_line *t, int offset, struct coord *lpnt, int min,
  567                           int flags) {
  568     int value=0;
  569     struct street_data *sd=t->street;
  570     dbg(lvl_info, "%d: (0x%x,0x%x)-(0x%x,0x%x)", offset, sd->c[offset].x, sd->c[offset].y, sd->c[offset+1].x,
  571         sd->c[offset+1].y);
  572     if (flags & 1) {
  573         struct coord c1, c2, cp;
  574         c1.x = sd->c[offset].x;
  575         c1.y = sd->c[offset].y;
  576         c2.x = sd->c[offset+1].x;
  577         c2.y = sd->c[offset+1].y;
  578         cp.x = tr->curr_in.x;
  579         cp.y = tr->curr_in.y;
  580         value+=transform_distance_line_sq(&c1, &c2, &cp, lpnt);
  581     }
  582     if (value >= min)
  583         return value;
  584     if (flags & 2)
  585         value += tracking_angle_delta(tr, tr->curr_angle, t->angle[offset], sd->flags)*tr->angle_pref>>4;
  586     if (value >= min)
  587         return value;
  588     if ((flags & 4)  && tr->connected_pref)
  589         value += tracking_is_connected(tr, tr->last, &sd->c[offset]);
  590     if ((flags & 8)  && tr->nostop_pref)
  591         value += tracking_is_no_stop(tr, lpnt, &tr->last_out);
  592     if (value >= min)
  593         return value;
  594     if ((flags & 16) && tr->route_pref)
  595         value += tracking_is_on_route(tr, tr->rt, &sd->item);
  596     if ((flags & 32) && tr->overspeed_percent_pref && tr->overspeed_pref ) {
  597         struct roadprofile *roadprofile=g_hash_table_lookup(tr->vehicleprofile->roadprofile_hash, (void *)t->street->item.type);
  598         if (roadprofile && tr->speed > roadprofile->speed * tr->overspeed_percent_pref/ 100)
  599             value += tr->overspeed_pref;
  600     }
  601     if ((flags & 64) && !!(sd->flags & AF_UNDERGROUND) != tr->no_gps)
  602         value+=200;
  603     return value;
  604 }
  605 
  606 
  607 /**
  608  * @brief Processes a position update.
  609  *
  610  * @param tr The {@code struct tracking} which will receive the position update
  611  * @param v The vehicle whose position has changed
  612  * @param vehicleprofile The vehicle profile to use
  613  * @param pro The projection to use for transformations
  614  */
  615 void tracking_update(struct tracking *tr, struct vehicle *v, struct vehicleprofile *vehicleprofile,
  616                      enum projection pro) {
  617     struct tracking_line *t;
  618     int i,value,min,time;
  619     struct coord lpnt;
  620     struct coord cin;
  621     struct attr valid,speed_attr,direction_attr,coord_geo,lag,time_attr,static_speed,static_distance;
  622     double speed, direction;
  623     if (v)
  624         tr->vehicle=v;
  625     if (vehicleprofile)
  626         tr->vehicleprofile=vehicleprofile;
  627 
  628     if (! tr->vehicle)
  629         return;
  630     if (!vehicle_get_attr(tr->vehicle, attr_position_valid, &valid, NULL))
  631         valid.u.num=attr_position_valid_valid;
  632     if (valid.u.num == attr_position_valid_invalid) {
  633         tr->valid=valid.u.num;
  634         return;
  635     }
  636     if (!vehicle_get_attr(tr->vehicle, attr_position_speed, &speed_attr, NULL) ||
  637             !vehicle_get_attr(tr->vehicle, attr_position_direction, &direction_attr, NULL) ||
  638             !vehicle_get_attr(tr->vehicle, attr_position_coord_geo, &coord_geo, NULL) ||
  639             !vehicle_get_attr(tr->vehicle, attr_position_time_iso8601, &time_attr, NULL)) {
  640         dbg(lvl_error,"failed to get position data speed:%d direction:%d coord:%d time:%d",
  641             vehicle_get_attr(tr->vehicle, attr_position_speed, &speed_attr, NULL),
  642             vehicle_get_attr(tr->vehicle, attr_position_direction, &direction_attr, NULL),
  643             vehicle_get_attr(tr->vehicle, attr_position_coord_geo, &coord_geo, NULL),
  644             vehicle_get_attr(tr->vehicle, attr_position_time_iso8601, &time_attr, NULL));
  645         return;
  646     }
  647     if (tr->tunnel_extrapolation) {
  648         struct attr fix_type;
  649         if (!vehicle_get_attr(tr->vehicle, attr_position_fix_type, &fix_type, NULL))
  650             fix_type.u.num=2;
  651         if (fix_type.u.num) {
  652             tr->no_gps=0;
  653             tr->tunnel=0;
  654         } else
  655             tr->no_gps=1;
  656     }
  657     if (!vehicleprofile_get_attr(vehicleprofile,attr_static_speed,&static_speed,NULL)
  658             || !vehicleprofile_get_attr(vehicleprofile,attr_static_distance,&static_distance,NULL)) {
  659         static_speed.u.num=3;
  660         static_distance.u.num=10;
  661         dbg(lvl_debug,"Using defaults for static position detection");
  662     }
  663     dbg(lvl_info,"Static speed: %ld, static distance: %ld",static_speed.u.num, static_distance.u.num);
  664     speed=*speed_attr.u.numd;
  665     direction=*direction_attr.u.numd;
  666     tr->valid=attr_position_valid_valid;
  667     transform_from_geo(pro, coord_geo.u.coord_geo, &tr->curr_in);
  668     if ((speed < static_speed.u.num && transform_distance(pro, &tr->last_in, &tr->curr_in) < static_distance.u.num )) {
  669         dbg(lvl_debug,"static speed %f coord 0x%x,0x%x vs 0x%x,0x%x",speed,tr->last_in.x,tr->last_in.y, tr->curr_in.x,
  670             tr->curr_in.y);
  671         tr->valid=attr_position_valid_static;
  672         tr->speed=0;
  673         return;
  674     }
  675     if (tr->tunnel) {
  676         tr->curr_in=tr->curr_out;
  677         dbg(lvl_debug,"tunnel extrapolation speed %f dir %f",tr->speed,tr->direction);
  678         dbg(lvl_debug,"old 0x%x,0x%x",tr->curr_in.x, tr->curr_in.y);
  679         speed=tr->speed;
  680         direction=tr->curr_line->angle[tr->pos];
  681         transform_project(pro, &tr->curr_in, tr->speed*tr->tunnel_extrapolation/36, tr->direction, &tr->curr_in);
  682         dbg(lvl_debug,"new 0x%x,0x%x",tr->curr_in.x, tr->curr_in.y);
  683     } else if (vehicle_get_attr(tr->vehicle, attr_lag, &lag, NULL) && lag.u.num > 0) {
  684         double espeed;
  685         time=iso8601_to_secs(time_attr.u.str);
  686         int edirection;
  687         if (time-tr->time == 1) {
  688             dbg(lvl_debug,"extrapolating speed from %f and %f (%f)",tr->speed, speed, speed-tr->speed);
  689             espeed=speed+(speed-tr->speed)*lag.u.num/10;
  690             dbg(lvl_debug,"extrapolating angle from %f and %f (%d)",tr->direction, direction, tracking_angle_diff(direction,
  691                     tr->direction,360));
  692             edirection=direction+tracking_angle_diff(direction,tr->direction,360)*lag.u.num/10;
  693         } else {
  694             dbg(lvl_debug,"no speed and direction extrapolation");
  695             espeed=speed;
  696             edirection=direction;
  697         }
  698         dbg(lvl_debug,"lag %ld speed %f direction %d",lag.u.num,espeed,edirection);
  699         dbg(lvl_debug,"old 0x%x,0x%x",tr->curr_in.x, tr->curr_in.y);
  700         transform_project(pro, &tr->curr_in, espeed*lag.u.num/36, edirection, &tr->curr_in);
  701         tr->time=time;
  702         dbg(lvl_debug,"new 0x%x,0x%x",tr->curr_in.x, tr->curr_in.y);
  703     }
  704     tr->pro=pro;
  705     tr->curr_angle=tr->direction=direction;
  706     tr->speed=speed;
  707     tr->last_in=tr->curr_in;
  708     tr->last_out=tr->curr_out;
  709     tr->last[0]=tr->curr[0];
  710     tr->last[1]=tr->curr[1];
  711     if (!tr->lines || transform_distance(pro, &tr->last_updated, &tr->curr_in) > 500) {
  712         dbg(lvl_debug, "update");
  713         tracking_flush(tr);
  714         tracking_doupdate_lines(tr, &tr->curr_in, pro);
  715         tr->last_updated=tr->curr_in;
  716         dbg(lvl_debug,"update end");
  717     }
  718 
  719     tr->street_direction=0;
  720     t=tr->lines;
  721     tr->curr_line=NULL;
  722     min=INT_MAX/2;
  723     while (t) {
  724         struct street_data *sd=t->street;
  725         for (i = 0; i < sd->count-1 ; i++) {
  726             value=tracking_value(tr,t,i,&lpnt,min,-1);
  727             if (value < min) {
  728                 struct coord lpnt_tmp;
  729                 int angle_delta=tracking_angle_abs_diff(tr->curr_angle, t->angle[i], 360);
  730                 tr->curr_line=t;
  731                 tr->pos=i;
  732                 tr->curr[0]=sd->c[i];
  733                 tr->curr[1]=sd->c[i+1];
  734                 tr->direction_matched=t->angle[i];
  735                 dbg(lvl_debug,"lpnt.x=0x%x,lpnt.y=0x%x pos=%d %d+%d+%d+%d=%d", lpnt.x, lpnt.y, i,
  736                     transform_distance_line_sq(&sd->c[i], &sd->c[i+1], &cin, &lpnt_tmp),
  737                     tracking_angle_delta(tr, tr->curr_angle, t->angle[i], 0)*tr->angle_pref,
  738                     tracking_is_connected(tr, tr->last, &sd->c[i]) ? tr->connected_pref : 0,
  739                     lpnt.x == tr->last_out.x && lpnt.y == tr->last_out.y ? tr->nostop_pref : 0,
  740                     value
  741                    );
  742                 tr->curr_out.x=lpnt.x;
  743                 tr->curr_out.y=lpnt.y;
  744                 tr->coord_geo_valid=0;
  745                 if (angle_delta < 70)
  746                     tr->street_direction=1;
  747                 else if (angle_delta > 110)
  748                     tr->street_direction=-1;
  749                 else
  750                     tr->street_direction=0;
  751                 min=value;
  752             }
  753         }
  754         t=t->next;
  755     }
  756     dbg(lvl_debug,"tr->curr_line=%p min=%d", tr->curr_line, min);
  757     if (!tr->curr_line || min > tr->offroad_limit_pref) {
  758         tr->curr_out=tr->curr_in;
  759         tr->coord_geo_valid=0;
  760         tr->street_direction=0;
  761     }
  762     if (tr->curr_line && (tr->curr_line->street->flags & AF_UNDERGROUND)) {
  763         if (tr->no_gps)
  764             tr->tunnel=1;
  765     } else if (tr->tunnel) {
  766         tr->speed=0;
  767     }
  768     dbg(lvl_debug,"found 0x%x,0x%x", tr->curr_out.x, tr->curr_out.y);
  769     callback_list_call_attr_0(tr->callback_list, attr_position_coord_geo);
  770 }
  771 
  772 static int tracking_set_attr_do(struct tracking *tr, struct attr *attr, int initial) {
  773     switch (attr->type) {
  774     case attr_angle_pref:
  775         tr->angle_pref=attr->u.num;
  776         return 1;
  777     case attr_connected_pref:
  778         tr->connected_pref=attr->u.num;
  779         return 1;
  780     case attr_nostop_pref:
  781         tr->nostop_pref=attr->u.num;
  782         return 1;
  783     case attr_offroad_limit_pref:
  784         tr->offroad_limit_pref=attr->u.num;
  785         return 1;
  786     case attr_route_pref:
  787         tr->route_pref=attr->u.num;
  788         return 1;
  789     case attr_overspeed_pref:
  790         tr->overspeed_pref=attr->u.num;
  791         return 1;
  792     case attr_overspeed_percent_pref:
  793         tr->overspeed_percent_pref=attr->u.num;
  794         return 1;
  795     case attr_tunnel_extrapolation:
  796         tr->tunnel_extrapolation=attr->u.num;
  797         return 1;
  798     default:
  799         return 0;
  800     }
  801 }
  802 
  803 int tracking_set_attr(struct tracking *tr, struct attr *attr) {
  804     return tracking_set_attr_do(tr, attr, 0);
  805 }
  806 
  807 int tracking_add_attr(struct tracking *this_, struct attr *attr) {
  808     switch (attr->type) {
  809     case attr_callback:
  810         callback_list_add(this_->callback_list, attr->u.callback);
  811         return 1;
  812     default:
  813         return 0;
  814     }
  815 }
  816 
  817 int tracking_remove_attr(struct tracking *this_, struct attr *attr) {
  818     switch (attr->type) {
  819     case attr_callback:
  820         callback_list_remove(this_->callback_list, attr->u.callback);
  821         return 1;
  822     default:
  823         return 0;
  824     }
  825 }
  826 
  827 struct object_func tracking_func = {
  828     attr_trackingo,
  829     (object_func_new)tracking_new,
  830     (object_func_get_attr)tracking_get_attr,
  831     (object_func_iter_new)NULL,
  832     (object_func_iter_destroy)NULL,
  833     (object_func_set_attr)tracking_set_attr,
  834     (object_func_add_attr)tracking_add_attr,
  835     (object_func_remove_attr)tracking_remove_attr,
  836     (object_func_init)tracking_init,
  837     (object_func_destroy)tracking_destroy,
  838     (object_func_dup)NULL,
  839     (object_func_ref)navit_object_ref,
  840     (object_func_unref)navit_object_unref,
  841 };
  842 
  843 
  844 struct tracking *
  845 tracking_new(struct attr *parent, struct attr **attrs) {
  846     struct tracking *this=g_new0(struct tracking, 1);
  847     struct attr hist_size;
  848     this->func=&tracking_func;
  849     navit_object_ref((struct navit_object *)this);
  850     this->angle_pref=10;
  851     this->connected_pref=10;
  852     this->nostop_pref=10;
  853     this->offroad_limit_pref=5000;
  854     this->route_pref=300;
  855     this->callback_list=callback_list_new();
  856 
  857 
  858     if (! attr_generic_get_attr(attrs, NULL, attr_cdf_histsize, &hist_size, NULL)) {
  859         hist_size.u.num = 0;
  860     }
  861     if (attrs) {
  862         for (; *attrs; attrs++)
  863             tracking_set_attr_do(this, *attrs, 1);
  864     }
  865 
  866     tracking_init_cdf(&this->cdf, hist_size.u.num);
  867 
  868     return this;
  869 }
  870 
  871 void tracking_set_mapset(struct tracking *this, struct mapset *ms) {
  872     this->ms=ms;
  873 }
  874 
  875 void tracking_set_route(struct tracking *this, struct route *rt) {
  876     this->rt=rt;
  877 }
  878 
  879 void tracking_destroy(struct tracking *tr) {
  880     if (tr->attr)
  881         attr_free(tr->attr);
  882     tracking_flush(tr);
  883     callback_list_destroy(tr->callback_list);
  884     g_free(tr);
  885 }
  886 
  887 struct map *
  888 tracking_get_map(struct tracking *this_) {
  889     struct attr *attrs[5];
  890     struct attr type,navigation,data,description;
  891     type.type=attr_type;
  892     type.u.str="tracking";
  893     navigation.type=attr_trackingo;
  894     navigation.u.tracking=this_;
  895     data.type=attr_data;
  896     data.u.str="";
  897     description.type=attr_description;
  898     description.u.str="Tracking";
  899 
  900     attrs[0]=&type;
  901     attrs[1]=&navigation;
  902     attrs[2]=&data;
  903     attrs[3]=&description;
  904     attrs[4]=NULL;
  905     if (! this_->map)
  906         this_->map=map_new(NULL, attrs);
  907     return this_->map;
  908 }
  909 
  910 
  911 struct map_priv {
  912     struct tracking *tracking;
  913 };
  914 
  915 struct map_rect_priv {
  916     struct tracking *tracking;
  917     struct item item;
  918     struct tracking_line *curr,*next;
  919     int coord;
  920     enum attr_type attr_next;
  921     int ccount;
  922     int debug_idx;
  923     char *str;
  924 };
  925 
  926 static void tracking_map_item_coord_rewind(void *priv_data) {
  927     struct map_rect_priv *this=priv_data;
  928     this->ccount=0;
  929 }
  930 
  931 static int tracking_map_item_coord_get(void *priv_data, struct coord *c, int count) {
  932     struct map_rect_priv *this=priv_data;
  933     enum projection pro;
  934     int ret=0;
  935     dbg(lvl_debug,"enter");
  936     while (this->ccount < 2 && count > 0) {
  937         pro = map_projection(this->curr->street->item.map);
  938         if (projection_mg != pro) {
  939             transform_from_to(&this->curr->street->c[this->ccount+this->coord],
  940                               pro,
  941                               c,projection_mg);
  942         } else
  943             *c=this->curr->street->c[this->ccount+this->coord];
  944         dbg(lvl_debug,"coord %d 0x%x,0x%x",this->ccount,c->x,c->y);
  945         this->ccount++;
  946         ret++;
  947         c++;
  948         count--;
  949     }
  950     return ret;
  951 }
  952 
  953 static void tracking_map_item_attr_rewind(void *priv_data) {
  954     struct map_rect_priv *this_=priv_data;
  955     this_->debug_idx=0;
  956     this_->attr_next=attr_debug;
  957 }
  958 
  959 static int tracking_map_item_attr_get(void *priv_data, enum attr_type attr_type, struct attr *attr) {
  960     struct map_rect_priv *this_=priv_data;
  961     struct coord lpnt,*c;
  962     struct tracking *tr=this_->tracking;
  963     int value;
  964     attr->type=attr_type;
  965 
  966     if (this_->str) {
  967         g_free(this_->str);
  968         this_->str=NULL;
  969     }
  970 
  971     switch(attr_type) {
  972     case attr_debug:
  973         switch(this_->debug_idx) {
  974         case 0:
  975             this_->debug_idx++;
  976             this_->str=attr->u.str=g_strdup_printf("overall: %d (limit %d)",tracking_value(tr, this_->curr, this_->coord, &lpnt,
  977                                                    INT_MAX/2, -1), tr->offroad_limit_pref);
  978             return 1;
  979         case 1:
  980             this_->debug_idx++;
  981             c=&this_->curr->street->c[this_->coord];
  982             value=tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 1);
  983             this_->str=attr->u.str=g_strdup_printf("distance: (0x%x,0x%x) from (0x%x,0x%x)-(0x%x,0x%x) at (0x%x,0x%x) %d",
  984                                                    tr->curr_in.x, tr->curr_in.y,
  985                                                    c[0].x, c[0].y, c[1].x, c[1].y,
  986                                                    lpnt.x, lpnt.y, value);
  987             return 1;
  988         case 2:
  989             this_->debug_idx++;
  990             this_->str=attr->u.str=g_strdup_printf("angle: %d to %d (flags %d) %d",
  991                                                    tr->curr_angle, this_->curr->angle[this_->coord], this_->curr->street->flags & 3,
  992                                                    tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2, 2));
  993             return 1;
  994         case 3:
  995             this_->debug_idx++;
  996             this_->str=attr->u.str=g_strdup_printf("connected: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2,
  997                                                    4));
  998             return 1;
  999         case 4:
 1000             this_->debug_idx++;
 1001             this_->str=attr->u.str=g_strdup_printf("no_stop: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2,
 1002                                                    8));
 1003             return 1;
 1004         case 5:
 1005             this_->debug_idx++;
 1006             this_->str=attr->u.str=g_strdup_printf("route: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2,
 1007                                                    16));
 1008             return 1;
 1009         case 6:
 1010             this_->debug_idx++;
 1011             this_->str=attr->u.str=g_strdup_printf("overspeed: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2,
 1012                                                    32));
 1013             return 1;
 1014         case 7:
 1015             this_->debug_idx++;
 1016             this_->str=attr->u.str=g_strdup_printf("tunnel: %d", tracking_value(tr, this_->curr, this_->coord, &lpnt, INT_MAX/2,
 1017                                                    64));
 1018             return 1;
 1019         case 8:
 1020             this_->debug_idx++;
 1021             this_->str=attr->u.str=g_strdup_printf("line %p", this_->curr);
 1022             return 1;
 1023         default:
 1024             this_->attr_next=attr_none;
 1025             return 0;
 1026         }
 1027     case attr_any:
 1028         while (this_->attr_next != attr_none) {
 1029             if (tracking_map_item_attr_get(priv_data, this_->attr_next, attr))
 1030                 return 1;
 1031         }
 1032         return 0;
 1033     default:
 1034         attr->type=attr_none;
 1035         return 0;
 1036     }
 1037 }
 1038 
 1039 static struct item_methods tracking_map_item_methods = {
 1040     tracking_map_item_coord_rewind,
 1041     tracking_map_item_coord_get,
 1042     tracking_map_item_attr_rewind,
 1043     tracking_map_item_attr_get,
 1044 };
 1045 
 1046 
 1047 static void tracking_map_destroy(struct map_priv *priv) {
 1048     g_free(priv);
 1049 }
 1050 
 1051 static void tracking_map_rect_init(struct map_rect_priv *priv) {
 1052     priv->next=priv->tracking->lines;
 1053     priv->curr=NULL;
 1054     priv->coord=0;
 1055     priv->item.id_lo=0;
 1056     priv->item.id_hi=0;
 1057 }
 1058 
 1059 static struct map_rect_priv *tracking_map_rect_new(struct map_priv *priv, struct map_selection *sel) {
 1060     struct tracking *tracking=priv->tracking;
 1061     struct map_rect_priv *ret=g_new0(struct map_rect_priv, 1);
 1062     ret->tracking=tracking;
 1063     tracking_map_rect_init(ret);
 1064     ret->item.meth=&tracking_map_item_methods;
 1065     ret->item.priv_data=ret;
 1066     ret->item.type=type_tracking_100;
 1067     return ret;
 1068 }
 1069 
 1070 static void tracking_map_rect_destroy(struct map_rect_priv *priv) {
 1071     g_free(priv);
 1072 }
 1073 
 1074 static struct item *tracking_map_get_item(struct map_rect_priv *priv) {
 1075     struct item *ret=&priv->item;
 1076     int value;
 1077     struct coord lpnt;
 1078 
 1079     if (!priv->next)
 1080         return NULL;
 1081     if (! priv->curr || priv->coord + 2 >= priv->curr->street->count) {
 1082         priv->curr=priv->next;
 1083         priv->next=priv->curr->next;
 1084         priv->coord=0;
 1085         priv->item.id_lo=0;
 1086         priv->item.id_hi++;
 1087     } else {
 1088         priv->coord++;
 1089         priv->item.id_lo++;
 1090     }
 1091     value=tracking_value(priv->tracking, priv->curr, priv->coord, &lpnt, INT_MAX/2, -1);
 1092     if (value < 64)
 1093         priv->item.type=type_tracking_100;
 1094     else if (value < 128)
 1095         priv->item.type=type_tracking_90;
 1096     else if (value < 256)
 1097         priv->item.type=type_tracking_80;
 1098     else if (value < 512)
 1099         priv->item.type=type_tracking_70;
 1100     else if (value < 1024)
 1101         priv->item.type=type_tracking_60;
 1102     else if (value < 2048)
 1103         priv->item.type=type_tracking_50;
 1104     else if (value < 4096)
 1105         priv->item.type=type_tracking_40;
 1106     else if (value < 8192)
 1107         priv->item.type=type_tracking_30;
 1108     else if (value < 16384)
 1109         priv->item.type=type_tracking_20;
 1110     else if (value < 32768)
 1111         priv->item.type=type_tracking_10;
 1112     else
 1113         priv->item.type=type_tracking_0;
 1114     dbg(lvl_debug,"item %d %d points", priv->coord, priv->curr->street->count);
 1115     tracking_map_item_coord_rewind(priv);
 1116     tracking_map_item_attr_rewind(priv);
 1117     return ret;
 1118 }
 1119 
 1120 static struct item *tracking_map_get_item_byid(struct map_rect_priv *priv, int id_hi, int id_lo) {
 1121     struct item *ret;
 1122     tracking_map_rect_init(priv);
 1123     while ((ret=tracking_map_get_item(priv))) {
 1124         if (ret->id_hi == id_hi && ret->id_lo == id_lo)
 1125             return ret;
 1126     }
 1127     return NULL;
 1128 }
 1129 
 1130 static struct map_methods tracking_map_meth = {
 1131     projection_mg,
 1132     "utf-8",
 1133     tracking_map_destroy,
 1134     tracking_map_rect_new,
 1135     tracking_map_rect_destroy,
 1136     tracking_map_get_item,
 1137     tracking_map_get_item_byid,
 1138     NULL,
 1139     NULL,
 1140     NULL,
 1141 };
 1142 
 1143 static struct map_priv *tracking_map_new(struct map_methods *meth, struct attr **attrs, struct callback_list *cbl) {
 1144     struct map_priv *ret;
 1145     struct attr *tracking_attr;
 1146 
 1147     tracking_attr=attr_search(attrs, attr_trackingo);
 1148     if (! tracking_attr)
 1149         return NULL;
 1150     ret=g_new0(struct map_priv, 1);
 1151     *meth=tracking_map_meth;
 1152     ret->tracking=tracking_attr->u.tracking;
 1153 
 1154     return ret;
 1155 }
 1156 
 1157 
 1158 void tracking_init(void) {
 1159     plugin_register_category_map("tracking", tracking_map_new);
 1160 }