void vrpn_SerialPort::clear_rts() {
requiresOpen();
int ret = vrpn_clear_rts(_comm);
if (ret == -1) {
throw RTSFailure();
}
}
vrpn_Tracker_NDI_Polaris::vrpn_Tracker_NDI_Polaris(const char *name,
vrpn_Connection *c,
const char *port,
int numOfRigidBodies,
const char** rigidBodyNDIRomFileNames) : vrpn_Tracker(name,c)
{
/////////////////////////////////////////////////////////
//STEP 1: open com port at NDI's default speed
/////////////////////////////////////////////////////////
serialFd=vrpn_open_commport(port,9600);
if (serialFd==-1){
fprintf(stderr,"vrpn_Tracker_NDI_Polaris: Can't open serial port: %s\n",port);
} else {
printf("connected to NDI Polaris at default 9600 baud on device:%s.\n",port);
//send a reset
#ifdef DEBUG
printf("DEBUG: Reseting com port");
#endif
vrpn_set_rts(serialFd);
vrpn_SleepMsecs(100);
vrpn_clear_rts(serialFd);
printf("done\n");
vrpn_SleepMsecs(100); //if the NDI was previously running at the higher rate, it will need some time to reset
vrpn_flush_input_buffer(serialFd); //get rid of any reset message the NDI might have sent
/////////////////////////////////////////////////////////
//STEP 2: switch to a higher baud rate
/////////////////////////////////////////////////////////
switchToHigherBaudRate(port);
/////////////////////////////////////////////////////////
//STEP 3: INIT tracker
/////////////////////////////////////////////////////////
sendCommand("INIT ");
readResponse();
#ifdef DEBUG
printf("DEBUG:Init response: >%s<\n",latestResponseStr);
#endif
//0 = 20hz(default), 1= 30Hz, 2=60Hz
sendCommand("IRATE 0"); //set the illumination to the default
readResponse();
#ifdef DEBUG
printf("DEBUG: IRATE response: >%s<\n",latestResponseStr);
#endif
/////////////////////////////////////////////////////////
//STEP 4: SETUP EACH TOOL (i.e. rigid body)
/////////////////////////////////////////////////////////
this->numOfRigidBodies=numOfRigidBodies;
//declare an array of filenames, one for each tool
for (int t=0; t<numOfRigidBodies; t++) {
if (setupOneTool(rigidBodyNDIRomFileNames[t])<1) {
fprintf(stderr,"vrpn_Tracker_NDI_Polaris: tool %s didn't init properly!\n",rigidBodyNDIRomFileNames[t]);
}
}
/////////////////////////////////////////////////////////
//STEP 5: GO TO TRACKING MODE
/////////////////////////////////////////////////////////
sendCommand("TSTART 80"); //80 resets the frame counter to zero
readResponse();
#ifdef DEBUG
printf("DEBUG: Tstart response: >%s<\n",latestResponseStr);
#endif
}
}
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;
}
