#include #include #include #include #include #include #ifdef __sgi #include #endif #include "dyna-trackd.h" void trackd_cleanup(int); void get_tracker_mem(int shmemKey); void init_tracker_data(void); void get_timestamp(uint32_t stamp[2]); void dyna_init_tracker(char *portname); int dyna_read_data(CAVE_SENSOR_ST *sensor); void dyna_handle_command(int cmd); void dyna_reset_tracker(void); static int dyna_fd; static struct TRACKD_TRACKING *tracker=NULL; main(int argc,char **argv) { int loop, sensornum, shmemKey=9900; CAVE_SENSOR_ST sensor; char *portname = "/dev/ttyS0"; signal(SIGINT,trackd_cleanup); /* catch interrupts to close nicely */ #ifdef __sgi schedctl(NDPRI,0,NDPHIMIN); /* set a non-degrading priority */ #endif if (argc > 1) shmemKey = atoi(argv[1]); if (argc > 2) portname = argv[2]; get_tracker_mem(shmemKey); init_tracker_data(); dyna_init_tracker(portname); while (1) { fd_set fdset; FD_ZERO(&fdset); FD_SET(dyna_fd, &fdset); select(32, &fdset, NULL, NULL, NULL); if (FD_ISSET(dyna_fd, &fdset)) { if (dyna_read_data(&sensor)) { tracker->sensor[0] = sensor; get_timestamp(tracker->header.timestamp); } } if (tracker->header.command) { dyna_handle_command(tracker->header.command); tracker->header.command = 0; } } } void dyna_init_tracker(char *portname) { struct termio port_attributes; dyna_fd = open(portname,O_RDWR | O_NOCTTY | O_NDELAY); if (dyna_fd == -1) { fprintf(stderr,"Serial port can not be opened\n"); perror(portname); exit(-1); } /* get port attributes */ ioctl(dyna_fd,TCGETA,&port_attributes); /* set new port attributes */ port_attributes.c_iflag = 0; port_attributes.c_oflag = 0; port_attributes.c_lflag = 0; port_attributes.c_line = 0; port_attributes.c_cflag = (B19200 | CS8 | CLOCAL | CREAD); port_attributes.c_cc[0] = 0; port_attributes.c_cc[1] = 0; port_attributes.c_cc[2] = 0; port_attributes.c_cc[3] = 0; port_attributes.c_cc[4] = 0; port_attributes.c_cc[5] = 100; ioctl(dyna_fd,TCSETA,&port_attributes); dyna_reset_tracker(); } #define DYNABUFSIZE 32 static unsigned char dyna_buf[DYNABUFSIZE]; static int dyna_bytes = 0; void dyna_process_byte(unsigned char c) { dyna_buf[dyna_bytes++] = c; if (dyna_bytes < 3) { if ((c & 0xf0) != 0x80) dyna_bytes = 0; } else if (dyna_bytes == 3) { if ((c & 0xf0) == 0x80) { dyna_buf[0] = dyna_buf[1]; dyna_buf[1] = dyna_buf[2]; dyna_bytes = 2; } } if (dyna_bytes >= DYNABUFSIZE) { fprintf(stderr,"ERROR: Overflow of input buffer " "(this should never even happen!)\n"); dyna_bytes = 0; } } /* According to the manual, the reported x/y/z are "scaled at .05mm per least significant bit". .05 mm is .000164042 feet */ #define DYNASIGHT_RESOLUTION 0.000164042 int dyna_read_data(CAVE_SENSOR_ST *sensor) { unsigned char c=0; while ((read(dyna_fd,&c,1) > 0) && (dyna_bytes < 8)) dyna_process_byte(c); if (dyna_bytes >= 8) { int exponent; signed short x, y, z; dyna_bytes = 0; if ((dyna_buf[0] & 0x0c) != 0) return 0; exponent = dyna_buf[0] & 3; x = (dyna_buf[2] << 8) | dyna_buf[3]; y = (dyna_buf[4] << 8) | dyna_buf[5]; z = (dyna_buf[6] << 8) | dyna_buf[7]; sensor->x = x * (DYNASIGHT_RESOLUTION * (1 << exponent)); sensor->y = y * (DYNASIGHT_RESOLUTION * (1 << exponent)); sensor->z = z * (DYNASIGHT_RESOLUTION * (1 << exponent)); sensor->azim = 0; sensor->elev = 0; sensor->roll = 0; get_timestamp((uint32_t*)&sensor->timestamp); return 1; } return 0; } void dyna_handle_command(int cmd) { if (cmd == CAVE_TRACKD_RESET_COMMAND) dyna_reset_tracker(); } void dyna_reset_tracker(void) { /* Set tracker to 'retro' mode */ write(dyna_fd,"\003",1); write(dyna_fd,"0",1); } /******************************************************************/ #define PERMS 0666 static int tracker_shmid=-1; /************************************************************************* void trackd_cleanup() When the daemon is interrupted by SIGINT, this routine closes the connections, disposes of the memory, and exits. **************************************************************************/ void trackd_cleanup(int foo) { fprintf(stderr,"Interrupted. Quitting...\n"); if (tracker) if (shmdt(tracker) < 0) perror("Detaching 'tracker' shared memory"); if (tracker_shmid > -1) if (shmctl(tracker_shmid,IPC_RMID,NULL) < 0) perror("Removing 'tracker_shmid'"); exit(-1); } /************************************************************************* void get_tracker_mem() Allocates shared memory for the sensors. The global 'tracker' points to the chunk. **************************************************************************/ void get_tracker_mem(int shmemKey) { tracker_shmid = shmget(shmemKey, sizeof(struct TRACKD_TRACKING), PERMS | IPC_CREAT); if (tracker_shmid < 0) { fprintf(stderr,"can't get shared memory\n"); exit(-1); } tracker = (struct TRACKD_TRACKING *) shmat(tracker_shmid,(char *) 0, 0); if (tracker == (struct TRACKD_TRACKING *) -1) { fprintf(stderr,"can't attach shared memory\n"); exit(-1); } } /************************************************************************* void init_tracker_data() Fills in the header of the tracker data, and fills all the sensors with defaults (0 0 0). **************************************************************************/ void init_tracker_data(void) { int loop; CAVE_SENSOR_ST sensor; tracker->header.version = CAVELIB_2_6; tracker->header.numSensors = 2; tracker->header.sensorOffset = ((char *)&tracker->sensor[0]) - ((char *)tracker); tracker->header.sensorSize = sizeof(CAVE_SENSOR_ST); tracker->header.timestamp[0] = tracker->header.timestamp[1] = 0; tracker->header.command = 0; sensor.x = 0; sensor.y = 0; sensor.z = 0; sensor.elev = 0; sensor.azim = 0; sensor.roll = 0; sensor.calibrated = 0; sensor.frame = CAVE_TRACKER_FRAME; for (loop=0; loop < TRACKD_MAX_SENSORS; ++loop) tracker->sensor[loop] = sensor; } void get_timestamp(uint32_t stamp[2]) { struct timeval curtime; gettimeofday(&curtime,NULL); stamp[0] = curtime.tv_sec; stamp[1] = curtime.tv_usec; }