|
|
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 | |
488 | void |
507 | void |
489 | tracking_flush(struct tracking *tr) |
508 | tracking_flush(struct tracking *tr) |
… | |
… | |
581 | static int |
600 | static int |
582 | tracking_value(struct tracking *tr, struct tracking_line *t, int offset, struct coord *lpnt, int min, int flags) |
601 | tracking_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 | |
761 | static int |
806 | static int |
762 | tracking_set_attr_do(struct tracking *tr, struct attr *attr, int initial) |
807 | tracking_set_attr_do(struct tracking *tr, struct attr *attr, int initial) |
763 | { |
808 | { |