void vrpn_SerialPort::close() {
requiresOpen();
int ret = vrpn_close_commport(_comm);
if (ret != 0) {
throw CloseFailure();
}
}
vrpn_Serial_Analog::~vrpn_Serial_Analog ()
{
// Close com port when destroyed.
if (serial_fd != -1) {
vrpn_close_commport(serial_fd);
}
}
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_Atmel::~vrpn_Atmel()
{
#ifdef VRPN_ATMEL_SERIAL_VRPN
vrpn_close_commport(serial_fd);
#else
closePort(serial_fd , &init_params);
#endif
}
//------------------------------------
vrpn_Tracker_GPS::~vrpn_Tracker_GPS()
{
//Cleanup
if (testfile != NULL) fclose(testfile);
if (serial_fd >=0) {
vrpn_close_commport(serial_fd);
serial_fd = -1;
}
}
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");
}
vrpn_Tracker_NDI_Polaris::~vrpn_Tracker_NDI_Polaris()
{
vrpn_close_commport(serialFd);
}
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;
}
vrpn_VPJoystick::~vrpn_VPJoystick()
{
vrpn_close_commport(serial_fd);
}