/* Copyright (C) 2003-2009 Egon Wanke Name this file "tracker_toa.cc" and compile it by > g++ -Wall -lm -o tracker_toa tracker_toa.cc then try > ./tracker_toa -h */ /******************************************************************************/ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include /******************************************************************************/ /******************************************************************************/ /******************************************************************************/ #define VERSION "LinuxTracker Ver. 10.3" #define SLEEP_TIME_SEC 20 #define STRIKES_SERVER_IP "81.169.143.239" #define STRIKE_SERVER_PORT 8308 #define STRING_BUFFER_SIZE 256 #define STRIKE_BUFFER_SIZE 10000 #define RING_BUFFER_SIZE 30 #define PRECISION 0.000001000 char init_gps_sjn[]="\ $PFEC,GPint,GGA00,GLL00,GSA00,GSV00,RMC01,DTM00,VTG00,ZDA00*00\n"; char init_gps_garmin[]= "\ $PGRMO,GPGGA,0*00\n\ $PGRMO,GPGSA,0*00\n\ $PGRMO,GPGSV,0*00\n\ $PGRMO,GPRMC,1*00\n\ $PGRMO,GPVTG,0*00\n\ $PGRMO,PGRMT,0*00\n\ $PGRMCE*00\n\ $PGRMC,,51.5,,,,,,,,3,,2,4,*00\n"; char init_gps_sirf[]= "\ $PSRF103,00,00,00,01*00\n\ $PSRF103,01,00,00,01*00\n\ $PSRF103,02,00,00,01*00\n\ $PSRF103,03,00,00,01*00\n\ $PSRF103,04,00,01,01*00\n\ $PSRF103,05,00,00,01*00\n\ $PSRF103,06,00,00,01*00\n\ $PSRF103,08,00,00,01*00\n"; /******************************************************************************/ /******************************************************************************/ /******************************************************************************/ extern int errno; bool verbose_flag; long long sleep_time; struct strike_type { int year; int mon; int day; int hour; int min; int sec; long long nsec; long double lat; long double lon; int channel_A; int channel_B; char status; }; struct strike_type strikes [STRIKE_BUFFER_SIZE]; int cnt1= 0; int cnt2= 0; int cnt3= 0; char *device; char *username; char *password; int last_year, last_mon, last_day, last_hour, last_min, last_sec; long double last_lat, last_lon; long long last_time; long long last_time_send= 0ll; char last_status; long long counter_ring_buffer [RING_BUFFER_SIZE]; struct position_type { long double lat; long double lon; } position_ring_buffer [RING_BUFFER_SIZE]; int ring_buffer_cnt= 0; long long counter_last; long long counter_sum= 1; long double counter_average; long double counter_precision; pid_t cld_pid= 0; /******************************************************************************/ /***** initialization *********************************************************/ /******************************************************************************/ char int_to_hex (int b) { b&= 0x0F; if (b > 9) { return(b+'A'-10); } else { return (b+'0'); } } void fill_checksum (char *buf) { unsigned int i; int c= 0; for (i=0; i < strlen (buf); i++) { if (buf[i] == '$') { c= 0; } else if (buf[i] == '*') { buf [i+1]= int_to_hex (c>>4); buf [i+2]= int_to_hex (c); i+=2; } else { c^=buf[i]; } } } void init_gps (int f, char *gps_device) { char init_string [STRING_BUFFER_SIZE]; if (strcmp (gps_device, "sjn") == 0) { strcpy (init_string, init_gps_sjn); } else if (strcmp (gps_device, "garmin") == 0) { strcpy (init_string, init_gps_garmin); } else if (strcmp (gps_device, "sirf") == 0) { strcpy (init_string, init_gps_sirf); } fill_checksum (init_string); if (verbose_flag) { printf ("%s",init_string); } write (f, init_string, strlen(init_string)); } /******************************************************************************/ /***** time functions *********************************************************/ /******************************************************************************/ long long utc_ctime_to_ensec (int year, int mon, int day, int hour, int min, long double sec_nsec) { int sec= (int)sec_nsec; long long nsec= (long long)(sec_nsec*1000000000ll)-sec*1000000000ll; struct tm t; t.tm_year= year-1900; t.tm_mon= mon-1; t.tm_mday= day; t.tm_hour= hour; t.tm_min= min; t.tm_sec= (int)sec; time_t esec= timegm(&t); return((long long)esec*1000000000ll+nsec); } void ensec_to_utc_ctime (long long ensec, int *year, int *mon, int *day, int *hour, int *min, long double *sec_nsec) { time_t esec= ensec/1000000000ll; struct tm *t = gmtime(&esec); *year= t->tm_year+1900; *mon= t->tm_mon+1; *day= t->tm_mday; *hour= t->tm_hour; *min= t->tm_min; *sec_nsec= t->tm_sec+(ensec%1000000000ll)/1000000000.0; } long long ensec_time () { struct timeval t; gettimeofday(&t,(struct timezone *)0); return((long long)(t.tv_sec)*1000000000ll+(long long)t.tv_usec*1000ll); } int compare_ensec (long long x, long long y) { if (x < y) { return(-1); } if (x > y) { return(+1); } return(0); } /******************************************************************************/ /***** open connection ********************************************************/ /******************************************************************************/ int open_connection () { struct hostent *hostinfo; int sockfd; char buf [STRING_BUFFER_SIZE]; sockfd= socket (AF_INET, SOCK_STREAM, 0); if (sockfd == -1) { exit (0); } sockaddr_in serv_addr; serv_addr.sin_family= AF_INET; serv_addr.sin_port= htons(STRIKE_SERVER_PORT); serv_addr.sin_addr.s_addr= inet_addr (STRIKES_SERVER_IP); if (serv_addr.sin_addr.s_addr == INADDR_NONE) { /* host not given by IP but by name */ hostinfo= gethostbyname (STRIKES_SERVER_IP); if (hostinfo == NULL) { close (sockfd); exit (0); } memcpy((char*) &serv_addr.sin_addr.s_addr, hostinfo->h_addr, hostinfo->h_length); } if (connect(sockfd, (sockaddr *) &serv_addr, sizeof(sockaddr)) == -1) { exit (0); } if (recv (sockfd, buf, STRING_BUFFER_SIZE, 0) == -1) { exit (0); } return sockfd; } /******************************************************************************/ /***** send data **************************************************************/ /******************************************************************************/ void send_data () { int sockfd= open_connection (); char buf [STRING_BUFFER_SIZE]; while (cnt3 != cnt2) { sprintf (buf, "%04d-%02d-%02d %02d:%02d:%02d.%09lld %.6Lf %.6Lf %s %s %d %d %c %s\n", strikes [cnt3].year, strikes [cnt3].mon, strikes [cnt3].day, strikes [cnt3].hour, strikes [cnt3].min, strikes [cnt3].sec, strikes [cnt3].nsec, strikes [cnt3].lat, strikes [cnt3].lon, username, password, strikes [cnt3].channel_A, strikes [cnt3].channel_B, strikes [cnt3].status, VERSION); if (send (sockfd, buf, strlen(buf), 0) == -1) { exit (0); } if (verbose_flag) { printf ("%s",buf); } cnt3++; cnt3%= STRIKE_BUFFER_SIZE; } send (sockfd, "\n\n", 2, 0); close (sockfd); exit (1); } /******************************************************************************/ /***** evaluate ***************************************************************/ /******************************************************************************/ void evaluate (char * buf, int f) { int year, mon, day, hour, min, sec; int lat_deg, lon_deg; long double lat_min, lon_min; char ns, we, status; long long counter, difference, channel_A, channel_B; if (sscanf (buf, "$BLSEC,%2d%2d%2d,%2d%2d%2d,%c,%2d%Lf,%c,%3d%Lf,%c,%llx", &hour, &min, &sec, &day, &mon, &year, &status, &lat_deg, &lat_min, &ns, &lon_deg, &lon_min, &we, &counter) == 14) { last_lat= lat_deg+lat_min/60.0; if (ns == 'S') { last_lat= -last_lat; } position_ring_buffer [ring_buffer_cnt].lat = last_lat; last_lon= lon_deg+lon_min/60.0; if (ns == 'W') { last_lon= -last_lon; } position_ring_buffer [ring_buffer_cnt].lon = last_lon; if (counter > counter_last) { difference= counter-counter_last; } else { difference= (counter+0x1000000ll)-counter_last; } counter_last= counter; counter_ring_buffer [ring_buffer_cnt]= difference; ring_buffer_cnt= (ring_buffer_cnt+1)%RING_BUFFER_SIZE; counter_sum= 0; last_lat= 0.0; last_lon= 0.0; for (int i=0; i last_time_send + sleep_time) && (cnt2 != cnt3)) { if (cld_pid > 0) { kill (cld_pid, SIGKILL); } cld_pid= fork (); if (cld_pid == 0) { send_data (); } cnt3= cnt2; last_time_send= last_time; } } else if (sscanf (buf, "$BLSIG,%llx,%llx,%llx", &counter, &channel_A, &channel_B) == 3) { if (counter > counter_last) { difference= counter-counter_last; } else { difference= (counter+0x1000000ll)-counter_last; } channel_A= (channel_A>>2)-0x200; channel_B= (channel_B>>2)-0x200; if (verbose_flag) { if (channel_B != 0) { printf ("%7lld, Channel_A=%4lld, Channel_B=%4lld, Angle=%6.1Lf\n", difference, channel_A, channel_B, atanl((long double)channel_A/(long double)channel_B)*180.0/M_PI); } else { printf ("%7lld, Channel_A=%4lld, Channel_B=%4lld, Angle=-\n", difference, channel_A, channel_B); } } long long last_nsec= (difference*RING_BUFFER_SIZE*1000000000ll)/counter_sum; if ((last_nsec >= 0) && (last_nsec < 1000000000ll)) { strikes [cnt1].year= last_year; strikes [cnt1].mon= last_mon; strikes [cnt1].day= last_day; strikes [cnt1].hour= last_hour; strikes [cnt1].min= last_min; strikes [cnt1].sec= last_sec; strikes [cnt1].nsec= last_nsec; strikes [cnt1].lat= last_lat; strikes [cnt1].lon= last_lon; strikes [cnt1].status= last_status; strikes [cnt1].channel_A= channel_A; strikes [cnt1].channel_B= channel_B; cnt1++; cnt1%= STRIKE_BUFFER_SIZE; } } } /******************************************************************************/ /***** main *******************************************************************/ /******************************************************************************/ int main (int argc, char **argv) { char program_name [STRING_BUFFER_SIZE]; program_name[0]= 0; if (argc >0) { strcpy(program_name,argv[0]); argc--; argv++; } sleep_time= SLEEP_TIME_SEC*1000000000ll; bool tty_device_flag= true; verbose_flag= false; char gps_device[STRING_BUFFER_SIZE]; gps_device[0] = 0; int gps_init_day= 0; bool help_flag= false; bool flag_found; do { flag_found= false; if ((argc > 1) && (strcmp(argv[0],"-t") == 0)) { flag_found= true; sleep_time= atoll(argv[1])*1000000000ll; argc-= 2; argv+= 2; } if ((argc > 1) && (strcmp(argv[0],"-i") == 0)) { flag_found= true; strcpy (gps_device, argv[1]); argc-= 2; argv+= 2; } if ((argc > 0) && (strcmp(argv[0],"-n") == 0)) { flag_found= true; tty_device_flag= false; argc-=1; argv+=1; } if ((argc > 0) && (strcmp(argv[0],"-v") == 0)) { flag_found= true; verbose_flag= true; argc-=1; argv+=1; } if ((argc > 0) && (strcmp(argv[0],"-h") == 0)) { flag_found= true; help_flag= true; argc--; argv++; } } while (flag_found); if ((help_flag)||(argc != 3)) { printf ("%s: [-t sleep_time] [-n] [-v] [-h] device username password\n",program_name); printf ("device : serial device (example: /dev/ttyS0)\n"); printf ("username : username (example: PeterPim) \n"); printf ("password : password (example: xxxxxxxx)\n"); printf ("-t sec : sleep time (default = %d sec)\n",SLEEP_TIME_SEC); printf ("-i gps : initialization with sjn, garmin, or sirf\n"); printf ("-n : device is not a tty\n"); printf ("-v : verbose mode\n"); printf ("-h : print this help text\n"); exit (-1); } device= argv[0]; username= argv[1]; password= argv[2]; struct sigaction act; act.sa_handler = NULL; act.sa_flags = SA_NOCLDWAIT; sigaction(SIGCHLD,&act,NULL); int f= open(device, O_RDWR | O_NOCTTY ); if (f < 0) { printf("Can't open %s\n",device); exit(-1); } if (tty_device_flag) { struct termios tio; tio.c_iflag= IGNBRK | IGNPAR ; tio.c_oflag= OPOST | ONLCR ; tio.c_cflag= B4800 | CS8 | CLOCAL | CREAD ; tio.c_lflag= 0; tio.c_cc[VTIME]= 0; tio.c_cc[VMIN]= 1; tcsetattr(f, TCSANOW, &tio); } char buf[STRING_BUFFER_SIZE], c; int i=0; int year, mon, day, hour, min; long double sec_nsec; while (true) { ensec_to_utc_ctime (ensec_time(), &year, &mon, &day, &hour, &min, &sec_nsec); if (day != gps_init_day) { init_gps (f, gps_device); gps_init_day= day; } if (read(f, &c, 1) == 1) { buf [i]= c; if (verbose_flag) { putchar (c); } if (i < STRING_BUFFER_SIZE-2) { i++; } if (c == '\n') { buf [i]= 0; evaluate (buf, f); i= 0; } } } }