#include "mapset.h"
#include "plugin.h"
#include "vehicleprofile.h"
+#include "vehicle.h"
+#include "util.h"
struct tracking_line
{
struct mapset *ms;
struct route *rt;
struct map *map;
+ struct vehicle *vehicle;
struct vehicleprofile *vehicleprofile;
- struct pcoord last_updated;
+ struct coord last_updated;
struct tracking_line *lines;
struct tracking_line *curr_line;
int pos;
- struct coord curr[2];
- struct pcoord curr_in, curr_out;
+ struct coord curr[2], curr_in, curr_out;
int curr_angle;
- struct coord last[2];
- struct pcoord last_in, last_out;
+ struct coord last[2], last_in, last_out;
struct cdf_data cdf;
+ struct attr *attr;
+ int valid;
+ int time;
+ double direction;
+ double speed;
+ int coord_geo_valid;
+ struct coord_geo coord_geo;
+ enum projection pro;
+ int street_direction;
};
// Mininum distance (square of it..), below which we ignore gps updates
#define CDF_MINDIST 49 // 7 meters, I guess this value has to be changed for pedestrians.
+#if 0
static void
tracking_process_cdf(struct cdf_data *cdf, struct pcoord *pin, struct pcoord *pout, int dirin, int *dirout, int cur_speed, time_t fixtime)
{
cdf->available = 1;
}
+#endif
int
tracking_get_angle(struct tracking *tr)
return tr->curr_angle;
}
-struct pcoord *
+struct coord *
tracking_get_pos(struct tracking *tr)
{
return &tr->curr_out;
}
int
+tracking_get_street_direction(struct tracking *tr)
+{
+ return tr->street_direction;
+}
+
+int
tracking_get_segment_pos(struct tracking *tr)
{
return tr->pos;
}
int
-tracking_get_current_attr(struct tracking *_this, enum attr_type type, struct attr *attr)
+tracking_get_attr(struct tracking *_this, enum attr_type type, struct attr *attr, struct attr_iter *attr_iter)
{
struct item *item;
struct map_rect *mr;
int result=0;
- if (! _this->curr_line || ! _this->curr_line->street)
- return 0;
- item=&_this->curr_line->street->item;
- mr=map_rect_new(item->map,NULL);
- item=map_rect_get_item_byid(mr, item->id_hi, item->id_lo);
- if (item_attr_get(item, type, attr))
- result=1;
- map_rect_destroy(mr);
- return result;
+ dbg(1,"enter %s\n",attr_to_name(type));
+ if (_this->attr) {
+ attr_free(_this->attr);
+ _this->attr=NULL;
+ }
+ switch (type) {
+ case attr_position_valid:
+ attr->u.num=_this->valid;
+ return 1;
+ case attr_position_direction:
+ attr->u.numd=&_this->direction;
+ return 1;
+ case attr_position_speed:
+ attr->u.numd=&_this->speed;
+ return 1;
+ case attr_position_coord_geo:
+ if (!_this->coord_geo_valid) {
+ struct coord c;
+ c.x=_this->curr_out.x;
+ c.y=_this->curr_out.y;
+ transform_to_geo(_this->pro, &c, &_this->coord_geo);
+ _this->coord_geo_valid=1;
+ }
+ attr->u.coord_geo=&_this->coord_geo;
+ return 1;
+ default:
+ if (! _this->curr_line || ! _this->curr_line->street)
+ return 0;
+ item=&_this->curr_line->street->item;
+ mr=map_rect_new(item->map,NULL);
+ item=map_rect_get_item_byid(mr, item->id_hi, item->id_lo);
+ if (item_attr_get(item, type, attr)) {
+ _this->attr=attr_dup(attr);
+ *attr=*_this->attr;
+ result=1;
+ }
+ map_rect_destroy(mr);
+ return result;
+ }
}
struct item *
static void
-tracking_doupdate_lines(struct tracking *tr, struct pcoord *pc)
+tracking_doupdate_lines(struct tracking *tr, struct coord *pc, enum projection pro)
{
int max_dist=1000;
struct map_selection *sel;
dbg(1,"enter\n");
h=mapset_open(tr->ms);
- while ((m=mapset_next(h,1))) {
+ while ((m=mapset_next(h,2))) {
cc.x = pc->x;
cc.y = pc->y;
- if (map_projection(m) != pc->pro) {
- transform_to_geo(pc->pro, &cc, &g);
+ if (map_projection(m) != pro) {
+ transform_to_geo(pro, &cc, &g);
transform_from_geo(map_projection(m), &g, &cc);
}
sel = route_rect(18, &cc, &cc, 0, max_dist);
}
static int
-tracking_angle_abs_diff(int a1, int a2, int full)
+tracking_angle_diff(int a1, int a2, int full)
{
- int ret;
-
- if (a2 > a1)
- ret=(a2-a1)%full;
- else
- ret=(a1-a2)%full;
+ int ret=(a1-a2)%full;
if (ret > full/2)
- ret=full-ret;
+ ret-=full;
+ if (ret < -full/2)
+ ret+=full;
+ return ret;
+}
+
+static int
+tracking_angle_abs_diff(int a1, int a2, int full)
+{
+ int ret=tracking_angle_diff(a1, a2, full);
+ if (ret < 0)
+ ret=-ret;
return ret;
}
}
static int
-tracking_is_no_stop(struct coord *c1, struct pcoord *c2)
+tracking_is_no_stop(struct coord *c1, struct coord *c2)
{
if (c1->x == c2->x && c1->y == c2->y)
return nostop_pref;
{
if (! rt)
return 0;
- if (route_pos_contains(rt, item))
- return 0;
if (route_contains(rt, item))
return 0;
return route_pref;
}
-int
-tracking_update(struct tracking *tr, struct vehicleprofile *vehicleprofile, struct pcoord *pc, int angle, double *hdop, int speed, time_t fixtime)
+void
+tracking_update(struct tracking *tr, struct vehicle *v, struct vehicleprofile *vehicleprofile, enum projection pro)
{
struct tracking_line *t;
- int i,value,min;
+ int i,value,min,time;
struct coord lpnt;
struct coord cin;
- struct pcoord pcf; // Coordinate filtered through the CDF
- int anglef; // Angle filtered through the CDF
- dbg(1,"enter(%p,%p,%d)\n", tr, pc, angle);
- dbg(1,"c=%d:0x%x,0x%x\n", pc->pro, pc->x, pc->y);
- tr->vehicleprofile=vehicleprofile;
-
- if (pc->x == tr->curr_in.x && pc->y == tr->curr_in.y) {
- if (tr->curr_out.x && tr->curr_out.y)
- *pc=tr->curr_out;
- return 0;
+ struct attr valid,speed_attr,direction_attr,coord_geo,lag,time_attr;
+ double speed, direction;
+ if (v)
+ tr->vehicle=v;
+ if (vehicleprofile)
+ tr->vehicleprofile=vehicleprofile;
+
+ if (! tr->vehicle)
+ return;
+ if (!vehicle_get_attr(tr->vehicle, attr_position_valid, &valid, NULL))
+ valid.u.num=attr_position_valid_valid;
+ if (valid.u.num == attr_position_valid_invalid) {
+ tr->valid=valid.u.num;
+ return;
}
-
- if (hdop && *hdop > 3.5f) { // This value has been taken from julien cayzac's CDF implementation
- *pc = tr->curr_out;
- return 0;
+ if (!vehicle_get_attr(tr->vehicle, attr_position_speed, &speed_attr, NULL) ||
+ !vehicle_get_attr(tr->vehicle, attr_position_direction, &direction_attr, NULL) ||
+ !vehicle_get_attr(tr->vehicle, attr_position_coord_geo, &coord_geo, NULL) ||
+ !vehicle_get_attr(tr->vehicle, attr_position_time_iso8601, &time_attr, NULL)) {
+ dbg(0,"failed to get position data\n");
+ return;
+ }
+ time=iso8601_to_secs(time_attr.u.str);
+ speed=*speed_attr.u.numd;
+ direction=*direction_attr.u.numd;
+ tr->valid=attr_position_valid_valid;
+ transform_from_geo(pro, coord_geo.u.coord_geo, &tr->curr_in);
+ if ((speed < 3 && transform_distance(pro, &tr->last_in, &tr->curr_in) < 10 )) {
+ dbg(1,"static speed %f coord 0x%x,0x%x vs 0x%x,0x%x\n",speed,tr->last_in.x,tr->last_in.y, tr->curr_in.x, tr->curr_in.y);
+ tr->valid=attr_position_valid_static;
+ tr->speed=0;
+ return;
+ }
+ if (vehicle_get_attr(tr->vehicle, attr_lag, &lag, NULL) && lag.u.num > 0) {
+ double espeed;
+ int edirection;
+ if (time-tr->time == 1) {
+ dbg(1,"extrapolating speed from %f and %f (%f)\n",tr->speed, speed, speed-tr->speed);
+ espeed=speed+(speed-tr->speed)*lag.u.num/10;
+ dbg(1,"extrapolating angle from %f and %f (%d)\n",tr->direction, direction, tracking_angle_diff(direction,tr->direction,360));
+ edirection=direction+tracking_angle_diff(direction,tr->direction,360)*lag.u.num/10;
+ } else {
+ dbg(1,"no speed and direction extrapolation\n");
+ espeed=speed;
+ edirection=direction;
+ }
+ dbg(1,"lag %d speed %f direction %d\n",lag.u.num,espeed,edirection);
+ dbg(1,"old 0x%x,0x%x\n",tr->curr_in.x, tr->curr_in.y);
+ transform_project(pro, &tr->curr_in, espeed*lag.u.num/36, edirection, &tr->curr_in);
+ dbg(1,"new 0x%x,0x%x\n",tr->curr_in.x, tr->curr_in.y);
}
+ tr->time=time;
+ tr->pro=pro;
+#if 0
tracking_process_cdf(&tr->cdf, pc, &pcf, angle, &anglef, speed, fixtime);
-
+#endif
+ tr->curr_angle=tr->direction=direction;
+ tr->speed=speed;
tr->last_in=tr->curr_in;
tr->last_out=tr->curr_out;
tr->last[0]=tr->curr[0];
tr->last[1]=tr->curr[1];
- tr->curr_in=pcf;
- tr->curr_angle=anglef;
- cin.x = pcf.x;
- cin.y = pcf.y;
- if (!tr->lines || transform_distance_sq_pc(&tr->last_updated, &pcf) > 250000) {
+ if (!tr->lines || transform_distance(pro, &tr->last_updated, &tr->curr_in) > 500) {
dbg(1, "update\n");
tracking_free_lines(tr);
- tracking_doupdate_lines(tr, &pcf);
- tr->last_updated=pcf;
+ tracking_doupdate_lines(tr, &tr->curr_in, pro);
+ tr->last_updated=tr->curr_in;
dbg(1,"update end\n");
}
-
+
t=tr->lines;
- if (! t)
- return 0;
tr->curr_line=NULL;
min=INT_MAX/2;
while (t) {
for (i = 0; i < sd->count-1 ; i++) {
value=tracking_value(tr,t,i,&lpnt,min,-1);
if (value < min) {
+ int angle_delta=tracking_angle_abs_diff(tr->curr_angle, t->angle[i], 360);
tr->curr_line=t;
tr->pos=i;
tr->curr[0]=sd->c[i];
tr->curr[1]=sd->c[i+1];
dbg(1,"lpnt.x=0x%x,lpnt.y=0x%x pos=%d %d+%d+%d+%d=%d\n", lpnt.x, lpnt.y, i,
transform_distance_line_sq(&sd->c[i], &sd->c[i+1], &cin, &lpnt),
- tracking_angle_delta(tr, anglef, t->angle[i], 0)*angle_factor,
+ tracking_angle_delta(tr, tr->curr_angle, t->angle[i], 0)*angle_factor,
tracking_is_connected(tr->last, &sd->c[i]) ? connected_pref : 0,
lpnt.x == tr->last_out.x && lpnt.y == tr->last_out.y ? nostop_pref : 0,
value
);
tr->curr_out.x=lpnt.x;
tr->curr_out.y=lpnt.y;
- tr->curr_out.pro = pcf.pro;
+ tr->coord_geo_valid=0;
+ if (angle_delta < 70)
+ tr->street_direction=1;
+ else if (angle_delta > 110)
+ tr->street_direction=-1;
+ else
+ tr->street_direction=0;
min=value;
}
}
t=t->next;
}
dbg(1,"tr->curr_line=%p min=%d\n", tr->curr_line, min);
- if (!tr->curr_line || min > offroad_limit_pref)
- return 0;
+ if (!tr->curr_line || min > offroad_limit_pref) {
+ tr->curr_out=tr->curr_in;
+ tr->coord_geo_valid=0;
+ }
dbg(1,"found 0x%x,0x%x\n", tr->curr_out.x, tr->curr_out.y);
- *pc=tr->curr_out;
- return 1;
}
struct tracking *
projects / navit-package / blobdiff