summaryrefslogtreecommitdiff
path: root/navit
diff options
context:
space:
mode:
Diffstat (limited to 'navit')
-rw-r--r--navit/item_def.h1
-rw-r--r--navit/navigation.c18
-rw-r--r--navit/route.c17
-rw-r--r--navit/track.c82
-rw-r--r--navit/track.h22
-rw-r--r--navit/transform.c18
-rw-r--r--navit/transform.h24
7 files changed, 128 insertions, 54 deletions
diff --git a/navit/item_def.h b/navit/item_def.h
index dec5d24f..a6970274 100644
--- a/navit/item_def.h
+++ b/navit/item_def.h
@@ -304,6 +304,7 @@ ITEM(route_start)
ITEM(route_end)
ITEM(selected_point)
ITEM(power_tower)
+ITEM(route_start_reverse)
/* Line */
ITEM2(0x80000000,line)
ITEM2(0x80000001,line_unspecified)
diff --git a/navit/navigation.c b/navit/navigation.c
index da7860c5..8236cb53 100644
--- a/navit/navigation.c
+++ b/navit/navigation.c
@@ -70,7 +70,6 @@ struct navigation {
int turn_around;
int turn_around_limit;
int distance_turn;
- int distance_last;
struct callback *route_cb;
int announce[route_item_last-route_item_first+1][3];
};
@@ -148,7 +147,6 @@ navigation_new(struct attr *parent, struct attr **attrs)
ret->callback=callback_list_new();
ret->callback_speech=callback_list_new();
ret->level_last=-2;
- ret->distance_last=-2;
ret->distance_turn=50;
ret->turn_around_limit=3;
ret->navit=parent->u.navit;
@@ -1691,6 +1689,10 @@ navigation_update(struct navigation *this_, struct route *route, struct attr *at
return;
dbg(1,"enter\n");
while ((ritem=map_rect_get_item(mr))) {
+ if (ritem->type == type_route_start && this_->turn_around > -this_->turn_around_limit+1)
+ this_->turn_around--;
+ if (ritem->type == type_route_start_reverse && this_->turn_around < this_->turn_around_limit)
+ this_->turn_around++;
if (ritem->type != type_street_route)
continue;
if (first && item_attr_get(ritem, attr_street_item, &street_item)) {
@@ -1714,6 +1716,7 @@ navigation_update(struct navigation *this_, struct route *route, struct attr *at
}
navigation_itm_new(this_, ritem);
}
+ dbg(2,"turn_around=%d\n", this_->turn_around);
if (first)
navigation_destroy_itms_cmds(this_, NULL);
else {
@@ -1722,17 +1725,6 @@ navigation_update(struct navigation *this_, struct route *route, struct attr *at
make_maneuvers(this_,this_->route);
}
calculate_dest_distance(this_, incr);
- dbg(2,"destination distance old=%d new=%d\n", this_->distance_last, this_->first->dest_length);
- if (this_->first->dest_length > this_->distance_last && this_->distance_last >= 0)
- this_->turn_around++;
- else
- this_->turn_around--;
- if (this_->turn_around > this_->turn_around_limit)
- this_->turn_around=this_->turn_around_limit;
- else if (this_->turn_around < -this_->turn_around_limit+1)
- this_->turn_around=-this_->turn_around_limit+1;
- dbg(2,"turn_around=%d\n", this_->turn_around);
- this_->distance_last=this_->first->dest_length;
profile(0,"end");
navigation_call_callbacks(this_, FALSE);
}
diff --git a/navit/route.c b/navit/route.c
index f405c31a..44174864 100644
--- a/navit/route.c
+++ b/navit/route.c
@@ -179,6 +179,8 @@ struct route_info {
int lenextra; /**< Distance between lp and c */
int percent; /**< ratio of lenneg to lenght of whole street in percent */
struct street_data *street; /**< The street lp is on */
+ int street_direction; /**< Direction of vehicle on street -1 = Negative direction, 1 = Positive direction, 0 = Unknown */
+ int dir; /**< Direction to take when following the route -1 = Negative direction, 1 = Positive direction */
};
/**
@@ -718,6 +720,7 @@ route_set_position(struct route *this, struct pcoord *pos)
route_info_free(this->pos);
this->pos=NULL;
this->pos=route_find_nearest_street(this->vehicleprofile, this->ms, pos);
+ this->pos->street_direction=0;
dbg(1,"this->pos=%p\n", this->pos);
if (! this->pos)
return;
@@ -751,6 +754,7 @@ route_set_position_from_tracking(struct route *this, struct tracking *tracking,
ret->c=*c;
ret->lp=*c;
ret->pos=tracking_get_segment_pos(tracking);
+ ret->street_direction=tracking_get_street_direction(tracking);
sd=tracking_get_street_data(tracking);
if (sd) {
ret->street=street_data_dup(sd);
@@ -1327,6 +1331,7 @@ route_path_add_item_from_graph(struct route_path *this, struct route_path *oldpa
len=pos->lenneg;
}
}
+ pos->dir=dir;
} else if (dst) {
extra=1;
dbg(1,"dst dir=%d\n", dir);
@@ -2548,11 +2553,11 @@ rm_coord_get(void *priv_data, struct coord *c, int count)
if (pro == projection_none)
return 0;
- if (mr->item.type == type_route_start || mr->item.type == type_route_end) {
+ if (mr->item.type == type_route_start || mr->item.type == type_route_start_reverse || mr->item.type == type_route_end) {
if (! count || mr->last_coord)
return 0;
mr->last_coord=1;
- if (mr->item.type == type_route_start)
+ if (mr->item.type == type_route_start || mr->item.type == type_route_start_reverse)
c[0]=r->pos->c;
else
c[0]=r->dst->c;
@@ -2940,12 +2945,16 @@ rp_get_item_byid(struct map_rect_priv *mr, int id_hi, int id_lo)
static struct item *
rm_get_item(struct map_rect_priv *mr)
{
+ struct route *route=mr->mpriv->route;
dbg(1,"enter\n", mr->pos);
switch (mr->item.type) {
case type_none:
- mr->item.type=type_route_start;
- if (mr->mpriv->route->pos)
+ if (route->pos && route->pos->street_direction && route->pos->street_direction != route->pos->dir)
+ mr->item.type=type_route_start_reverse;
+ else
+ mr->item.type=type_route_start;
+ if (route->pos)
break;
default:
mr->item.type=type_street_route;
diff --git a/navit/track.c b/navit/track.c
index cdd50f5d..e48abd17 100644
--- a/navit/track.c
+++ b/navit/track.c
@@ -34,6 +34,7 @@
#include "plugin.h"
#include "vehicleprofile.h"
#include "vehicle.h"
+#include "util.h"
struct tracking_line
{
@@ -91,11 +92,13 @@ struct tracking {
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;
};
@@ -295,6 +298,12 @@ tracking_get_pos(struct tracking *tr)
}
int
+tracking_get_street_direction(struct tracking *tr)
+{
+ return tr->street_direction;
+}
+
+int
tracking_get_segment_pos(struct tracking *tr)
{
return tr->pos;
@@ -478,16 +487,22 @@ tracking_free_lines(struct tracking *tr)
}
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;
}
@@ -579,10 +594,11 @@ 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 attr valid,speed,direction,coord_geo;
+ struct attr valid,speed_attr,direction_attr,coord_geo,lag,time_attr;
+ double speed, direction;
if (v)
tr->vehicle=v;
if (vehicleprofile)
@@ -596,25 +612,50 @@ tracking_update(struct tracking *tr, struct vehicle *v, struct vehicleprofile *v
tr->valid=valid.u.num;
return;
}
- if (!vehicle_get_attr(tr->vehicle, attr_position_speed, &speed, NULL) ||
- !vehicle_get_attr(tr->vehicle, attr_position_direction, &direction, NULL) ||
- !vehicle_get_attr(tr->vehicle, attr_position_coord_geo, &coord_geo, NULL))
+ 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.u.numd < 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.u.numd,tr->last_in.x,tr->last_in.y, tr->curr_in.x, tr->curr_in.y);
+ 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.u.numd;
- tr->speed=*speed.u.numd;
+ 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];
@@ -635,6 +676,7 @@ tracking_update(struct tracking *tr, struct vehicle *v, struct vehicleprofile *v
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];
@@ -649,6 +691,12 @@ tracking_update(struct tracking *tr, struct vehicle *v, struct vehicleprofile *v
tr->curr_out.x=lpnt.x;
tr->curr_out.y=lpnt.y;
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;
}
}
diff --git a/navit/track.h b/navit/track.h
index 0440f55d..8a8e085e 100644
--- a/navit/track.h
+++ b/navit/track.h
@@ -24,23 +24,33 @@
extern "C" {
#endif
/* prototypes */
+enum attr_type;
+enum projection;
+struct attr;
+struct attr_iter;
struct coord;
+struct item;
+struct map;
struct mapset;
+struct route;
struct street_data;
struct tracking;
-struct coord *tracking_get_pos(struct tracking *tr);
+struct vehicle;
+struct vehicleprofile;
int tracking_get_angle(struct tracking *tr);
+struct coord *tracking_get_pos(struct tracking *tr);
+int tracking_get_street_direction(struct tracking *tr);
int tracking_get_segment_pos(struct tracking *tr);
struct street_data *tracking_get_street_data(struct tracking *tr);
-void tracking_update(struct tracking *tr, struct vehicle *v, struct vehicleprofile *vehicleprofile, enum projection pro);
-struct tracking *tracking_new(struct attr *parent, struct attr **attrs);
-void tracking_set_mapset(struct tracking *this_, struct mapset *ms);
-void tracking_set_route(struct tracking *this_, struct route *rt);
int tracking_get_attr(struct tracking *_this, enum attr_type type, struct attr *attr, struct attr_iter *attr_iter);
struct item *tracking_get_current_item(struct tracking *_this);
int *tracking_get_current_flags(struct tracking *_this);
+void tracking_update(struct tracking *tr, struct vehicle *v, struct vehicleprofile *vehicleprofile, enum projection pro);
+struct tracking *tracking_new(struct attr *parent, struct attr **attrs);
+void tracking_set_mapset(struct tracking *this, struct mapset *ms);
+void tracking_set_route(struct tracking *this, struct route *rt);
void tracking_destroy(struct tracking *tr);
-struct map * tracking_get_map(struct tracking *this_);
+struct map *tracking_get_map(struct tracking *this_);
void tracking_init(void);
/* end of prototypes */
#ifdef __cplusplus
diff --git a/navit/transform.c b/navit/transform.c
index 10c1bd73..a5e9cd10 100644
--- a/navit/transform.c
+++ b/navit/transform.c
@@ -930,6 +930,24 @@ transform_distance(enum projection pro, struct coord *c1, struct coord *c2)
}
}
+void
+transform_project(enum projection pro, struct coord *c, int distance, int angle, struct coord *res)
+{
+ double scale;
+ switch (pro) {
+ case projection_mg:
+ scale=transform_scale(c->y);
+ res->x=c->x+distance*sin(angle*M_PI/180)*scale;
+ res->y=c->y+distance*cos(angle*M_PI/180)*scale;
+ break;
+ default:
+ dbg(0,"Unsupported projection: %d\n", pro);
+ return;
+ }
+
+}
+
+
double
transform_polyline_length(enum projection pro, struct coord *c, int count)
{
diff --git a/navit/transform.h b/navit/transform.h
index e1600b6a..ee617d72 100644
--- a/navit/transform.h
+++ b/navit/transform.h
@@ -41,7 +41,7 @@ int transform_get_hog(struct transformation *this_);
void transform_set_hog(struct transformation *this_, int hog);
int transformation_get_order_base(struct transformation *this_);
void transform_set_order_base(struct transformation *this_, int order_base);
-struct transformation * transform_dup(struct transformation *t);
+struct transformation *transform_dup(struct transformation *t);
void transform_to_geo(enum projection pro, struct coord *c, struct coord_geo *g);
void transform_from_geo(enum projection pro, struct coord_geo *g, struct coord *c);
void transform_from_to(struct coord *cfrom, enum projection from, struct coord *cto, enum projection to);
@@ -53,32 +53,29 @@ int transform(struct transformation *t, enum projection pro, struct coord *c, st
void transform_reverse(struct transformation *t, struct point *p, struct coord *c);
enum projection transform_get_projection(struct transformation *this_);
void transform_set_projection(struct transformation *this_, enum projection pro);
-struct map_selection * transform_get_selection(struct transformation *this_, enum projection pro, int order);
-struct coord * transform_center(struct transformation *this_);
-struct coord * transform_get_center(struct transformation *this_);
+struct map_selection *transform_get_selection(struct transformation *this_, enum projection pro, int order);
+struct coord *transform_center(struct transformation *this_);
+struct coord *transform_get_center(struct transformation *this_);
void transform_set_center(struct transformation *this_, struct coord *c);
-void transform_set_yaw(struct transformation *t,int yaw);
+void transform_set_yaw(struct transformation *t, int yaw);
int transform_get_yaw(struct transformation *this_);
-void transform_set_pitch(struct transformation *this_,int pitch);
+void transform_set_pitch(struct transformation *this_, int pitch);
int transform_get_pitch(struct transformation *this_);
-void transform_set_roll(struct transformation *this_,int roll);
+void transform_set_roll(struct transformation *this_, int roll);
int transform_get_roll(struct transformation *this_);
-void transform_set_distance(struct transformation *this_,int distance);
+void transform_set_distance(struct transformation *this_, int distance);
int transform_get_distance(struct transformation *this_);
-struct map_selection *transform_get_selection(struct transformation *this_, enum projection pro, int order);
-struct coord *transform_center(struct transformation *this_);
-void transform_set_angle(struct transformation *t, int angle);
-int transform_get_angle(struct transformation *this_, int angle);
void transform_set_screen_selection(struct transformation *t, struct map_selection *sel);
void transform_set_screen_center(struct transformation *t, struct point *p);
void transform_get_size(struct transformation *t, int *width, int *height);
-void transform_setup(struct transformation *t, struct pcoord *c, int scale, int angle);
+void transform_setup(struct transformation *t, struct pcoord *c, int scale, int yaw);
void transform_setup_source_rect(struct transformation *t);
long transform_get_scale(struct transformation *t);
void transform_set_scale(struct transformation *t, long scale);
int transform_get_order(struct transformation *t);
double transform_scale(int y);
double transform_distance(enum projection pro, struct coord *c1, struct coord *c2);
+void transform_project(enum projection pro, struct coord *c, int distance, int angle, struct coord *res);
double transform_polyline_length(enum projection pro, struct coord *c, int count);
int transform_distance_sq(struct coord *c1, struct coord *c2);
int transform_distance_sq_pc(struct pcoord *c1, struct pcoord *c2);
@@ -93,7 +90,6 @@ int transform_within_dist_polyline(struct coord *ref, struct coord *c, int count
int transform_within_dist_polygon(struct coord *ref, struct coord *c, int count, int dist);
int transform_within_dist_item(struct coord *ref, enum item_type type, struct coord *c, int count, int dist);
void transform_destroy(struct transformation *t);
-struct coord transform_add_distance(enum projection pro, struct coord *c, int len, int dir);
/* end of prototypes */
#ifdef __cplusplus
}
View Lorry Controller Status