vrpn_SerialPort::vrpn_SerialPort(const char * portname, long baud, int charsize, vrpn_SER_PARITY parity)
: _comm(vrpn_open_commport(portname, baud, charsize, parity))
, _rts_status(false) {
if (!is_open()) {
throw OpenFailure();
}
}
vrpn_Serial_Analog::vrpn_Serial_Analog (const char * name, vrpn_Connection * c,
const char * port, int baud, int bits,
vrpn_SER_PARITY parity, bool rts_flow) :
vrpn_Analog(name, c),
serial_fd(-1),
baudrate(0),
bufcounter(0)
{
// Initialize
portname[0] = '\0';
buffer[0] = '\0';
// Find out the port name and baud rate;
if (port == NULL) {
fprintf(stderr,"vrpn_Serial_Analog: NULL port name\n");
status = vrpn_ANALOG_FAIL;
return;
} else {
strncpy(portname, port, sizeof(portname));
portname[sizeof(portname)-1] = '\0';
}
baudrate = baud;
// Open the serial port we're going to use
if ( (serial_fd=vrpn_open_commport(portname, baudrate, bits, parity, rts_flow)) == -1) {
fprintf(stderr,"vrpn_Serial_Analog: Cannot Open serial port\n");
status = vrpn_ANALOG_FAIL;
}
// Reset the tracker and find out what time it is
status = vrpn_ANALOG_RESETTING;
vrpn_gettimeofday(×tamp, NULL);
}
void vrpn_Tracker_3DMouse::mainloop()
{
server_mainloop();
switch(status)
{
case vrpn_TRACKER_AWAITING_STATION:
case vrpn_TRACKER_PARTIAL:
case vrpn_TRACKER_SYNCING:
if (get_report()) send_report();
break;
case vrpn_TRACKER_RESETTING:
reset();
break;
case vrpn_TRACKER_FAIL:
fprintf(stderr, "3DMouse failed, trying to reset (Try power cycle if more than 4 attempts made)\n");
if (serial_fd >= 0)
{
vrpn_close_commport(serial_fd);
serial_fd = -1;
}
if ( (serial_fd=vrpn_open_commport(portname, baudrate)) == -1)
{
fprintf(stderr,"vrpn_Tracker_3DMouse::mainloop(): Cannot Open serial port\n");
status = vrpn_TRACKER_FAIL;
return;
}
status = vrpn_TRACKER_RESETTING;
break;
default:
break;
}
}
vrpn_VPJoystick::vrpn_VPJoystick(char* name, vrpn_Connection *c,
const char *port, long baud)
: vrpn_Button_Filter(name, c),
serial_fd(0), state( STATE_SYNCHING )
{
int i;
// Open the serial port
if ( (serial_fd=vrpn_open_commport(port, baud)) == -1) {
fprintf(stderr,"vrpn_VPJoystick: Cannot Open serial port\n");
}
// find out what time it is - needed?
vrpn_gettimeofday(×tamp, 0);
vrpn_Button::timestamp = timestamp;
num_buttons = vrpn_VPJOY_NUM_BUTTONS;
button_masks[0] = VP_BUTTON_1;
button_masks[1] = VP_BUTTON_2;
button_masks[2] = VP_BUTTON_3;
button_masks[3] = VP_BUTTON_4;
button_masks[4] = VP_BUTTON_5;
button_masks[5] = VP_BUTTON_6;
button_masks[6] = VP_BUTTON_7;
button_masks[7] = VP_BUTTON_8;
for( i=0; i< num_buttons; i++ ) {
buttons[i] = lastbuttons[i] = VRPN_BUTTON_OFF;
}
}
void vrpn_SerialPort::open(const char * portname, long baud, int charsize, vrpn_SER_PARITY parity) {
if (is_open()) {
throw AlreadyOpen();
}
_comm = vrpn_open_commport(portname, baud, charsize, parity);
if (!is_open()) {
throw OpenFailure();
}
}
/* static */ vrpn_Atmel *
vrpn_Atmel::Create(char* name, vrpn_Connection *c,
char *port, long baud,
int channel_count ,
int * channel_mode)
{
int fd;
#ifdef VRPN_ATMEL_SERIAL_VRPN
if ( (fd=vrpn_open_commport(port, baud)) == -1) {
fprintf(stderr,"vrpn_Atmel: can't Open serial port\n");
return NULL;
}
#else
// Open the serial port
if ( (fd=openPort(port, baud, &init_params)) == -1) {
fprintf(stderr,"vrpn_Atmel: can't Open serial port\n");
return NULL;
}
// look if the atmel is connected
if (! check_serial(fd) ) {
return NULL;
}
#endif
vrpn_Atmel * self = new vrpn_Atmel(name, c, fd);
if ( (self->vrpn_Analog_Server::setNumChannels(channel_count) != channel_count)
|| (self->vrpn_Analog_Output_Server::setNumChannels(channel_count) != channel_count) ) {
fprintf(stderr,"vrpn_Atmel: the requested number of channels is not available\n");
delete self;
return NULL;
}
// init the channels based on the infos given from the config file
self->init_channel_mode(channel_mode);
self->_status = VRPN_ATMEL_STATUS_WAITING_FOR_CONNECTION;
return self;
}
void vrpn_Tracker_NDI_Polaris::switchToHigherBaudRate(const char *port) {
printf("vrpn_Tracker_NDI_Polaris: Switching NDI to higher baud rate, and then reopening com port at higher rate...");
sendCommand("COMM 70000"); // 1.2Mbit (1228739 baud), which requires the NDI USB 2.0 adapter
readResponse();
#ifdef DEBUG
printf("DEBUG: COMM response: >%s<\n",latestResponseStr);
#endif
//if the response is "RESET", try again
if (strncmp("RESET",(char *) latestResponseStr,5)==0) {
//we got a reset, which means the track reset itself (without being commanded too?)
sendCommand("COMM 70000"); // 1.2Mbit, which requires the NDI USB 2.0 adapter
readResponse();
//printf("DEBUG: COMM response: >%s<\n",latestResponseStr);
}
vrpn_SleepMsecs(100); //we should wait 100 msec after getting the OKAY from the NDI before changing the PC's comm rate
vrpn_close_commport(serialFd);
#ifdef _WIN32
serialFd=vrpn_open_commport(port,19200); //19.2k is aliased to 1.2Mbit in the Window's version of the NDI USB virtual com port driver
#else
serialFd=vrpn_open_commport(port,1228739); //should work on linux (UNTESTED!)
#endif
printf("done\n");
}
// (RDK) serial mouse wired up as button device
vrpn_Button_SerialMouse::vrpn_Button_SerialMouse(const char *name,vrpn_Connection *c,
const char *port, int baud, vrpn_MOUSETYPE type)
: vrpn_Button_Filter(name, c)
{
status = BUTTON_FAIL;
printed_error = false;
// Find out the port name and baud rate;
if (port == NULL) {
fprintf(stderr,"vrpn_Button_SerialMouse: NULL port name\n");
return;
} else {
strncpy(portname, port, sizeof(portname));
portname[sizeof(portname)-1] = '\0';
}
num_buttons = 3;
baudrate = baud;
// Open the serial port we are going to use
if ( (serial_fd=vrpn_open_commport(portname, baudrate)) == -1) {
fprintf(stderr,"vrpn_Button_SerialMouse: Cannot open serial port\n");
return;
}
for (vrpn_int32 i = 0; i < num_buttons; i++) {
buttons[i] = lastbuttons[i] = 0;
buttonstate[i] = vrpn_BUTTON_MOMENTARY;
}
mousetype = type;
lastL = lastR = 0;
// first time in read(), this will get set to 0
lastM = (mousetype == THREEBUTTON_EMULATION)?1:0;
// Say we are ready and find out what time it is
status = BUTTON_READY;
vrpn_gettimeofday(×tamp, NULL);
}
vrpn_Tracker_NDI_Polaris::vrpn_Tracker_NDI_Polaris(const char *name,
vrpn_Connection *c,
const char *port,
int numOfRigidBodies,
const char** rigidBodyNDIRomFileNames) : vrpn_Tracker(name,c)
{
/////////////////////////////////////////////////////////
//STEP 1: open com port at NDI's default speed
/////////////////////////////////////////////////////////
serialFd=vrpn_open_commport(port,9600);
if (serialFd==-1){
fprintf(stderr,"vrpn_Tracker_NDI_Polaris: Can't open serial port: %s\n",port);
} else {
printf("connected to NDI Polaris at default 9600 baud on device:%s.\n",port);
//send a reset
#ifdef DEBUG
printf("DEBUG: Reseting com port");
#endif
vrpn_set_rts(serialFd);
vrpn_SleepMsecs(100);
vrpn_clear_rts(serialFd);
printf("done\n");
vrpn_SleepMsecs(100); //if the NDI was previously running at the higher rate, it will need some time to reset
vrpn_flush_input_buffer(serialFd); //get rid of any reset message the NDI might have sent
/////////////////////////////////////////////////////////
//STEP 2: switch to a higher baud rate
/////////////////////////////////////////////////////////
switchToHigherBaudRate(port);
/////////////////////////////////////////////////////////
//STEP 3: INIT tracker
/////////////////////////////////////////////////////////
sendCommand("INIT ");
readResponse();
#ifdef DEBUG
printf("DEBUG:Init response: >%s<\n",latestResponseStr);
#endif
//0 = 20hz(default), 1= 30Hz, 2=60Hz
sendCommand("IRATE 0"); //set the illumination to the default
readResponse();
#ifdef DEBUG
printf("DEBUG: IRATE response: >%s<\n",latestResponseStr);
#endif
/////////////////////////////////////////////////////////
//STEP 4: SETUP EACH TOOL (i.e. rigid body)
/////////////////////////////////////////////////////////
this->numOfRigidBodies=numOfRigidBodies;
//declare an array of filenames, one for each tool
for (int t=0; t<numOfRigidBodies; t++) {
if (setupOneTool(rigidBodyNDIRomFileNames[t])<1) {
fprintf(stderr,"vrpn_Tracker_NDI_Polaris: tool %s didn't init properly!\n",rigidBodyNDIRomFileNames[t]);
}
}
/////////////////////////////////////////////////////////
//STEP 5: GO TO TRACKING MODE
/////////////////////////////////////////////////////////
sendCommand("TSTART 80"); //80 resets the frame counter to zero
readResponse();
#ifdef DEBUG
printf("DEBUG: Tstart response: >%s<\n",latestResponseStr);
#endif
}
}
void vrpn_Tracker_Flock_Parallel::mainloop()
{
int i;
// Call the generic server code, since we are a server.
server_mainloop();
// call slave mainloops
for (i=0;i<cSensors;i++) {
rgSlaves[i]->mainloop();
}
// check slave status (master fails if any slave does
// and the master resets the slaves)
for (i=0;i<cSensors;i++) {
// first check for failure
if (rgSlaves[i]->status==vrpn_TRACKER_FAIL) {
status=vrpn_TRACKER_FAIL;
break;
}
// now check for reset being needed (cont to check for failures)
if (rgSlaves[i]->status==vrpn_TRACKER_RESETTING) {
status=vrpn_TRACKER_RESETTING;
continue;
}
}
// the master never has reports ready -- it is always either in
// fail, reset, or sync mode
// The slaves send messages on the master's connection
switch (status) {
case vrpn_TRACKER_SYNCING:
// everything is a-ok
break;
case vrpn_TRACKER_RESETTING:
reset();
break;
case vrpn_TRACKER_FAIL:
checkError();
if (cResets==4) {
fprintf(stderr, "\nvrpn_Tracker_Flock_Parallel: problems resetting ... check that: a) all cables are attached, b) all units have FLY/STANDBY switches in FLY mode, and c) no receiver is laying too close to the transmitter. When done checking, power cycle the flock.\nWill attempt to reset in 15 seconds.\n");
vrpn_SleepMsecs(1000.0*15);
cResets=0;
}
fprintf(stderr,
"\nvrpn_Tracker_Flock_Parallel: tracker failed, trying to reset ...");
// reset master serial i/o
vrpn_close_commport(serial_fd);
serial_fd = vrpn_open_commport(portname, baudrate);
// reset slave serial i/o
for (i=0;i<cSensors;i++) {
vrpn_close_commport(rgSlaves[i]->serial_fd);
rgSlaves[i]->serial_fd = vrpn_open_commport(rgSlaves[i]->portname,
rgSlaves[i]->baudrate);
rgSlaves[i]->status = vrpn_TRACKER_RESETTING;
}
status = vrpn_TRACKER_RESETTING;
break;
}
}
void vrpn_Tracker_Crossbow::reset() {
const char *cmd;
unsigned char recv_buf[8];
struct timeval timeout;
timeout.tv_sec = 1;
timeout.tv_usec = 0;
#if 0 // doesn't help
// First, take the comm port offline for a second
vrpn_close_commport(serial_fd);
vrpn_SleepMsecs(1000);
serial_fd = vrpn_open_commport(portname, baudrate);
#endif
vrpn_flush_output_buffer(serial_fd);
vrpn_flush_input_buffer(serial_fd);
// Try resetting by toggling the RTS line of the serial port
vrpn_set_rts(serial_fd);
vrpn_SleepMsecs(750);
vrpn_clear_rts(serial_fd);
vrpn_SleepMsecs(250);
vrpn_gettimeofday(×tamp, NULL);
vrpn_flush_input_buffer(serial_fd);
cmd = "P";
vrpn_write_characters(serial_fd, reinterpret_cast<const unsigned char*> (cmd), 1);
vrpn_SleepMsecs(50); // Sleep long enough to stop receiving data
vrpn_flush_input_buffer(serial_fd);
cmd = "RSv";
vrpn_write_characters(serial_fd, reinterpret_cast<const unsigned char*> (cmd), 3);
vrpn_drain_output_buffer(serial_fd);
if (vrpn_read_available_characters(serial_fd, recv_buf, 8, &timeout) != 8) {
fprintf(stderr, "vrpn_Tracker_Crossbow::reset: Crossbow not responding to stimulus\n");
status = vrpn_TRACKER_FAIL;
return;
}
if ((recv_buf[0] != 'H') || (recv_buf[1] != 255) || (recv_buf[7] != 255)) {
fprintf(stderr, "vrpn_Tracker_Crossbow::reset: Crossbow gave unexpected ping response\n");
status = vrpn_TRACKER_FAIL;
return;
}
if (recv_buf[6] != ((recv_buf[2] + recv_buf[3] + recv_buf[4] + recv_buf[5]) & 0xFF)) {
fprintf(stderr, "vrpn_Tracker_Crossbow::reset: Crossbow gave invalid serial number checksum\n");
status = vrpn_TRACKER_FAIL;
return;
}
const char *bufptr = reinterpret_cast<const char *>(&recv_buf[2]);
vrpn_unbuffer(&bufptr, &device_serial);
if (0) do {
if (!vrpn_read_available_characters(serial_fd, recv_buf, 1, &timeout)) {
fprintf(stderr, "vrpn_Tracker_Crossbow::reset: Crossbow not responding to stimulus\n");
status = vrpn_TRACKER_FAIL;
return;
}
} while (*recv_buf != 255);
int curSize = 4, curLen = 0;
device_version = (char *) realloc(device_version, curSize * sizeof(char));
if (device_version == NULL) {
fprintf(stderr, "vrpn_Tracker_Crossbow::reset: Out of memory\n");
status = vrpn_TRACKER_FAIL;
return;
}
do {
if (!vrpn_read_available_characters(serial_fd, recv_buf, 1, &timeout)) {
fprintf(stderr, "vrpn_Tracker_Crossbow::reset: Crossbow not responding to stimulus\n");
status = vrpn_TRACKER_FAIL;
return;
}
if (*recv_buf != '$')
device_version[curLen++] = *recv_buf;
if (curLen == curSize)
device_version = (char *) realloc(device_version, curSize *= 2);
} while (*recv_buf != '$');
// Now null-terminate the version string, expanding it one last time if necessary
if (curLen == curSize)
device_version = (char *) realloc(device_version, ++curSize);
device_version[curLen] = 0;
//printf("Serial %u\tVersion '%s'\n", device_serial, device_version);
just_read_something = 0;
status = vrpn_TRACKER_SYNCING;
}
