/[zanavi_public1]/navit/navit/track.c
ZANavi

Diff of /navit/navit/track.c

Parent Directory Parent Directory | Revision Log Revision Log | View Patch Patch

Revision 2 Revision 27
1/**
2 * ZANavi, Zoff Android Navigation system.
3 * Copyright (C) 2011-2012 Zoff <zoff@zoff.cc>
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
1/** 20/**
2 * Navit, a modular navigation system. 21 * Navit, a modular navigation system.
3 * Copyright (C) 2005-2008 Navit Team 22 * Copyright (C) 2005-2008 Navit Team
4 * 23 *
5 * This program is free software; you can redistribute it and/or 24 * This program is free software; you can redistribute it and/or
449 struct street_data *street; 468 struct street_data *street;
450 struct tracking_line *tl; 469 struct tracking_line *tl;
451 struct coord_geo g; 470 struct coord_geo g;
452 struct coord cc; 471 struct coord cc;
453 472
454 dbg(1,"enter\n"); 473 //dbg(1,"enter\n");
455 h=mapset_open(tr->ms); 474 h=mapset_open(tr->ms);
456 while ((m=mapset_next(h,2))) { 475 while ((m=mapset_next(h,2))) {
457 cc.x = pc->x; 476 cc.x = pc->x;
458 cc.y = pc->y; 477 cc.y = pc->y;
459 if (map_projection(m) != pro) { 478 if (map_projection(m) != pro) {
479 } 498 }
480 map_selection_destroy(sel); 499 map_selection_destroy(sel);
481 map_rect_destroy(mr); 500 map_rect_destroy(mr);
482 } 501 }
483 mapset_close(h); 502 mapset_close(h);
484 dbg(1, "exit\n"); 503 //dbg(1, "exit\n");
485} 504}
486 505
487 506
488void 507void
489tracking_flush(struct tracking *tr) 508tracking_flush(struct tracking *tr)
581static int 600static int
582tracking_value(struct tracking *tr, struct tracking_line *t, int offset, struct coord *lpnt, int min, int flags) 601tracking_value(struct tracking *tr, struct tracking_line *t, int offset, struct coord *lpnt, int min, int flags)
583{ 602{
584 int value=0; 603 int value=0;
585 struct street_data *sd=t->street; 604 struct street_data *sd=t->street;
586 dbg(2, "%d: (0x%x,0x%x)-(0x%x,0x%x)\n", offset, sd->c[offset].x, sd->c[offset].y, sd->c[offset+1].x, sd->c[offset+1].y); 605 //dbg(2, "%d: (0x%x,0x%x)-(0x%x,0x%x)\n", offset, sd->c[offset].x, sd->c[offset].y, sd->c[offset+1].x, sd->c[offset+1].y);
587 if (flags & 1) { 606 if (flags & 1)
607 {
588 struct coord c1, c2, cp; 608 struct coord c1, c2, cp;
589 c1.x = sd->c[offset].x; 609 c1.x = sd->c[offset].x;
590 c1.y = sd->c[offset].y; 610 c1.y = sd->c[offset].y;
591 c2.x = sd->c[offset+1].x; 611 c2.x = sd->c[offset+1].x;
592 c2.y = sd->c[offset+1].y; 612 c2.y = sd->c[offset+1].y;
606 value += tracking_is_no_stop(tr, lpnt, &tr->last_out); 626 value += tracking_is_no_stop(tr, lpnt, &tr->last_out);
607 if (value >= min) 627 if (value >= min)
608 return value; 628 return value;
609 if ((flags & 16) && tr->route_pref) 629 if ((flags & 16) && tr->route_pref)
610 value += tracking_is_on_route(tr, tr->rt, &sd->item); 630 value += tracking_is_on_route(tr, tr->rt, &sd->item);
611 if ((flags & 32) && tr->overspeed_percent_pref && tr->overspeed_pref ) { 631 if ((flags & 32) && tr->overspeed_percent_pref && tr->overspeed_pref )
632 {
612 struct roadprofile *roadprofile=g_hash_table_lookup(tr->vehicleprofile->roadprofile_hash, (void *)t->street->item.type); 633 struct roadprofile *roadprofile=g_hash_table_lookup(tr->vehicleprofile->roadprofile_hash, (void *)t->street->item.type);
613 if (roadprofile && tr->speed > roadprofile->speed * tr->overspeed_percent_pref/ 100) 634 if (roadprofile && tr->speed > roadprofile->speed * tr->overspeed_percent_pref/ 100)
614 value += tr->overspeed_pref; 635 value += tr->overspeed_pref;
615 } 636 }
616 return value; 637 return value;
633 654
634 if (! tr->vehicle) 655 if (! tr->vehicle)
635 return; 656 return;
636 if (!vehicle_get_attr(tr->vehicle, attr_position_valid, &valid, NULL)) 657 if (!vehicle_get_attr(tr->vehicle, attr_position_valid, &valid, NULL))
637 valid.u.num=attr_position_valid_valid; 658 valid.u.num=attr_position_valid_valid;
638 if (valid.u.num == attr_position_valid_invalid) { 659 if (valid.u.num == attr_position_valid_invalid)
660 {
639 tr->valid=valid.u.num; 661 tr->valid=valid.u.num;
640 return; 662 return;
641 } 663 }
642 if (!vehicle_get_attr(tr->vehicle, attr_position_speed, &speed_attr, NULL) || 664 if (!vehicle_get_attr(tr->vehicle, attr_position_speed, &speed_attr, NULL) ||
643 !vehicle_get_attr(tr->vehicle, attr_position_direction, &direction_attr, NULL) || 665 !vehicle_get_attr(tr->vehicle, attr_position_direction, &direction_attr, NULL) ||
644 !vehicle_get_attr(tr->vehicle, attr_position_coord_geo, &coord_geo, NULL) || 666 !vehicle_get_attr(tr->vehicle, attr_position_coord_geo, &coord_geo, NULL) ||
645 !vehicle_get_attr(tr->vehicle, attr_position_time_iso8601, &time_attr, NULL)) { 667 !vehicle_get_attr(tr->vehicle, attr_position_time_iso8601, &time_attr, NULL))
668 {
646 dbg(0,"failed to get position data %d %d %d %d\n", 669 //dbg(0,"failed to get position data %d %d %d %d\n",
647 vehicle_get_attr(tr->vehicle, attr_position_speed, &speed_attr, NULL), 670 //vehicle_get_attr(tr->vehicle, attr_position_speed, &speed_attr, NULL),
648 vehicle_get_attr(tr->vehicle, attr_position_direction, &direction_attr, NULL), 671 //vehicle_get_attr(tr->vehicle, attr_position_direction, &direction_attr, NULL),
649 vehicle_get_attr(tr->vehicle, attr_position_coord_geo, &coord_geo, NULL), 672 //vehicle_get_attr(tr->vehicle, attr_position_coord_geo, &coord_geo, NULL),
650 vehicle_get_attr(tr->vehicle, attr_position_time_iso8601, &time_attr, NULL)); 673 //vehicle_get_attr(tr->vehicle, attr_position_time_iso8601, &time_attr, NULL));
651 return; 674 return;
652 } 675 }
653 if (!vehicleprofile_get_attr(vehicleprofile,attr_static_speed,&static_speed,NULL) || !vehicleprofile_get_attr(vehicleprofile,attr_static_distance,&static_distance,NULL)) { 676 if (!vehicleprofile_get_attr(vehicleprofile,attr_static_speed,&static_speed,NULL) || !vehicleprofile_get_attr(vehicleprofile,attr_static_distance,&static_distance,NULL))
677 {
654 static_speed.u.num=3; 678 static_speed.u.num=3;
655 static_distance.u.num=10; 679 static_distance.u.num=10;
656 dbg(1,"Using defaults for static position detection\n"); 680 //dbg(1,"Using defaults for static position detection\n");
657 } 681 }
658 dbg(2,"Static speed: %u, static distance: %u\n",static_speed.u.num, static_distance.u.num); 682 //dbg(2,"Static speed: %u, static distance: %u\n",static_speed.u.num, static_distance.u.num);
659 time=iso8601_to_secs(time_attr.u.str); 683 time=iso8601_to_secs(time_attr.u.str);
660 speed=*speed_attr.u.numd; 684 speed=*speed_attr.u.numd;
661 direction=*direction_attr.u.numd; 685 direction=*direction_attr.u.numd;
662 tr->valid=attr_position_valid_valid; 686 tr->valid=attr_position_valid_valid;
663 transform_from_geo(pro, coord_geo.u.coord_geo, &tr->curr_in); 687 transform_from_geo(pro, coord_geo.u.coord_geo, &tr->curr_in);
664 if ((speed < static_speed.u.num && transform_distance(pro, &tr->last_in, &tr->curr_in) < static_distance.u.num )) { 688 if ((speed < static_speed.u.num && transform_distance(pro, &tr->last_in, &tr->curr_in) < static_distance.u.num ))
689 {
665 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); 690 //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);
666 tr->valid=attr_position_valid_static; 691 tr->valid=attr_position_valid_static;
667 tr->speed=0; 692 tr->speed=0;
668 return; 693 return;
669 } 694 }
695
670 if (vehicle_get_attr(tr->vehicle, attr_lag, &lag, NULL) && lag.u.num > 0) { 696 if (vehicle_get_attr(tr->vehicle, attr_lag, &lag, NULL) && lag.u.num > 0)
697 {
671 double espeed; 698 double espeed;
672 int edirection; 699 int edirection;
673 if (time-tr->time == 1) { 700 if (time - tr->time == 1)
701 {
674 dbg(1,"extrapolating speed from %f and %f (%f)\n",tr->speed, speed, speed-tr->speed); 702 /* dbg(1,"extrapolating speed from %f and %f (%f)\n",tr->speed, speed, speed-tr->speed); */
675 espeed=speed+(speed-tr->speed)*lag.u.num/10; 703 espeed=speed+(speed-tr->speed)*lag.u.num/10;
676 dbg(1,"extrapolating angle from %f and %f (%d)\n",tr->direction, direction, tracking_angle_diff(direction,tr->direction,360)); 704 /* dbg(1,"extrapolating angle from %f and %f (%d)\n",tr->direction, direction, tracking_angle_diff(direction,tr->direction,360)); */
677 edirection=direction+tracking_angle_diff(direction,tr->direction,360)*lag.u.num/10; 705 edirection=direction+tracking_angle_diff(direction,tr->direction,360)*lag.u.num/10;
706 }
678 } else { 707 else
708 {
679 dbg(1,"no speed and direction extrapolation\n"); 709 //dbg(1,"no speed and direction extrapolation\n");
680 espeed=speed; 710 espeed=speed;
681 edirection=direction; 711 edirection=direction;
682 } 712 }
683 dbg(1,"lag %d speed %f direction %d\n",lag.u.num,espeed,edirection); 713 //dbg(1,"lag %d speed %f direction %d\n",lag.u.num,espeed,edirection);
684 dbg(1,"old 0x%x,0x%x\n",tr->curr_in.x, tr->curr_in.y); 714 //dbg(1,"old 0x%x,0x%x\n",tr->curr_in.x, tr->curr_in.y);
685 transform_project(pro, &tr->curr_in, espeed*lag.u.num/36, edirection, &tr->curr_in); 715 transform_project(pro, &tr->curr_in, espeed*lag.u.num/36, edirection, &tr->curr_in);
686 dbg(1,"new 0x%x,0x%x\n",tr->curr_in.x, tr->curr_in.y); 716 //dbg(1,"new 0x%x,0x%x\n",tr->curr_in.x, tr->curr_in.y);
687 } 717 }
688 tr->time=time; 718 tr->time=time;
689 tr->pro=pro; 719 tr->pro=pro;
690#if 0 720#if 0
691 721
695 tr->speed=speed; 725 tr->speed=speed;
696 tr->last_in=tr->curr_in; 726 tr->last_in=tr->curr_in;
697 tr->last_out=tr->curr_out; 727 tr->last_out=tr->curr_out;
698 tr->last[0]=tr->curr[0]; 728 tr->last[0]=tr->curr[0];
699 tr->last[1]=tr->curr[1]; 729 tr->last[1]=tr->curr[1];
700 if (!tr->lines || transform_distance(pro, &tr->last_updated, &tr->curr_in) > 500) { 730 if (!tr->lines || transform_distance(pro, &tr->last_updated, &tr->curr_in) > 500)
731 {
701 dbg(1, "update\n"); 732 //dbg(1, "update\n");
702 tracking_flush(tr); 733 tracking_flush(tr);
703 tracking_doupdate_lines(tr, &tr->curr_in, pro); 734 tracking_doupdate_lines(tr, &tr->curr_in, pro);
704 tr->last_updated=tr->curr_in; 735 tr->last_updated=tr->curr_in;
705 dbg(1,"update end\n"); 736 //dbg(1,"update end\n");
706 } 737 }
707 738
708 tr->street_direction=0; 739 tr->street_direction=0;
709 t=tr->lines; 740 t=tr->lines;
710 tr->curr_line=NULL; 741 tr->curr_line=NULL;
711 min=INT_MAX/2; 742 min=INT_MAX/2;
712 while (t) { 743 while (t)
744 {
713 struct street_data *sd=t->street; 745 struct street_data *sd=t->street;
714 for (i = 0; i < sd->count-1 ; i++) { 746 for (i = 0; i < sd->count-1 ; i++)
747 {
715 value=tracking_value(tr,t,i,&lpnt,min,-1); 748 value=tracking_value(tr,t,i,&lpnt,min,-1);
716 if (value < min) { 749 if (value < min)
750 {
717 struct coord lpnt_tmp; 751 struct coord lpnt_tmp;
718 int angle_delta=tracking_angle_abs_diff(tr->curr_angle, t->angle[i], 360); 752 int angle_delta=tracking_angle_abs_diff(tr->curr_angle, t->angle[i], 360);
719 tr->curr_line=t; 753 tr->curr_line=t;
720 tr->pos=i; 754 tr->pos=i;
721 tr->curr[0]=sd->c[i]; 755 tr->curr[0]=sd->c[i];
722 tr->curr[1]=sd->c[i+1]; 756 tr->curr[1]=sd->c[i+1];
757 /*
723 dbg(1,"lpnt.x=0x%x,lpnt.y=0x%x pos=%d %d+%d+%d+%d=%d\n", lpnt.x, lpnt.y, i, 758 dbg(1,"lpnt.x=0x%x,lpnt.y=0x%x pos=%d %d+%d+%d+%d=%d\n", lpnt.x, lpnt.y, i,
724 transform_distance_line_sq(&sd->c[i], &sd->c[i+1], &cin, &lpnt_tmp), 759 transform_distance_line_sq(&sd->c[i], &sd->c[i+1], &cin, &lpnt_tmp),
725 tracking_angle_delta(tr, tr->curr_angle, t->angle[i], 0)*tr->angle_pref, 760 tracking_angle_delta(tr, tr->curr_angle, t->angle[i], 0)*tr->angle_pref,
726 tracking_is_connected(tr, tr->last, &sd->c[i]) ? tr->connected_pref : 0, 761 tracking_is_connected(tr, tr->last, &sd->c[i]) ? tr->connected_pref : 0,
727 lpnt.x == tr->last_out.x && lpnt.y == tr->last_out.y ? tr->nostop_pref : 0, 762 lpnt.x == tr->last_out.x && lpnt.y == tr->last_out.y ? tr->nostop_pref : 0,
728 value 763 value
729 ); 764 );
765 */
730 tr->curr_out.x=lpnt.x; 766 tr->curr_out.x=lpnt.x;
731 tr->curr_out.y=lpnt.y; 767 tr->curr_out.y=lpnt.y;
732 tr->coord_geo_valid=0; 768 tr->coord_geo_valid=0;
733 if (angle_delta < 70) 769 if (angle_delta < 70)
734 tr->street_direction=1; 770 tr->street_direction=1;
739 min=value; 775 min=value;
740 } 776 }
741 } 777 }
742 t=t->next; 778 t=t->next;
743 } 779 }
780
744 dbg(1,"tr->curr_line=%p min=%d\n", tr->curr_line, min); 781 //dbg(1,"tr->curr_line=%p min=%d\n", tr->curr_line, min);
782
745 if (!tr->curr_line || min > tr->offroad_limit_pref) { 783 if (!tr->curr_line || min > tr->offroad_limit_pref)
784 {
746 tr->curr_out=tr->curr_in; 785 tr->curr_out=tr->curr_in;
747 tr->coord_geo_valid=0; 786 tr->coord_geo_valid=0;
748 tr->street_direction=0; 787 tr->street_direction=0;
749 } 788 }
789
750 if (tr->curr_line && (tr->curr_line->street->flags & AF_UNDERGROUND)) { 790 if (tr->curr_line && (tr->curr_line->street->flags & AF_UNDERGROUND))
791 {
751 //dbg(0,"AF_UNDERGROUND 1"); 792 //dbg(0,"AF_UNDERGROUND 1");
752 if (tr->no_gps) 793 if (tr->no_gps)
794 {
753 tr->tunnel=1; 795 tr->tunnel=1;
796 }
797 }
754 } else if (tr->tunnel) { 798 else if (tr->tunnel)
799 {
755 //dbg(0,"AF_UNDERGROUND 2"); 800 //dbg(0,"AF_UNDERGROUND 2");
756 tr->speed=0; 801 tr->speed=0;
757 } 802 }
758 dbg(1,"found 0x%x,0x%x\n", tr->curr_out.x, tr->curr_out.y); 803 //dbg(1,"found 0x%x,0x%x\n", tr->curr_out.x, tr->curr_out.y);
759} 804}
760 805
761static int 806static int
762tracking_set_attr_do(struct tracking *tr, struct attr *attr, int initial) 807tracking_set_attr_do(struct tracking *tr, struct attr *attr, int initial)
763{ 808{

Legend:
Removed from v.2  
changed lines
  Added in v.27

   
Visit the ZANavi Wiki