HIDDevice::HIDDevice ( VRDevice::Factory* sFactory,
VRDeviceManager* sDeviceManager,
Misc::ConfigurationFile& configFile )
: VRDevice ( sFactory, sDeviceManager, configFile ),
hidDeviceInterface ( NULL ),
hidQueueInterface ( NULL ),
cfRunLoop ( NULL )
{
// Get the desired HID device
io_object_t hidDevice ( getHIDDevice ( configFile ) ) ;
// Create a device interface to the found HID device
hidDeviceInterface = createHIDDeviceInterface ( hidDevice ) ;
IOObjectRelease ( hidDevice ) ;
// Setup the button and axis maps
setupButtonAndAxisMaps ( configFile ) ;
// Set number of trackers, buttons, and valuators on device
setNumTrackers ( 0, configFile ) ;
setNumButtons ( buttonMap . size ( ), configFile ) ;
setNumValuators ( absAxisMap . size ( ), configFile ) ;
// Open the HID device
if ( (*hidDeviceInterface) -> open ( hidDeviceInterface, 0 ) != kIOReturnSuccess )
Misc::throwStdErr ( "HIDDevice: Unable to open HID device" ) ;
// Setup the event queue
setupEventQueue ( ) ;
}
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);
}
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;
}
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();
}
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");
}
}
RazerHydraDevice::RazerHydraDevice(VRDevice::Factory* sFactory,VRDeviceManager* sDeviceManager,Misc::ConfigurationFile& configFile)
:VRDevice(sFactory,sDeviceManager,configFile),
hydra(0),
reportEvents(false),
notFirstMeasurement(false),
keepRunning(true)
{
/* Set device configuration: */
setNumButtons(7*2,configFile);
setNumValuators(3*2,configFile);
setNumTrackers(2,configFile);
/* Open the Razer Hydra device: */
hydra=new RazerHydra(usbContext,configFile.retrieveValue<unsigned int>("./deviceIndex",0));
/* Set the position unit: */
if(configFile.retrieveValue<bool>("./unitInches",false))
hydra->setPositionInches();
else if(configFile.retrieveValue<bool>("./unitMMs",false))
hydra->setPositionMMs();
else
hydra->setPositionConversionFactor(configFile.retrieveValue<RazerHydra::Scalar>("./unitFactor",RazerHydra::Scalar(1)));
/* Set filtering parameters: */
hydra->setApplyInterleaveFilter(configFile.retrieveValue<bool>("./applyInterleaveFilter",true));
hydra->setApplyLowpassFilter(configFile.retrieveValue<bool>("./applyLowpassFilter",true));
hydra->setLowpassFilterStrength(configFile.retrieveValue<RazerHydra::Scalar>("./lowpassFilterStrength",RazerHydra::Scalar(24)));
/* Initialize device states: */
for(int i=0;i<7*2;++i)
deviceButtonStates[i]=false;
for(int i=0;i<3*2;++i)
deviceValuatorStates[i]=0.0f;
for(int i=0;i<2;++i)
{
deviceTrackerStates[i].positionOrientation=PositionOrientation::identity;
deviceTrackerStates[i].linearVelocity=TrackerState::LinearVelocity::zero;
deviceTrackerStates[i].angularVelocity=TrackerState::AngularVelocity::zero;
}
/* Start device thread (Razer Hydra device cannot be suspended and runs the entire time): */
startDeviceThread();
}
DummyDevice::DummyDevice(VRDevice::Factory* sFactory,VRDeviceManager* sDeviceManager,Misc::ConfigurationFile& configFile)
:VRDevice(sFactory,sDeviceManager,configFile),
sleepTime(configFile.retrieveValue<int>("./sleepTime"))
{
/* Read device layout: */
int numTrackers=configFile.retrieveValue<int>("./numTrackers",0);
int numButtons=configFile.retrieveValue<int>("./numButtons",0);
int numValuators=configFile.retrieveValue<int>("./numValuators",0);
setNumTrackers(numTrackers,configFile);
setNumButtons(numButtons,configFile);
setNumValuators(numValuators,configFile);
state.setLayout(numTrackers,numButtons,numValuators);
/* Read fake state data: */
for(int i=0;i<numTrackers;++i)
{
char trackerStateName[40];
sprintf(trackerStateName,"./trackerState%d",i);
Vrui::VRDeviceState::TrackerState ts;
ts.positionOrientation=configFile.retrieveValue<Vrui::VRDeviceState::TrackerState::PositionOrientation>(trackerStateName,Vrui::VRDeviceState::TrackerState::PositionOrientation::identity);
ts.linearVelocity=Vrui::VRDeviceState::TrackerState::LinearVelocity::zero;
ts.angularVelocity=Vrui::VRDeviceState::TrackerState::AngularVelocity::zero;
state.setTrackerState(i,ts);
}
for(int i=0;i<numButtons;++i)
{
char buttonStateName[40];
sprintf(buttonStateName,"./buttonState%d",i);
state.setButtonState(i,configFile.retrieveValue<Vrui::VRDeviceState::ButtonState>(buttonStateName,Vrui::VRDeviceState::ButtonState(false)));
}
for(int i=0;i<numValuators;++i)
{
char valuatorStateName[40];
sprintf(valuatorStateName,"./valuatorState%d",i);
state.setValuatorState(i,configFile.retrieveValue<Vrui::VRDeviceState::ValuatorState>(valuatorStateName,Vrui::VRDeviceState::ValuatorState(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];
}
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();
}
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
}
WiimoteTracker::WiimoteTracker(VRDevice::Factory* sFactory,VRDeviceManager* sDeviceManager,Misc::ConfigurationFile& configFile)
:VRDevice(sFactory,sDeviceManager,configFile),
wiimote(0),ledMask(configFile.retrieveValue<int>("./ledMask",0x1)),
enableTracker(configFile.retrieveValue<bool>("./enableTracker",false)),
wiiCamera(configFile.retrieveValue<Pixel>("./cameraCenter",Pixel(512,384)),
configFile.retrieveValue<Scalar>("./cameraFocalLength",Scalar(1280))),
homeTransform(configFile.retrieveValue<Transform>("./homeTransform",Transform::identity)),
firstEvent(true),
reportEvents(false)
{
/* Set device configuration: */
if(enableTracker)
setNumTrackers(1,configFile);
setNumButtons(13,configFile); // Total number of buttons on Wii controller and Nunchuck extension
setNumValuators(2,configFile); // Analog axes on Nunchuck extension
/* Connect to the Wiimote device: */
std::string deviceName=configFile.retrieveValue<std::string>("./deviceName","");
if(deviceName!="")
{
printf("WiimoteTracker: Connecting to bluetooth device %s.\n",deviceName.c_str());
printf("WiimoteTracker: Please press buttons 1 and 2 to initiate connection...");
fflush(stdout);
wiimote=new Wiimote(deviceName.c_str(),configFile);
printf(" done\n");
fflush(stdout);
}
else
{
printf("WiimoteTracker: Connecting to first compatible bluetooth device.\n");
printf("WiimoteTracker: Please press buttons 1 and 2 to initiate connection...");
fflush(stdout);
wiimote=new Wiimote(0,configFile);
printf(" done\n");
fflush(stdout);
}
#ifdef VERBOSE
/* Print the Wiimote's battery level: */
printf("WiimoteTracker: Connected wiimote's battery level is %d%%\n",wiimote->getBatteryLevel());
fflush(stdout);
#endif
if(enableTracker)
{
/* Initialize the camera tracker: */
Transform targetTransform=configFile.retrieveValue<Transform>("./targetTransformation",Transform::identity);
std::vector<Point> targetPoints=configFile.retrieveValue<std::vector<Point> >("./targetPoints");
if(targetPoints.size()!=4)
Misc::throwStdErr("WiimoteTracker: Wrong number of target points (%d, need 4)",int(targetPoints.size()));
for(int i=0;i<4;++i)
wiiCamera.setTargetPoint(i,targetTransform.transform(targetPoints[i]));
wiiCamera.setTransform(homeTransform);
wiiCameraMinimizer.maxNumIterations=1000;
wiipos=wiiCamera.getTransform();
}
/* Register an event callback with the Wiimote: */
if(enableTracker)
wiimote->getEventCallbacks().add(this,&WiimoteTracker::wiimoteEventCallback);
else
wiimote->getEventCallbacks().add(this,&WiimoteTracker::wiimoteEventCallbackNoTracker);
}
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');
}
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()];
}
