示例#1
0
void vrpn_SerialPort::clear_rts() {
	requiresOpen();
	int ret = vrpn_clear_rts(_comm);
	if (ret == -1) {
		throw RTSFailure();
	}
}
示例#2
0
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
	}
	
}
示例#3
0
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(&timestamp, 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;

}