Beispiel #1
0
SpaceBall::SpaceBall(VRDevice::Factory* sFactory,VRDeviceManager* sDeviceManager,Misc::ConfigurationFile& configFile)
	:VRDevice(sFactory,sDeviceManager,configFile),
	 devicePort(configFile.retrieveString("./devicePort").c_str()),
	 linearGain(configFile.retrieveValue<double>("./linearGain",1.0)),
	 angularGain(configFile.retrieveValue<double>("./angularGain",1.0)),
	 currentPositionOrientation(configFile.retrieveValue<PositionOrientation>("./initialState"))
	{
	/* Set device configuration: */
	setNumTrackers(1,configFile);
	setNumButtons(12,configFile);
	
	/* Set device port parameters: */
	int deviceBaudRate=configFile.retrieveValue<int>("./deviceBaudRate",9600);
	devicePort.setSerialSettings(deviceBaudRate,8,Comm::SerialPort::PARITY_NONE,2,false);
	devicePort.setRawMode(1,0);
	
	/* Wait for the fourth carriage return to arrive (end of startup message): */
	#ifdef VERBOSE
	printf("SpaceBall: Reading initialization message\n");
	fflush(stdout);
	#endif
	int numCrs=0;
	while(numCrs<4)
		if(devicePort.readByte()=='\r')
			++numCrs;
	}
RemoteDevice::RemoteDevice(VRDevice::Factory* sFactory,VRDeviceManager* sDeviceManager,Misc::ConfigurationFile& configFile)
	:VRDevice(sFactory,sDeviceManager,configFile),
	 pipe(configFile.retrieveString("./serverName").c_str(),configFile.retrieveValue<int>("./serverPort"))
	{
	/* Initiate connection: */
	#ifdef VERBOSE
	printf("RemoteDevice: Connecting to device server\n");
	fflush(stdout);
	#endif
	pipe.writeMessage(Vrui::VRDevicePipe::CONNECT_REQUEST);
	
	/* Wait for server's reply: */
	if(!pipe.waitForData(Misc::Time(10,0))) // Throw exception if reply does not arrive in time
		Misc::throwStdErr("RemoteDevice: Timeout while waiting for CONNECT_REPLY");
	if(pipe.readMessage()!=Vrui::VRDevicePipe::CONNECT_REPLY)
		Misc::throwStdErr("RemoteDevice: Mismatching message while waiting for CONNECT_REPLY");
	
	/* Read server's layout and initialize current state: */
	state.readLayout(pipe);
	#ifdef VERBOSE
	printf("RemoteDevice: Serving %d trackers, %d buttons, %d valuators\n",state.getNumTrackers(),state.getNumButtons(),state.getNumValuators());
	fflush(stdout);
	#endif
	setNumTrackers(state.getNumTrackers(),configFile);
	setNumButtons(state.getNumButtons(),configFile);
	setNumValuators(state.getNumValuators(),configFile);
	}
FakespacePinchGlove::FakespacePinchGlove(VRDevice::Factory* sFactory,VRDeviceManager* sDeviceManager,Misc::ConfigurationFile& configFile)
	:VRDevice(sFactory,sDeviceManager,configFile),
	 devicePort(configFile.retrieveString("./devicePort").c_str()),
	 reportEvents(false)
	{
	/* Set device configuration: */
	setNumButtons(2*4,configFile); // Four buttons for each hand
	
	/* Set device port parameters: */
	int deviceBaudRate=configFile.retrieveValue<int>("./deviceBaudRate");
	devicePort.setSerialSettings(deviceBaudRate,8,Comm::SerialPort::PARITY_NONE,1,false);
	devicePort.setRawMode(1,0);
	
	/* Synchronize the device's command stream: */
	#ifdef VERBOSE
	printf("FakespacePinchGlove: Synchronizing command stream\n");
	fflush(stdout);
	#endif
	devicePort.writeByte('*'); // Write a byte to complete eventually incomplete command sequences
	if(!devicePort.waitForByte(Misc::Time(0.5)))
		devicePort.writeByte('*'); // We screwed things up; write another byte to complete the first completion byte
	if(devicePort.waitForByte(Misc::Time(0.5)))
		ignoreReply(); // Ignore the device's reply
	else
		Misc::throwStdErr("FakespacePinchGlove: Pinch glove device not responding");
	
	/* Set device parameters: */
	char replyBuffer[256];
	
	/* Turn off timestamps: */
	#ifdef VERBOSE
	printf("FakespacePinchGlove: Turning off timestamps\n");
	fflush(stdout);
	#endif
	writeCommand("T0");
	readReply(256,replyBuffer);
	if(replyBuffer[0]!='0')
		Misc::throwStdErr("FakespacePinchGlove: Unable to disable time stamps on pinch glove device");
	
	/* Set inferface to version 1: */
	#ifdef VERBOSE
	printf("FakespacePinchGlove: Setting interface version\n");
	fflush(stdout);
	#endif
	writeCommand("V1");
	readReply(256,replyBuffer);
	if(replyBuffer[0]!='1')
		Misc::throwStdErr("FakespacePinchGlove: Unable to set interface version on pinch glove device");
	
	/* Initialize pinch masks: */
	for(int i=0;i<2;++i)
		pinchMasks[i]=0x0;
	
	/* Start device thread (pinch glove device cannot be disabled): */
	startDeviceThread();
	}
Beispiel #4
0
Joystick::Joystick(VRDevice::Factory* sFactory,VRDeviceManager* sDeviceManager,Misc::ConfigurationFile& configFile)
    :VRDevice(sFactory,sDeviceManager,configFile),
     joystickDeviceFd(open(configFile.retrieveString("./joystickDeviceFile").c_str(),O_RDONLY)),
     axisGains(0),
     reportEvents(false),
     buttonStates(0),valuatorStates(0)
{
    /* Check if the joystick device port was properly opened: */
    if(joystickDeviceFd<0)
        Misc::throwStdErr("Joystick: Unable to open joystick device port");

    /* Query the joystick's geometry: */
    char cNumButtons,cNumAxes;
    ioctl(joystickDeviceFd,JSIOCGBUTTONS,&cNumButtons);
    ioctl(joystickDeviceFd,JSIOCGAXES,&cNumAxes);
#ifdef VERBOSE
    /* Query the joystick's name: */
    char joystickName[256];
    if(ioctl(joystickDeviceFd,JSIOCGNAME(sizeof(joystickName)),joystickName)>=0)
    {
        joystickName[sizeof(joystickName)-1]='\0';
        printf("Joystick: %s with %d buttons and %d axes found\n",joystickName,int(cNumButtons),int(cNumAxes));
    }
    else
        printf("Joystick: Unknown joystick with %d buttons and %d axes found\n",int(cNumButtons),int(cNumAxes));
#endif

    /* Set device configuration: */
    setNumTrackers(0,configFile);
    setNumButtons(cNumButtons,configFile);
    setNumValuators(cNumAxes,configFile);

    /* Initialize gain arrays: */
    axisGains=new float[getNumValuators()];
    for(int i=0; i<getNumValuators(); ++i)
    {
        char axisGainTag[40];
        snprintf(axisGainTag,sizeof(axisGainTag),"./axisGain%d",i);
        axisGains[i]=configFile.retrieveValue<float>(axisGainTag,1.0f);
    }

    /* Initialize state arrays: */
    buttonStates=new bool[getNumButtons()];
    for(int i=0; i<getNumButtons(); ++i)
        buttonStates[i]=false;
    valuatorStates=new float[getNumValuators()];
    for(int i=0; i<getNumValuators(); ++i)
        valuatorStates[i]=0.0f;

    /* Start device thread (joystick device cannot be disabled): */
    startDeviceThread();
}
Beispiel #5
0
MouseButtons::MouseButtons(VRDevice::Factory* sFactory,VRDeviceManager* sDeviceManager,Misc::ConfigurationFile& configFile)
	:VRDevice(sFactory,sDeviceManager,configFile),
	 mouseDeviceFile(configFile.retrieveString("./mouseDeviceFile").c_str(),"rb",Misc::File::DontCare),
	 reportEvents(false)
	{
	/* Set device's layout: */
	setNumButtons(3,configFile);
	
	/* Initialize button states: */
	for(int i=0;i<3;++i)
		buttonStates[i]=false;
	
	/* Start device thread (mouse button device cannot be disabled): */
	startDeviceThread();
	}
Beispiel #6
0
SpaceBallRaw::SpaceBallRaw(VRDevice::Factory* sFactory,VRDeviceManager* sDeviceManager,Misc::ConfigurationFile& configFile)
	:VRDevice(sFactory,sDeviceManager,configFile),
	 devicePort(configFile.retrieveString("./devicePort").c_str())
	{
	/* Set device configuration: */
	setNumTrackers(0,configFile);
	setNumButtons(12,configFile);
	setNumValuators(6,configFile);
	
	/* Read axis manipulation factors: */
	double axisGain=configFile.retrieveValue<double>("./axisGain",1.0);
	double linearAxisGain=configFile.retrieveValue<double>("./linearAxisGain",axisGain);
	double angularAxisGain=configFile.retrieveValue<double>("./angularAxisGain",axisGain);
	for(int i=0;i<6;++i)
		{
		char axisGainTag[40];
		snprintf(axisGainTag,sizeof(axisGainTag),"./axisGain%d",i);
		axisGains[i]=configFile.retrieveValue<double>(axisGainTag,i<3?linearAxisGain:angularAxisGain);
		}
	
	/* Set device port parameters: */
	int deviceBaudRate=configFile.retrieveValue<int>("./deviceBaudRate",9600);
	devicePort.setSerialSettings(deviceBaudRate,8,Comm::SerialPort::PARITY_NONE,2,false);
	devicePort.setRawMode(1,0);
	
	/* Wait for status message from device: */
	#ifdef VERBOSE
	printf("SpaceBallRaw: Reading initialization message\n");
	fflush(stdout);
	#endif
	char lineBuffer[256];
	const int numResponses=4;
	char* responseTexts[numResponses]={"\021","@1 Spaceball alive and well","","@2 Firmware version"};
	int responseLengths[numResponses]={2,27,1,19};
	for(int i=0;i<numResponses;++i)
		{
		/* Try reading a line from the device port: */
		if(!readLine(256,lineBuffer,10.0))
			Misc::throwStdErr("SpaceBallRaw: Timeout while reading status message");
		
		/* Check if line is correct SpaceBall response: */
		if(strncmp(lineBuffer,responseTexts[i],responseLengths[i])!=0)
			Misc::throwStdErr("SpaceBallRaw: Incorrect response while reading status message");
		}
	}
Beispiel #7
0
PCWand::PCWand(VRDevice::Factory* sFactory,VRDeviceManager* sDeviceManager,Misc::ConfigurationFile& configFile)
    :VRDevice(sFactory,sDeviceManager,configFile),
     devicePort(configFile.retrieveString("./devicePort").c_str()),
     reportEvents(false)
{
    /* Set device configuration: */
    setNumButtons(3,configFile);
    setNumValuators(2,configFile);

    /* Set device port parameters: */
    devicePort.ref();
    int deviceBaudRate=configFile.retrieveValue<int>("./deviceBaudRate");
    devicePort.setSerialSettings(deviceBaudRate,8,Comm::SerialPort::NoParity,1,false);
    devicePort.setRawMode(1,0);

    /* Initialize device values: */
    for(int i=0; i<numButtons; ++i)
        deviceButtonStates[i]=false;
    for(int i=0; i<numValuators; ++i)
        deviceValuatorStates[i]=0.0f;

    /* Start device thread (dedicated PC cannot be disabled): */
    startDeviceThread();
}
Beispiel #8
0
InterSense::InterSense(VRDevice::Factory* sFactory,VRDeviceManager* sDeviceManager,Misc::ConfigurationFile& configFile)
	:VRDevice(sFactory,sDeviceManager,configFile),
	 serialPort(configFile.retrieveString("./serialPort").c_str()),
	 stations(0),
	 timers(0),notFirstMeasurements(0),oldPositionOrientations(0)
	{
	/* Set device port parameters: */
	int deviceBaudRate=configFile.retrieveValue<int>("./deviceBaudRate");
	serialPort.setSerialSettings(deviceBaudRate,8,Comm::SerialPort::PARITY_NONE,1,false);
	serialPort.setRawMode(1,0);
	
	if(configFile.retrieveValue<bool>("./resetDevice",false))
		{
		/* Reset device: */
		#ifdef VERBOSE
		printf("InterSense: Resetting device\n");
		fflush(stdout);
		#endif
		serialPort.writeByte('\31');
		delay(15.0);
		}
	else
		{
		/* Stop continuous mode (in case it's still active): */
		#ifdef VERBOSE
		printf("InterSense: Disabling continuous mode\n");
		fflush(stdout);
		#endif
		serialPort.writeByte('c');
		}
	
	/* Request status record to check if device is okey-dokey: */
	#ifdef VERBOSE
	printf("InterSense: Requesting status record\n");
	fflush(stdout);
	#endif
	serialPort.writeByte('S');
	if(!readStatusReply())
		{
		/* Try resetting the device, seeing if that helps: */
		#ifdef VERBOSE
		printf("InterSense: Resetting device\n");
		fflush(stdout);
		#endif
		serialPort.writeByte('\31');
		delay(15.0);
		
		/* Request another status record: */
		#ifdef VERBOSE
		printf("InterSense: Re-requesting status record\n");
		fflush(stdout);
		#endif
		serialPort.writeByte('S');
		if(!readStatusReply())
			Misc::throwStdErr("InterSense: Device not responding");
		}
	
	/* Get the array of station enable flags: */
	#ifdef VERBOSE
	printf("InterSense: Detecting enabled stations\n");
	fflush(stdout);
	#endif
	serialPort.writeString("l*\r\n");
	delay(0.1);
	char buffer[256];
	readLine(sizeof(buffer),buffer,Misc::Time(1.0));
	if(strncmp(buffer,"21l",3)!=0)
		Misc::throwStdErr("InterSense: Unable to detect enabled stations");
	
	/* Disable all stations: */
	for(int i=0;i<32;++i)
		{
		stationIdToIndex[i]=-1;
		if(buffer[i+3]=='1')
			{
			char command[20];
			snprintf(command,sizeof(command),"l%d,0\r\n",i+1);
			serialPort.writeString(command);
			delay(0.1);
			}
		}
	
	/* Probe the constellation configuration if asked: */
	if(configFile.retrieveValue<bool>("./probeConstellation",false))
		{
		/* Check if the device has a valid constellation configuration: */
		#ifdef VERBOSE
		printf("InterSense: Probing constellation configuration\n");
		fflush(stdout);
		#endif
		serialPort.writeString("MCF\r\n");
		delay(0.1);
		int numTransmitters=0;
		while(true)
			{
			/* Read the next transmitter line: */
			readLine(sizeof(buffer),buffer,Misc::Time(1.0));
			
			/* Check if it's a valid transmitter line: */
			int transmitterNum,transmitterId;
			double posX,posY,posZ,dirX,dirY,dirZ;
			if(sscanf(buffer,"31F %d %lf %lf %lf %lf %lf %lf %d",&transmitterNum,&posX,&posY,&posZ,&dirX,&dirY,&dirZ,&transmitterId)!=8)
				break;
			
			/* Increase the number of transmitters: */
			++numTransmitters;
			}
		#ifdef VERBOSE
		printf("InterSense: Detected %d configured transmitters\n",numTransmitters);
		fflush(stdout);
		#endif
		}
	
	/* Upload a constellation configuration if asked: */
	std::string constellationName=configFile.retrieveString("./uploadConstellationConfiguration","");
	if(constellationName!="")
		{
		#ifdef VERBOSE
		printf("InterSense: Uploading constellation configuration %s\n",constellationName.c_str());
		fflush(stdout);
		#endif
		
		/* Go to constellation configuration's section: */
		configFile.setCurrentSection(constellationName.c_str());
		
		Transmitter* transmitters=0;
		
		try
			{
			/* Get the number of configured transmitters and the base number for internal IDs: */
			int numTransmitters=configFile.retrieveValue<int>("./numTransmitters");
			int transmitterIdBase=configFile.retrieveValue<int>("./transmitterIdBase",5001);
			
			/* Get the conversion factor from configuration units to meters: */
			float unitSize=configFile.retrieveValue<float>("./unitSize",1.0f);
			
			/* Read all transmitter configurations to check for validity first: */
			transmitters=new Transmitter[numTransmitters];
			for(int i=0;i<numTransmitters;++i)
				{
				/* Read the transmitter's configuration: */
				char transmitterName[20];
				snprintf(transmitterName,sizeof(transmitterName),"./MCF%d",i+1);
				transmitters[i]=configFile.retrieveValue<Transmitter>(transmitterName);
				
				/* Convert the transmitter's position to meters: */
				for(int j=0;j<3;++j)
					transmitters[i].pos[j]*=unitSize;
				
				/* Normalize the transmitter's direction: */
				transmitters[i].dir.normalize();
				}
			
			/* Upload constellation configuration to device: */
			serialPort.writeString("MCC\r\n");
			delay(0.1);
			for(int i=0;i<numTransmitters;++i)
				{
				const Transmitter& t=transmitters[i];
				char transmitterLine[256];
				snprintf(transmitterLine,sizeof(transmitterLine),
				         "MCF%d, %8.4f, %8.4f, %8.4f, %6.3f, %6.3f, %6.3f, %d\r\n",
				         i+1,t.pos[0],t.pos[1],t.pos[2],t.dir[0],t.dir[1],t.dir[2],i+transmitterIdBase);
				serialPort.writeString(transmitterLine);
				delay(0.1);
				}
			serialPort.writeString("MCe\r\n");
			delay(0.1);
			}
		catch(std::runtime_error err)
			{
			printf("InterSense: Ignoring constellation configuration %s due to exception %s\n",constellationName.c_str(),err.what());
			}
		
		/* Clean up: */
		delete[] transmitters;
		
		/* Go back to device's section: */
		configFile.setCurrentSection("..");
		}
	
	/* Retrieve list of station names: */
	typedef std::vector<std::string> StringList;
	StringList stationNames=configFile.retrieveValue<StringList>("./stationNames");
	setNumTrackers(stationNames.size(),configFile);
	stations=new Station[numTrackers];
	int totalNumButtons=0;
	int totalNumValuators=0;
	
	/* Initialize all tracked stations: */
	#ifdef VERBOSE
	printf("InterSense: Initializing tracked stations\n");
	fflush(stdout);
	#endif
	for(int i=0;i<numTrackers;++i)
		{
		char command[80];
		
		/* Go to station's section: */
		configFile.setCurrentSection(stationNames[i].c_str());
		
		/* Read station's configuration: */
		stations[i].id=configFile.retrieveValue<int>("./id",i+1);
		stationIdToIndex[stations[i].id]=i;
		stations[i].numButtons=configFile.retrieveValue<int>("./numButtons",0);
		stations[i].firstButtonIndex=totalNumButtons;
		totalNumButtons+=stations[i].numButtons;
		stations[i].joystick=configFile.retrieveValue<bool>("./joystick",false);
		stations[i].firstValuatorIndex=totalNumValuators;
		if(stations[i].joystick)
			totalNumValuators+=2;
		
		/* Enable station: */
		snprintf(command,sizeof(command),"l%d,1\r\n",stations[i].id);
		serialPort.writeString(command);
		delay(0.1);
		
		/* Reset station's alignment frame: */
		snprintf(command,sizeof(command),"R%d\r\n",stations[i].id);
		serialPort.writeString(command);
		delay(0.1);
		
		/* Disable boresight mode: */
		snprintf(command,sizeof(command),"b%d\r\n",stations[i].id);
		serialPort.writeString(command);
		delay(0.1);
		
		/* Reset station's tip offset: */
		snprintf(command,sizeof(command),"N%d,%8.4f,%8.4f,%8.4f\r\n",stations[i].id,0.0f,0.0f,0.0f);
		serialPort.writeString(command);
		delay(0.1);
		
		/* Set station's output format: */
		snprintf(command,sizeof(command),"O%d,2,4,22,23,1\r\n",stations[i].id);
		serialPort.writeString(command);
		delay(0.1);
		
		/* Set stations' motion prediction: */
		int predictionTime=configFile.retrieveValue<int>("./predictionTime",0);
		snprintf(command,sizeof(command),"Mp%d,%d\r\n",stations[i].id,predictionTime);
		serialPort.writeString(command);
		delay(0.1);
		
		/* Set stations' perceptual enhancement level: */
		int perceptualEnhancement=configFile.retrieveValue<int>("./perceptualEnhancement",2);
		snprintf(command,sizeof(command),"MF%d,%d\r\n",stations[i].id,perceptualEnhancement);
		serialPort.writeString(command);
		delay(0.1);
		
		/* Set station's rotational sensitivity: */
		int rotationalSensitivity=configFile.retrieveValue<int>("./rotationalSensitivity",3);
		snprintf(command,sizeof(command),"MQ%d,%d\r\n",stations[i].id,rotationalSensitivity);
		serialPort.writeString(command);
		delay(0.1);
		
		/* Go back to device's section: */
		configFile.setCurrentSection("..");
		}
	
	/* Enable/disable sonistrip LEDs: */
	if(configFile.retrieveValue<bool>("./enableLEDs",true))
		{
		/* Enable LEDs: */
		#ifdef VERBOSE
		printf("InterSense: Enabling sonistrip LEDs\n");
		fflush(stdout);
		#endif
		serialPort.writeString("ML1\r\n");
		delay(0.1);
		}
	else
		{
		/* Enable LEDs: */
		#ifdef VERBOSE
		printf("InterSense: Disabling sonistrip LEDs\n");
		fflush(stdout);
		#endif
		serialPort.writeString("ML0\r\n");
		delay(0.1);
		}
	
	/* Set unit mode to inches: */
	#ifdef VERBOSE
	printf("InterSense: Setting unit mode\n");
	fflush(stdout);
	#endif
	serialPort.writeByte('U');
	delay(0.1);
	
	/* Enable binary mode: */
	#ifdef VERBOSE
	printf("InterSense: Enabling binary mode\n");
	fflush(stdout);
	#endif
	serialPort.writeByte('f');
	
	/* Set number of buttons and valuators: */
	setNumButtons(totalNumButtons,configFile);
	setNumValuators(totalNumValuators,configFile);
	
	/* Create free-running timers: */
	timers=new Misc::Timer[numTrackers];
	notFirstMeasurements=new bool[numTrackers];
	oldPositionOrientations=new PositionOrientation[numTrackers];
	}
Beispiel #9
0
VRPNClient::VRPNClient(VRDevice::Factory* sFactory,VRDeviceManager* sDeviceManager,Misc::ConfigurationFile& configFile)
	:VRDevice(sFactory,sDeviceManager,configFile),
	 VRPNConnection(configFile.retrieveString("./serverName").c_str(),configFile.retrieveValue<int>("./serverPort",3883)),
	 reportEvents(false),
	 trackerStates(0),trackerFlags(0),buttonStates(0),valuatorStates(0)
	{
	#ifdef VERBOSE
	printf("VRPNClient: Initializing senders...");
	fflush(stdout);
	#endif
	
	/* Check if the z axis if incoming position data needs to be flipped: */
	setFlipZAxis(configFile.retrieveValue<bool>("./flipZAxis",false));
	
	/* Retrieve list of sender names: */
	typedef std::vector<std::string> StringList;
	StringList senderNames=configFile.retrieveValue<StringList>("./senderNames");
	
	/* Process all senders: */
	int totalNumTrackers=0;
	int totalNumButtons=0;
	int totalNumValuators=0;
	for(StringList::const_iterator snIt=senderNames.begin();snIt!=senderNames.end();++snIt)
		{
		/* Go to the sender's section: */
		configFile.setCurrentSection(snIt->c_str());
		
		/* Read the number of trackers, buttons, and valuators for this sender: */
		int numTrackers=configFile.retrieveValue<int>("./numTrackers",0);
		if(numTrackers>0)
			{
			requestTrackers(snIt->c_str(),totalNumTrackers,numTrackers);
			totalNumTrackers+=numTrackers;
			}
		int numButtons=configFile.retrieveValue<int>("./numButtons",0);
		if(numButtons>0)
			{
			requestButtons(snIt->c_str(),totalNumButtons,numButtons);
			totalNumButtons+=numButtons;
			}
		int numValuators=configFile.retrieveValue<int>("./numValuators",0);
		if(numValuators>0)
			{
			requestValuators(snIt->c_str(),totalNumValuators,numValuators);
			totalNumValuators+=numValuators;
			}
		
		/* Go back to device's section: */
		configFile.setCurrentSection("..");
		}
	
	#ifdef VERBOSE
	printf(" done\n");
	fflush(stdout);
	#endif
	
	/* Set number of trackers, buttons, and valuators: */
	setNumTrackers(totalNumTrackers,configFile);
	setNumButtons(totalNumButtons,configFile);
	setNumValuators(totalNumValuators,configFile);
	
	/* Read the initial position/orientation for all trackers: */
	PositionOrientation defaultPosition=configFile.retrieveValue<PositionOrientation>("./defaultPosition",PositionOrientation::identity);
	
	/* Initialize the local state arrays: */
	trackerStates=new TrackerState[getNumTrackers()];
	trackerFlags=new int[getNumTrackers()];
	for(int i=0;i<getNumTrackers();++i)
		{
		trackerStates[i].positionOrientation=defaultPosition;
		trackerStates[i].linearVelocity=LinearVelocity::zero;
		trackerStates[i].angularVelocity=AngularVelocity::zero;
		trackerFlags[i]=0x0;
		}
	buttonStates=new ButtonState[getNumButtons()];
	for(int i=0;i<getNumButtons();++i)
		buttonStates[i]=false;
	valuatorStates=new ValuatorState[getNumValuators()];
	for(int i=0;i<getNumValuators();++i)
		valuatorStates[i]=ValuatorState(0);
	
	/* Start device communication thread: */
	startDeviceThread();
	}
Beispiel #10
0
HIDDevice::HIDDevice(VRDevice::Factory* sFactory,VRDeviceManager* sDeviceManager,Misc::ConfigurationFile& configFile)
	:VRDevice(sFactory,sDeviceManager,configFile),
	 deviceFd(-1),
	 keyMap(0),
	 absAxisMap(0),relAxisMap(0),axisConverters(0),
	 reportEvents(false),
	 buttonStates(0),valuatorStates(0)
	{
	/* First option: Open device by explicit event device file name: */
	if(deviceFd<0)
		{
		std::string deviceFileName=configFile.retrieveString("./deviceFileName","");
		if(deviceFileName!="")
			{
			#ifdef VERBOSE
			printf("HIDDevice: Opening device %s\n",deviceFileName.c_str());
			fflush(stdout);
			#endif
			deviceFd=open(deviceFileName.c_str(),O_RDONLY);
			if(deviceFd<0)
				Misc::throwStdErr("HIDDevice: Unable to open device file \"%s\"",deviceFileName.c_str());
			}
		}
	
	/* Second option: Open device by vendor ID / product ID: */
	if(deviceFd<0)
		{
		std::string deviceVendorProductId=configFile.retrieveString("./deviceVendorProductId","");
		if(deviceVendorProductId!="")
			{
			/* Split ID string into vendor ID / product ID: */
			char* colonPtr;
			int vendorId=strtol(deviceVendorProductId.c_str(),&colonPtr,16);
			char* endPtr;
			int productId=strtol(colonPtr+1,&endPtr,16);
			if(*colonPtr!=':'||*endPtr!='\0'||vendorId<0||productId<0)
				Misc::throwStdErr("HIDDevice: Malformed vendorId:productId string \"%s\"",deviceVendorProductId.c_str());
			#ifdef VERBOSE
			printf("HIDDevice: Searching device %04x:%04x\n",vendorId,productId);
			fflush(stdout);
			#endif
			deviceFd=findDevice(vendorId,productId);
			if(deviceFd<0)
				Misc::throwStdErr("HIDDevice: No device with vendorId:productId %04x:%04x found",vendorId,productId);
			}
		}
	
	/* Third option: Open device by device name: */
	if(deviceFd<0)
		{
		std::string deviceName=configFile.retrieveString("./deviceName","");
		if(deviceName!="")
			{
			#ifdef VERBOSE
			printf("HIDDevice: Searching device %s\n",deviceName.c_str());
			fflush(stdout);
			#endif
			deviceFd=findDevice(deviceName.c_str());
			if(deviceFd<0)
				Misc::throwStdErr("HIDDevice: No device with name \"%s\" found",deviceName.c_str());
			}
		}
	
	/* Bail out if no device was found: */
	if(deviceFd<0)
		Misc::throwStdErr("HIDDevice: No device specified");
	
	/* Set number of trackers on device: */
	setNumTrackers(0,configFile);
	
	/* Query all feature types of the device: */
	unsigned char featureTypeBits[EV_MAX/8+1];
	memset(featureTypeBits,0,EV_MAX/8+1);
	if(ioctl(deviceFd,EVIOCGBIT(0,sizeof(featureTypeBits)),featureTypeBits)<0)
		Misc::throwStdErr("HIDDevice: Unable to query device feature types");
	
	/* Query the number of keys on the device: */
	if(featureTypeBits[EV_KEY/8]&(1<<(EV_KEY%8)))
		{
		#ifdef VERBOSE
		printf("HIDDevice: Initializing buttons...\n");
		fflush(stdout);
		#endif
		
		/* Query key features: */
		unsigned char keyBits[KEY_MAX/8+1];
		memset(keyBits,0,KEY_MAX/8+1);
		if(ioctl(deviceFd,EVIOCGBIT(EV_KEY,sizeof(keyBits)),keyBits)<0)
			Misc::throwStdErr("HIDDevice: Unable to query device key features");
		
		/* Initialize the key translation array: */
		keyMap=new int[KEY_MAX+1];
		int numKeys=0;
		for(int i=0;i<=KEY_MAX;++i)
			{
			if(keyBits[i/8]&(1<<(i%8)))
				{
				keyMap[i]=numKeys;
				++numKeys;
				}
			else
				keyMap[i]=-1;
			}
		
		/* Set number of buttons on device: */
		#ifdef VERBOSE
		printf("HIDDevice: %d buttons found\n",numKeys);
		fflush(stdout);
		#endif
		setNumButtons(numKeys,configFile);
		}
	else
		setNumButtons(0,configFile);
	
	/* Count the number of absolute and relative axes: */
	int numAxes=0;
	
	/* Query the number of absolute axes on the device: */
	if(featureTypeBits[EV_ABS/8]&(1<<(EV_ABS%8)))
		{
		#ifdef VERBOSE
		printf("HIDDevice: Initializing absolute axes...\n");
		fflush(stdout);
		#endif
		
		/* Query absolute axis features: */
		unsigned char absAxisBits[ABS_MAX/8+1];
		memset(absAxisBits,0,ABS_MAX/8+1);
		if(ioctl(deviceFd,EVIOCGBIT(EV_ABS,sizeof(absAxisBits)),absAxisBits)<0)
			Misc::throwStdErr("HIDDevice: Unable to query device absolute axis features");
		
		/* Initialize the axis translation array: */
		absAxisMap=new int[ABS_MAX+1];
		int numAbsAxes=0;
		for(int i=0;i<=ABS_MAX;++i)
			{
			if(absAxisBits[i/8]&(1<<(i%8)))
				{
				absAxisMap[i]=numAxes;
				++numAxes;
				++numAbsAxes;
				}
			else
				absAxisMap[i]=-1;
			}
		
		#ifdef VERBOSE
		printf("HIDDevice: %d absolute axes found\n",numAbsAxes);
		fflush(stdout);
		#endif
		}
	
	/* Query the number of relative axes on the device: */
	if(featureTypeBits[EV_REL/8]&(1<<(EV_REL%8)))
		{
		#ifdef VERBOSE
		printf("HIDDevice: Initializing relative axes...\n");
		fflush(stdout);
		#endif
		
		/* Query relative axis features: */
		unsigned char relAxisBits[REL_MAX/8+1];
		memset(relAxisBits,0,REL_MAX/8+1);
		if(ioctl(deviceFd,EVIOCGBIT(EV_REL,sizeof(relAxisBits)),relAxisBits)<0)
			Misc::throwStdErr("HIDDevice: Unable to query device relative axis features");
		
		/* Initialize the axis translation array: */
		relAxisMap=new int[REL_MAX+1];
		int numRelAxes=0;
		for(int i=0;i<=REL_MAX;++i)
			{
			if(relAxisBits[i/8]&(1<<(i%8)))
				{
				relAxisMap[i]=numAxes;
				++numAxes;
				++numRelAxes;
				}
			else
				relAxisMap[i]=-1;
			}
		
		#ifdef VERBOSE
		printf("HIDDevice: %d relative axes found\n",numRelAxes);
		fflush(stdout);
		#endif
		}
	
	/* Set number of valuators on device: */
	setNumValuators(numAxes,configFile);
		
	/* Initialize axis converters: */
	axisConverters=new AxisConverter[numAxes];
	
	if(absAxisMap!=0)
		{
		/* Initialize absolute axis converters: */
		#ifdef VERBOSE
		printf("HIDDevice: Initializing absolute axis converters\n");
		fflush(stdout);
		#endif
		for(int i=0;i<=ABS_MAX;++i)
			if(absAxisMap[i]>=0)
				{
				/* Query configuration of this axis: */
				input_absinfo absAxisConf;
				if(ioctl(deviceFd,EVIOCGABS(i),&absAxisConf)<0)
					Misc::throwStdErr("HIDDevice: Unable to query device absolute axis configuration");
				
				#ifdef VERBOSE
				printf("Axis %2d: min %d, max %d, fuzz %d, flat %d\n",absAxisMap[i],absAxisConf.minimum,absAxisConf.maximum,absAxisConf.fuzz,absAxisConf.flat);
				fflush(stdout);
				#endif
				
				/* Initialize converter with queried values: */
				AxisConverter& converter=axisConverters[absAxisMap[i]];
				float mid=Math::mid(absAxisConf.minimum,absAxisConf.maximum);
				converter=AxisConverter(absAxisConf.minimum,mid-absAxisConf.flat,mid+absAxisConf.flat,absAxisConf.maximum);
				
				/* Override axis settings from configuration file: */
				char axisSettingsTag[20];
				snprintf(axisSettingsTag,sizeof(axisSettingsTag),"axis%dSettings",absAxisMap[i]);
				converter=configFile.retrieveValue<AxisConverter>(axisSettingsTag,converter);
				
				#ifdef VERBOSE
				printf("Axis %2d: %s\n",absAxisMap[i],Misc::ValueCoder<AxisConverter>::encode(converter).c_str());
				fflush(stdout);
				#endif
				}
		}
	
	if(relAxisMap!=0)
		{
		/* Initialize relative axis converters: */
		#ifdef VERBOSE
		printf("HIDDevice: Initializing relative axis converters\n");
		fflush(stdout);
		#endif
		for(int i=0;i<=REL_MAX;++i)
			if(relAxisMap[i]>=0)
				{
				/* Initialize converter with default values: */
				AxisConverter& converter=axisConverters[relAxisMap[i]];
				converter=AxisConverter(-1.0f,1.0f);
				
				/* Override axis settings from configuration file: */
				char axisSettingsTag[20];
				snprintf(axisSettingsTag,sizeof(axisSettingsTag),"axis%dSettings",relAxisMap[i]);
				converter=configFile.retrieveValue<AxisConverter>(axisSettingsTag,converter);
				
				#ifdef VERBOSE
				printf("Axis %2d: %s\n",relAxisMap[i],Misc::ValueCoder<AxisConverter>::encode(converter).c_str());
				fflush(stdout);
				#endif
				}
		}
	
	#if 0
	/* Initialize gain arrays: */
	valuatorGains=new float[getNumValuators()];
	for(int i=0;i<getNumValuators();++i)
		{
		char valuatorGainTag[40];
		snprintf(valuatorGainTag,sizeof(valuatorGainTag),"./valuatorGain%d",i);
		valuatorGains[i]=configFile.retrieveValue<float>(valuatorGainTag,1.0f);
		}
	#endif
	
	/* Initialize state arrays: */
	buttonStates=new bool[getNumButtons()];
	for(int i=0;i<getNumButtons();++i)
		buttonStates[i]=false;
	valuatorStates=new float[getNumValuators()];
	for(int i=0;i<getNumValuators();++i)
		valuatorStates[i]=0.0f;
	
	/* Start device thread (HID device cannot be disabled): */
	startDeviceThread();
	}
AscensionFlockOfBirds::AscensionFlockOfBirds(VRDevice::Factory* sFactory,VRDeviceManager* sDeviceManager,Misc::ConfigurationFile& configFile)
	:VRDevice(sFactory,sDeviceManager,configFile),
	 devicePort(configFile.retrieveString("./devicePort").c_str()),
	 masterId(configFile.retrieveValue<int>("./masterId",1)),
	 firstBirdId(configFile.retrieveValue<int>("./firstBirdId",1)),
	 ercId(configFile.retrieveValue<int>("./ercId",-1)),
	 ercTransmitterIndex(configFile.retrieveValue<int>("./ercTransmitterIndex",-1)),
	 trackerRange(configFile.retrieveValue<double>("./trackerRange",36.0)),
	 timers(0),notFirstMeasurements(0),oldPositionOrientations(0)
	{
	/* Query number of connected birds (includes ERC if present): */
	int numBirds=configFile.retrieveValue<int>("./numBirds");
	
	/* Set device configuration: */
	setNumTrackers(ercId==-1?numBirds:numBirds-1,configFile);
	
	/* Create free-running timers: */
	timers=new Misc::Timer[numTrackers];
	notFirstMeasurements=new bool[numTrackers];
	oldPositionOrientations=new PositionOrientation[numTrackers];
	
	/* Set device port parameters: */
	int deviceBaudRate=configFile.retrieveValue<int>("./deviceBaudRate");
	devicePort.setSerialSettings(deviceBaudRate,8,Comm::SerialPort::PARITY_NONE,1,false);
	devicePort.setRawMode(1,0);
	
	/* Check status of all birds: */
	bool allBirdsOk=true;
	for(int i=0;i<numBirds;++i)
		// if(i+firstBirdId!=ercId)
			{
			#ifdef VERBOSE
			printf("AscensionFlockOfBirds: Querying status of bird %d... ",i+firstBirdId);
			fflush(stdout);
			#endif
			devicePort.writeByte(0xf0|char(i+firstBirdId)); // Talk to bird i
			devicePort.writeByte(0x4f); // Examine value
			devicePort.writeByte(0x00); // Bird status
			if(devicePort.waitForByte(Misc::Time(2.0)))
				{
				/* Read the bird's answer: */
				int reply=readShort();
				#ifdef VERBOSE
				printf("ok (%.4x)\n",reply&0xffff);
				fflush(stdout);
				#endif
				}
			else
				{
				#ifdef VERBOSE
				printf("failed\n");
				fflush(stdout);
				#endif
				allBirdsOk=false;
				}
			}
	
	/* Stop if not all birds responded: */
	if(!allBirdsOk)
		Misc::throwStdErr("AscensionFlockOfBirds: Unable to communicate to all birds");
	
	#if 0
	/* Reset flock: */
	#ifdef VERBOSE
	printf("AscensionFlockOfBirds: Resetting flock\n");
	fflush(stdout);
	#endif
	devicePort.writeByte(0xf0|masterId); // Talk to FOB master
	devicePort.writeByte(0x2f); // FBB Reset
	delay(0.25);
	#endif
	
	/* Query flock configuration: */
	#ifdef VERBOSE
	printf("AscensionFlockOfBirds: Querying flock configuration\n");
	fflush(stdout);
	devicePort.writeByte(0xf0|masterId); // Talk to FOB master
	devicePort.writeByte(0x4f); // Change Value
	devicePort.writeByte(36); // Flock System Status
	delay(0.25);
	
	/* Receive flock state: */
	char flockStates[14];
	devicePort.readBytes(14,flockStates);
	
	/* Print status of all present birds: */
	for(int i=0;i<14;++i)
		if(flockStates[i]&0x80)
			{
			printf("AscensionFlockOfBirds: Status of bird %d: %.2x\n",i+1,int(flockStates[i])&0xff);
			fflush(stdout);
			}
	#endif
	
	/* Set FOB to report position/angles: */
	#ifdef VERBOSE
	printf("AscensionFlockOfBirds: Setting position/angles mode on all birds\n");
	fflush(stdout);
	#endif
	for(int i=0;i<numBirds;++i)
		if(i+firstBirdId!=ercId)
			{
			devicePort.writeByte(0xf0|char(i+firstBirdId)); // Talk to bird i
			devicePort.writeByte(0x59); // Position/Angles mode
			delay(0.25);
			}
	
	/* Set FOB's hemisphere: */
	char hemisphereBytes[6][2]={{0x00,0x00},{0x00,0x01},{0x0c,0x00},{0x0c,0x01},{0x06,0x00},{0x06,0x01}};
	std::string hemisphere=configFile.retrieveString("./trackerHemisphere","Forward");
	int hemisphereIndex=-1;
	if(hemisphere=="Forward"||hemisphere=="+X")
		hemisphereIndex=0;
	else if(hemisphere=="Aft"||hemisphere=="Rear"||hemisphere=="-X")
		hemisphereIndex=1;
	else if(hemisphere=="Lower"||hemisphere=="+Z")
		hemisphereIndex=2;
	else if(hemisphere=="Upper"||hemisphere=="-Z")
		hemisphereIndex=3;
	else if(hemisphere=="Right"||hemisphere=="+Y")
		hemisphereIndex=4;
	else if(hemisphere=="Left"||hemisphere=="-Y")
		hemisphereIndex=5;
	else
		Misc::throwStdErr("AscensionFlockOfBirds: Unrecognized hemisphere value %s",hemisphere.c_str());
	
	#ifdef VERBOSE
	printf("AscensionFlockOfBirds: Setting hemisphere on all birds\n");
	fflush(stdout);
	#endif
	for(int i=0;i<numBirds;++i)
		if(i+firstBirdId!=ercId)
			{
			devicePort.writeByte(0xf0|char(i+firstBirdId)); // Talk to bird i
			devicePort.writeByte(0x4c); // Hemisphere
			devicePort.writeByte(hemisphereBytes[hemisphereIndex][0]); // Code (pt 1)
			devicePort.writeByte(hemisphereBytes[hemisphereIndex][1]); // Code (pt 2)
			delay(0.25);
			}
	
	/* Set FOB's tracking range: */
	if(ercId!=-1)
		trackerRange=144.0;
	else if(trackerRange<Math::sqrt(36.0*72.0))
		trackerRange=36.0;
	else
		trackerRange=72.0;
	
	if(ercId==-1)
		{
		#ifdef VERBOSE
		printf("AscensionFlockOfBirds: Setting tracking range on all birds\n");
		fflush(stdout);
		#endif
		for(int i=0;i<numBirds;++i)
			{
			devicePort.writeByte(0xf0|char(i+firstBirdId)); // Talk to bird i
			devicePort.writeByte(0x50); // Change Value
			devicePort.writeByte(3); // Position Scaling
			if(trackerRange==36.0)
				{
				devicePort.writeByte(0x0); // 36 inch max range, pt. 1
				devicePort.writeByte(0x0); // 36 inch max range, pt. 2
				}
			else
				{
				devicePort.writeByte(0x1); // 72 inch max range, pt. 1
				devicePort.writeByte(0x0); // 72 inch max range, pt. 2
				}
			delay(0.25);
			}
		}
	
	/* Enable flock mode: */
	#ifdef VERBOSE
	printf("AscensionFlockOfBirds: Starting auto-configuration\n");
	fflush(stdout);
	#endif
	delay(0.35);
	devicePort.writeByte(0xf0|masterId); // Talk to FOB master
	devicePort.writeByte(0x50); // Change Value
	devicePort.writeByte(50); // FBB Auto-Configuration
	devicePort.writeByte(char(numBirds)); // numTrackers birds
	delay(0.6);
	
	#ifdef VERBOSE
	printf("AscensionFlockOfBirds: Enabling group mode\n");
	fflush(stdout);
	#endif
	devicePort.writeByte(0xf0|masterId); // Talk to FOB master
	devicePort.writeByte(0x50); // Change Value
	devicePort.writeByte(35); // Group Mode
	devicePort.writeByte(1); // Enable
	delay(0.25);
	
	if(ercId!=-1)
		{
		/* Activate ERC's transmitter: */
		#ifdef VERBOSE
		printf("AscensionFlockOfBirds: Enabling ERC transmitter\n");
		fflush(stdout);
		#endif
		devicePort.writeByte(0xf0|masterId); // Talk to FOB master
		devicePort.writeByte(0x30); // Next Transmitter
		devicePort.writeByte(((ercId&0xf)<<4)|ercTransmitterIndex); // ID of extended range controller and transmitter index
		delay(0.25);
		}
	
	/* Put birds to sleep: */
	#ifdef VERBOSE
	printf("AscensionFlockOfBirds: Disabling stream mode\n");
	fflush(stdout);
	#endif
	devicePort.writeByte(0xf0|masterId); // Talk to FOB master
	devicePort.writeByte(0x42); // Point
	delay(0.25);
	
	#ifdef VERBOSE
	printf("AscensionFlockOfBirds: Disabling tracker device\n");
	fflush(stdout);
	#endif
	devicePort.writeByte(0xf0|masterId); // Talk to FOB master
	devicePort.writeByte(0x47); // Sleep
	}
Beispiel #12
0
PolhemusFastrak::PolhemusFastrak(VRDevice::Factory* sFactory,VRDeviceManager* sDeviceManager,Misc::ConfigurationFile& configFile)
	:VRDevice(sFactory,sDeviceManager,configFile),
	 devicePort(configFile.retrieveString("./devicePort").c_str()),
	 stylusEnabled(configFile.retrieveValue<bool>("./stylusEnabled",true)),
	 timers(0),notFirstMeasurements(0),oldPositionOrientations(0)
	{
	/* Set device configuration: */
	setNumTrackers(configFile.retrieveValue<int>("./numReceivers",4),configFile);
	if(stylusEnabled)
		setNumButtons(1,configFile); // Assume that first receiver is a stylus
	
	/* Create free-running timers: */
	timers=new Misc::Timer[numTrackers];
	notFirstMeasurements=new bool[numTrackers];
	oldPositionOrientations=new PositionOrientation[numTrackers];
	
	/* Set device port parameters: */
	int deviceBaudRate=configFile.retrieveValue<int>("./deviceBaudRate");
	devicePort.setSerialSettings(deviceBaudRate,8,Comm::SerialPort::PARITY_NONE,1,false);
	devicePort.setRawMode(1,0);
	
	if(configFile.retrieveValue<bool>("./resetDevice",false))
		{
		/* Reset device: */
		#ifdef VERBOSE
		printf("PolhemusFastrak: Resetting device\n");
		fflush(stdout);
		#endif
		devicePort.writeByte('\31');
		delay(15.0);
		}
	else
		{
		/* Stop continuous mode (in case it's still active): */
		#ifdef VERBOSE
		printf("PolhemusFastrak: Disabling continuous mode\n");
		fflush(stdout);
		#endif
		devicePort.writeByte('c');
		}
	
	/* Request status record to check if device is okey-dokey: */
	#ifdef VERBOSE
	printf("PolhemusFastrak: Requesting status record\n");
	fflush(stdout);
	#endif
	devicePort.writeByte('S');
	if(!readStatusReply())
		{
		/* Try resetting the device, seeing if that helps: */
		#ifdef VERBOSE
		printf("PolhemusFastrak: Resetting device\n");
		fflush(stdout);
		#endif
		devicePort.writeByte('\31');
		delay(15.0);
		
		/* Request another status record: */
		#ifdef VERBOSE
		printf("PolhemusFastrak: Re-requesting status record\n");
		fflush(stdout);
		#endif
		devicePort.writeByte('S');
		if(!readStatusReply())
			Misc::throwStdErr("PolhemusFastrak: Device not responding");
		}
	
	/* Retrieve tracker hemisphere: */
	int hemisphereVectors[6][3]={{1,0,0},{-1,0,0},{0,0,1},{0,0,-1},{0,1,0},{0,-1,0}};
	std::string hemisphere=configFile.retrieveString("./trackerHemisphere","+X");
	int hemisphereIndex=-1;
	if(hemisphere=="+X")
		hemisphereIndex=0;
	else if(hemisphere=="-X")
		hemisphereIndex=1;
	else if(hemisphere=="+Z")
		hemisphereIndex=2;
	else if(hemisphere=="-Z")
		hemisphereIndex=3;
	else if(hemisphere=="+Y")
		hemisphereIndex=4;
	else if(hemisphere=="-Y")
		hemisphereIndex=5;
	else
		Misc::throwStdErr("PolhemusFastrak: Unrecognized hemisphere value %s",hemisphere.c_str());
	
	/* Initialize all receivers: */
	#ifdef VERBOSE
	printf("PolhemusFastrak: Initializing receivers\n");
	fflush(stdout);
	#endif
	for(int i=0;i<numTrackers;++i)
		{
		char command[40];
		
		/* Enable receiver: */
		snprintf(command,sizeof(command),"l%d,1\r\n",i+1);
		devicePort.writeString(command);
		delay(0.1);
		
		/* Reset receiver's alignment frame: */
		snprintf(command,sizeof(command),"R%d\r\n",i+1);
		devicePort.writeString(command);
		delay(0.1);
		
		/* Disable boresight mode: */
		snprintf(command,sizeof(command),"b%d\r\n",i+1);
		devicePort.writeString(command);
		delay(0.1);
		
		/* Set receiver's hemisphere of operation: */
		snprintf(command,sizeof(command),"H%d,%d,%d,%d\r\n",i+1,hemisphereVectors[hemisphereIndex][0],hemisphereVectors[hemisphereIndex][1],hemisphereVectors[hemisphereIndex][2]);
		devicePort.writeString(command);
		delay(0.1);
		
		/* Set receiver's output format: */
		snprintf(command,sizeof(command),"O%d,2,4,16,1\r\n",i+1);
		devicePort.writeString(command);
		delay(0.1);
		}
	
	/* Set stylus tip offset: */
	try
		{
		/* Try getting a tip offset from the configuration file: */
		Geometry::Vector<float,3> tipOffset=configFile.retrieveValue<Geometry::Vector<float,3> >("./stylusTipOffset");
		
		/* If the tag is there, set the stylus tip offset: */
		#ifdef VERBOSE
		printf("PolhemusFastrak: Setting stylus tip offset\n");
		fflush(stdout);
		#endif
		char command[80];
		snprintf(command,sizeof(command),"N1,%8.4f,%8.4f,%8.4f\r\n",tipOffset[0],tipOffset[1],tipOffset[2]);
		devicePort.writeString(command);
		delay(0.1);
		}
	catch(Misc::ConfigurationFile::TagNotFoundError error)
		{
		/* If no tag for stylus offset is given, just leave it to the factory default */
		}
	
	/* Set stylus button to "mouse mode": */
	#ifdef VERBOSE
	printf("PolhemusFastrak: Setting stylus button mode\n");
	fflush(stdout);
	#endif
	devicePort.writeString("e1,0\r\n");
	delay(0.1);
	
	#if 1
	/* Query stylus tip offset: */
	devicePort.writeByte('F');
	delay(0.1);
	devicePort.writeString("N1,\r\n");
	delay(0.1);
	char lineBuffer[80];
	printf("%s\n",readLine(80,lineBuffer));
	fflush(stdout);
	#endif
	
	/* Set fixed metal compensation: */
	#ifdef VERBOSE
	printf("PolhemusFastrak: Setting fixed metal compensation mode\n");
	fflush(stdout);
	#endif
	if(configFile.retrieveValue<bool>("./enableMetalCompensation",false))
		devicePort.writeByte('D');
	else
		devicePort.writeByte('d');
	delay(0.1);
	
	/* Set unit mode to inches: */
	#ifdef VERBOSE
	printf("PolhemusFastrak: Setting unit mode\n");
	fflush(stdout);
	#endif
	devicePort.writeByte('U');
	delay(0.1);
	
	/* Enable binary mode: */
	#ifdef VERBOSE
	printf("PolhemusFastrak: Enabling binary mode\n");
	fflush(stdout);
	#endif
	devicePort.writeByte('f');
	}
Beispiel #13
0
ViconTarsus::ViconTarsus(VRDevice::Factory* sFactory,VRDeviceManager* sDeviceManager,Misc::ConfigurationFile& configFile)
	:VRDevice(sFactory,sDeviceManager,configFile),
	 pipe(configFile.retrieveString("./serverName"),configFile.retrieveValue<int>("./serverPort",800),Comm::TCPPipe::LittleEndian),
	 trackerChannelIndices(0),
	 trackerSixDofs(0),
	 trackerMap(0),
	 channelPacketBuffer(0),
	 trackerStates(0)
	{
	/* Read the list of tracked bodies: */
	std::vector<std::string> trackedBodies=configFile.retrieveValue<std::vector<std::string> >("./trackedBodies");
	
	/* Set tracker's layout: */
	setNumTrackers(trackedBodies.size(),configFile);
	
	/* Initialize the tracker channel index array: */
	trackerChannelIndices=new int[getNumTrackers()*6];
	for(int i=0;i<getNumTrackers()*6;++i)
		trackerChannelIndices[i]=-1;
	
	/* Send info request to server: */
	#ifdef VERBOSE
	printf("ViconTarsus: Requesting info packet\n");
	#endif
	pipe.write<int>(1); // Info
	pipe.write<int>(0); // Request
	
	/* Wait for the server's reply: */
	if(pipe.read<int>()!=1)
		Misc::throwStdErr("ViconTarsus: Unable to connect to tracking server at %s",pipe.getPeerHostname().c_str());
	if(pipe.read<int>()!=1)
		Misc::throwStdErr("ViconTarsus: Unable to connect to tracking server at %s",pipe.getPeerHostname().c_str());
	
	/* Read the info packet's data: */
	numChannels=pipe.read<int>();
	#ifdef VERBOSE
	printf("ViconTarsus: Server reports %d channels\n",numChannels);
	#endif
	for(int channelIndex=0;channelIndex<numChannels;++channelIndex)
		{
		/* Read the channel descriptor (fortunately, Vicon's string protocol is compatible to TCPPipe's): */
		std::string channelName=pipe.read<std::string>();
		#ifdef VERBOSE
		printf("ViconTarsus: Server channel %2d: %s\n",channelIndex,channelName.c_str());
		#endif
		
		/* Parse the channel name: */
		const char* nameStart=channelName.c_str();
		const char* nameEnd;
		for(nameEnd=nameStart;*nameEnd!='\0'&&*nameEnd!=' '&&*nameEnd!='-'&&*nameEnd!='<'&&*nameEnd!='>';++nameEnd)
			;
		const char* codeStart;
		for(codeStart=nameEnd;*codeStart!='\0'&&*codeStart!='<';++codeStart)
			;
		const char* codeEnd;
		for(codeEnd=codeStart;*codeEnd!='\0'&&*codeEnd!='>';++codeEnd)
			;
		
		/* Check if the channel name is well-formed: */
		if(*codeStart=='<'&&*codeEnd=='>')
			{
			std::string code(codeStart+1,codeEnd);
			
			/* Check if the channel encodes a body parameter or a marker parameter: */
			if(code[0]=='P'||code[0]=='A'||code[0]=='T')
				{
				/* Check if the name corresponds to a tracked body: */
				std::string name(nameStart,nameEnd);
				int bodyIndex;
				for(bodyIndex=0;bodyIndex<getNumTrackers();++bodyIndex)
					if(trackedBodies[bodyIndex]==name)
						break;
				if(bodyIndex<getNumTrackers())
					{
					/* Parse the channel code: */
					if(code=="P-X"||code=="T-X")
						trackerChannelIndices[bodyIndex*6+0]=channelIndex;
					else if(code=="P-Y"||code=="T-Y")
						trackerChannelIndices[bodyIndex*6+1]=channelIndex;
					else if(code=="P-Z"||code=="T-Z")
						trackerChannelIndices[bodyIndex*6+2]=channelIndex;
					else if(code=="A-X")
						trackerChannelIndices[bodyIndex*6+3]=channelIndex;
					else if(code=="A-Y")
						trackerChannelIndices[bodyIndex*6+4]=channelIndex;
					else if(code=="A-Z")
						trackerChannelIndices[bodyIndex*6+5]=channelIndex;
					}
				}
			}
		}
	
	/* Check if all requested trackers have channels associated with them: */
	trackerSixDofs=new bool[getNumTrackers()];
	int numSixDofTrackers=0;
	for(int bodyIndex=0;bodyIndex<getNumTrackers();++bodyIndex)
		{
		/* Check if the tracker has 3-DOF data: */
		bool valid=true;
		for(int i=0;i<3;++i)
			if(trackerChannelIndices[bodyIndex*6+i]==-1)
				valid=false;
		if(!valid)
			Misc::throwStdErr("ViconTarsus: Insufficient data to track body %s",trackedBodies[bodyIndex].c_str());
		
		/* Check if the tracker has 6-DOF data (for later): */
		bool sixDof=true;
		for(int i=3;i<6;++i)
			if(trackerChannelIndices[bodyIndex*6+i]==-1)
				sixDof=false;
		trackerSixDofs[bodyIndex]=sixDof;
		if(sixDof)
			++numSixDofTrackers;
		}
	#ifdef VERBOSE
	printf("ViconTarsus: Tracking %d 3-DOF tracker(s) and %d 6-DOF tracker(s)\n",getNumTrackers()-numSixDofTrackers,numSixDofTrackers);
	#endif
	
	/* Construct the tracker map: */
	trackerMap=new std::pair<int,int>[numChannels];
	for(int i=0;i<numChannels;++i)
		trackerMap[i]=std::pair<int,int>(-1,-1);
	for(int bodyIndex=0;bodyIndex<getNumTrackers();++bodyIndex)
		for(int i=0;i<6;++i)
			{
			if(trackerChannelIndices[bodyIndex*6+i]!=-1)
				trackerMap[trackerChannelIndices[bodyIndex*6+i]]=std::pair<int,int>(bodyIndex,i);
			}
	
	/* Initialize the channel packet buffer: */
	channelPacketBuffer=new double[numChannels];
	
	/* Initialize tracker states: */
	trackerStates=new Vrui::VRDeviceState::TrackerState[getNumTrackers()];
	}
VRDeviceManager::VRDeviceManager(Misc::ConfigurationFile& configFile)
	:deviceFactories(configFile.retrieveString("./deviceDirectory",VRDEVICEDAEMON_CONFIG_VRDEVICESDIR),this),
	 calibratorFactories(configFile.retrieveString("./calibratorDirectory",VRDEVICEDAEMON_CONFIG_VRCALIBRATORSDIR)),
	 numDevices(0),
	 devices(0),trackerIndexBases(0),buttonIndexBases(0),valuatorIndexBases(0),
	 fullTrackerReportMask(0x0),trackerReportMask(0x0),trackerUpdateNotificationEnabled(false),
	 trackerUpdateCompleteCond(0)
	{
	/* Allocate device and base index arrays: */
	typedef std::vector<std::string> StringList;
	StringList deviceNames=configFile.retrieveValue<StringList>("./deviceNames");
	numDevices=deviceNames.size();
	devices=new VRDevice*[numDevices];
	trackerIndexBases=new int[numDevices];
	buttonIndexBases=new int[numDevices];
	valuatorIndexBases=new int[numDevices];
	
	/* Initialize VR devices: */
	for(currentDeviceIndex=0;currentDeviceIndex<numDevices;++currentDeviceIndex)
		{
		/* Save the device's base indices: */
		trackerIndexBases[currentDeviceIndex]=int(trackerNames.size());
		buttonIndexBases[currentDeviceIndex]=int(buttonNames.size());
		valuatorIndexBases[currentDeviceIndex]=int(valuatorNames.size());
		
		/* Go to device's section: */
		configFile.setCurrentSection(deviceNames[currentDeviceIndex].c_str());
		
		/* Retrieve device type: */
		std::string deviceType=configFile.retrieveString("./deviceType");
		
		/* Initialize device: */
		#ifdef VERBOSE
		printf("VRDeviceManager: Loading device %s of type %s\n",deviceNames[currentDeviceIndex].c_str(),deviceType.c_str());
		fflush(stdout);
		#endif
		DeviceFactoryManager::Factory* deviceFactory=deviceFactories.getFactory(deviceType);
		devices[currentDeviceIndex]=deviceFactory->createObject(configFile);
		
		if(configFile.hasTag("./trackerNames"))
			{
			StringList deviceTrackerNames=configFile.retrieveValue<StringList>("./trackerNames");
			int trackerIndex=trackerIndexBases[currentDeviceIndex];
			int numTrackers=trackerNames.size();
			for(StringList::iterator dtnIt=deviceTrackerNames.begin();dtnIt!=deviceTrackerNames.end()&&trackerIndex<numTrackers;++dtnIt,++trackerIndex)
				trackerNames[trackerIndex]=*dtnIt;
			}
		
		/* Override device's button names: */
		if(configFile.hasTag("./buttonNames"))
			{
			StringList deviceButtonNames=configFile.retrieveValue<StringList>("./buttonNames");
			int buttonIndex=buttonIndexBases[currentDeviceIndex];
			int numButtons=buttonNames.size();
			for(StringList::iterator dtnIt=deviceButtonNames.begin();dtnIt!=deviceButtonNames.end()&&buttonIndex<numButtons;++dtnIt,++buttonIndex)
				buttonNames[buttonIndex]=*dtnIt;
			}
		
		/* Override device's valuator names: */
		if(configFile.hasTag("./valuatorNames"))
			{
			StringList deviceValuatorNames=configFile.retrieveValue<StringList>("./valuatorNames");
			int valuatorIndex=valuatorIndexBases[currentDeviceIndex];
			int numValuators=valuatorNames.size();
			for(StringList::iterator dtnIt=deviceValuatorNames.begin();dtnIt!=deviceValuatorNames.end()&&valuatorIndex<numValuators;++dtnIt,++valuatorIndex)
				valuatorNames[valuatorIndex]=*dtnIt;
			}
		
		/* Return to parent section: */
		configFile.setCurrentSection("..");
		}
	#ifdef VERBOSE
	printf("VRDeviceManager: Managing %d trackers, %d buttons, %d valuators\n",int(trackerNames.size()),int(buttonNames.size()),int(valuatorNames.size()));
	fflush(stdout);
	#endif
	
	/* Set server state's layout: */
	state.setLayout(trackerNames.size(),buttonNames.size(),valuatorNames.size());
	
	/* Read names of all virtual devices: */
	StringList virtualDeviceNames=configFile.retrieveValue<StringList>("./virtualDeviceNames",StringList());
	
	/* Initialize virtual devices: */
	for(StringList::iterator vdnIt=virtualDeviceNames.begin();vdnIt!=virtualDeviceNames.end();++vdnIt)
		{
		/* Create a new virtual device descriptor by reading the virtual device's configuration file section: */
		Vrui::VRDeviceDescriptor* vdd=new Vrui::VRDeviceDescriptor;
		vdd->load(configFile.getSection(vdnIt->c_str()));
		
		/* Store the virtual device descriptor: */
		virtualDevices.push_back(vdd);
		}
	#ifdef VERBOSE
	printf("VRDeviceManager: Managing %d virtual devices\n",int(virtualDevices.size()));
	fflush(stdout);
	#endif
	}