void vrpn_SerialPort::flush_output_buffer() {
requiresOpen();
int ret = vrpn_flush_output_buffer(_comm);
if (ret == -1) {
throw FlushFailure();
}
}
vrpn_Tracker_Flock_Parallel_Slave::~vrpn_Tracker_Flock_Parallel_Slave() {
// Some compilers (CenterLine CC on sunos!?) still don't support
// automatic aggregate initialization
int cLen=0;
//unsigned char rgch[2]={'B','G'};
unsigned char rgch [2];
rgch[cLen++] = 'B';
// slaves can't do a sleep
// rgch[cLen++] = 'G';
fprintf(stderr,"\nvrpn_Tracker_Flock_Parallel_Slave %d: shutting down ...", d_sensor);
// clear output buffer
vrpn_flush_output_buffer( serial_fd );
// put the flock to sleep (B to get out of stream mode, G to sleep)
if (vrpn_write_characters(serial_fd, (const unsigned char *) rgch, cLen )!=cLen) {
fprintf(stderr,"\nvrpn_Tracker_Flock_Parallel_Slave %d: "
"failed writing sleep cmd to tracker", d_sensor);
status = vrpn_TRACKER_FAIL;
return;
}
// make sure the command is sent out
vrpn_drain_output_buffer( serial_fd );
fprintf(stderr, " done.\n");
}
void vrpn_Tracker_NDI_Polaris::sendCommand(const char* commandString ){
vrpn_write_characters(serialFd,(const unsigned char* )commandString,strlen(commandString));
vrpn_write_characters(serialFd,(const unsigned char* )"\r",1); //send the CR
vrpn_flush_output_buffer(serialFd);
}
/* extern */ error_t
getCmd (handle_t fd, struct command_t* Cmd)
{
struct timeval time_wait;
int ret;
#ifdef VRPN_ATMEL_SERIAL_VRPN
unsigned char read_val;
#endif
// some adds to measure time for reading out
#ifdef VRPN_ATMELLIB_TIME_MEASURE
struct timeval start;
struct timeval end;
int sec, usec;
gettimeofday( &start , 0);
#endif
unsigned char PossibilityOne;
unsigned char PossibilityTwo;
unsigned char Reference;
/* check if the given parameters are valid */
if (handle_invalid(fd))
return ENXIO;
if ((*Cmd).addr < 128)
return ATMELLIB_ERROR_NOCOMVAL;
PossibilityOne = (*Cmd).addr;
PossibilityTwo = (*Cmd).addr;
Reference = 0x01;
if (Reference & PossibilityTwo)
/* the LSB of the Address is 1 -> make 0 -> use XOR operator */
PossibilityTwo ^= Reference;
else
/* the LSB of the address is 0 -> make 1 -> use OR operator */
PossibilityTwo |= Reference;
#ifdef VRPN_ATMEL_SERIAL_VRPN
vrpn_flush_input_buffer(fd);
vrpn_flush_output_buffer(fd);
#else
tcflush( fd , TCIOFLUSH );
#endif
time_wait.tv_sec = VRPN_ATMELLIB_SELECT_WAIT_SEC;
time_wait.tv_usec = VRPN_ATMELLIB_SELECT_WAIT_USEC;
#ifdef VRPN_ATMEL_SERIAL_VRPN
vrpn_write_characters(fd, (&((*Cmd).addr)) , 1);
#else
/* you have to send the address first */
if ( (ret = select_write_wrapper( fd ,
&(time_wait),
(&((*Cmd).addr)) ,
1 ))
!= 1 ) {
fprintf(stderr, "\n vrpn_atmellib::getCmd: Error while writing down. error=%i\n",
ret);
return ret;
}
#endif
while (time_wait.tv_usec!=0) {
#ifdef VRPN_ATMEL_SERIAL_VRPN
if (( vrpn_read_available_characters(fd, &(read_val), 1, &time_wait)) != 1) {
fprintf(stderr, "vrpn_atmellib::getCmd: Error vrpn_read_available_characters for first byte\n");
break;
}
// else
ret = read_val;
#else
if ((ret = select_read_wrapper(fd, &time_wait)) < 0) {
fprintf(stderr, "vrpn_atmellib::getCmd:\
Error select_read_wrapper for first byte: %i\n" , ret);
break;
}
#endif
// found expected first byte
if ((ret==PossibilityOne) || (ret==PossibilityTwo )) {
(*Cmd).addr = ret;
#ifdef VRPN_ATMEL_SERIAL_VRPN
if (( vrpn_read_available_characters(fd, &(read_val), 1, &time_wait)) != 1) {
fprintf(stderr, "vrpn_atmellib::getCmd: Error vrpn_read_available_characters.\n");;
break;
}
//else
ret = read_val;
#else
ret = select_read_wrapper(fd, &time_wait);
#endif
if ((ret < 0) || (ret > 128)) {
fprintf(stderr, "vrpn_atmellib::getCmd: Error reading second byte: %i\n\n" , ret);
break;
}
(*Cmd).value = ret;
#ifdef VRPN_ATMELLIB_TIME_MEASURE
// display time for
gettimeofday( &end , 0);
sec=end.tv_sec-start.tv_sec;
usec=end.tv_usec-start.tv_usec;
printf("Time for reading out: sec=%i , usec=%i\n", sec, usec);
#endif
return ATMELLIB_NOERROR;
}
}
void vrpn_Tracker_Crossbow::reset() {
const char *cmd;
unsigned char recv_buf[8];
struct timeval timeout;
timeout.tv_sec = 1;
timeout.tv_usec = 0;
#if 0 // doesn't help
// First, take the comm port offline for a second
vrpn_close_commport(serial_fd);
vrpn_SleepMsecs(1000);
serial_fd = vrpn_open_commport(portname, baudrate);
#endif
vrpn_flush_output_buffer(serial_fd);
vrpn_flush_input_buffer(serial_fd);
// Try resetting by toggling the RTS line of the serial port
vrpn_set_rts(serial_fd);
vrpn_SleepMsecs(750);
vrpn_clear_rts(serial_fd);
vrpn_SleepMsecs(250);
vrpn_gettimeofday(×tamp, NULL);
vrpn_flush_input_buffer(serial_fd);
cmd = "P";
vrpn_write_characters(serial_fd, reinterpret_cast<const unsigned char*> (cmd), 1);
vrpn_SleepMsecs(50); // Sleep long enough to stop receiving data
vrpn_flush_input_buffer(serial_fd);
cmd = "RSv";
vrpn_write_characters(serial_fd, reinterpret_cast<const unsigned char*> (cmd), 3);
vrpn_drain_output_buffer(serial_fd);
if (vrpn_read_available_characters(serial_fd, recv_buf, 8, &timeout) != 8) {
fprintf(stderr, "vrpn_Tracker_Crossbow::reset: Crossbow not responding to stimulus\n");
status = vrpn_TRACKER_FAIL;
return;
}
if ((recv_buf[0] != 'H') || (recv_buf[1] != 255) || (recv_buf[7] != 255)) {
fprintf(stderr, "vrpn_Tracker_Crossbow::reset: Crossbow gave unexpected ping response\n");
status = vrpn_TRACKER_FAIL;
return;
}
if (recv_buf[6] != ((recv_buf[2] + recv_buf[3] + recv_buf[4] + recv_buf[5]) & 0xFF)) {
fprintf(stderr, "vrpn_Tracker_Crossbow::reset: Crossbow gave invalid serial number checksum\n");
status = vrpn_TRACKER_FAIL;
return;
}
const char *bufptr = reinterpret_cast<const char *>(&recv_buf[2]);
vrpn_unbuffer(&bufptr, &device_serial);
if (0) do {
if (!vrpn_read_available_characters(serial_fd, recv_buf, 1, &timeout)) {
fprintf(stderr, "vrpn_Tracker_Crossbow::reset: Crossbow not responding to stimulus\n");
status = vrpn_TRACKER_FAIL;
return;
}
} while (*recv_buf != 255);
int curSize = 4, curLen = 0;
device_version = (char *) realloc(device_version, curSize * sizeof(char));
if (device_version == NULL) {
fprintf(stderr, "vrpn_Tracker_Crossbow::reset: Out of memory\n");
status = vrpn_TRACKER_FAIL;
return;
}
do {
if (!vrpn_read_available_characters(serial_fd, recv_buf, 1, &timeout)) {
fprintf(stderr, "vrpn_Tracker_Crossbow::reset: Crossbow not responding to stimulus\n");
status = vrpn_TRACKER_FAIL;
return;
}
if (*recv_buf != '$')
device_version[curLen++] = *recv_buf;
if (curLen == curSize)
device_version = (char *) realloc(device_version, curSize *= 2);
} while (*recv_buf != '$');
// Now null-terminate the version string, expanding it one last time if necessary
if (curLen == curSize)
device_version = (char *) realloc(device_version, ++curSize);
device_version[curLen] = 0;
//printf("Serial %u\tVersion '%s'\n", device_serial, device_version);
just_read_something = 0;
status = vrpn_TRACKER_SYNCING;
}
