int vrpn_Tracker_Isotrak::set_sensor_output_format(int /*sensor*/)
{
char outstring[16];
// Set output format for the station to be position, quaternion
// Don't need the space anymore, though
sprintf(outstring, "O2,11\r");
if (vrpn_write_characters(serial_fd, (const unsigned char *)outstring,
strlen(outstring)) == (int)strlen(outstring)) {
vrpn_SleepMsecs(50); // Sleep for a bit to let command run
} else {
VRPN_MSG_ERROR("Write failed on format command");
status = vrpn_TRACKER_FAIL;
return -1;
}
return 0;
}
int vrpn_Streaming_Arduino::reset(void)
{
//-----------------------------------------------------------------------
// Set the values back to zero for all analogs
clear_values();
//-----------------------------------------------------------------------
// Opening the port should cause the board to reset. (We can also do this
// by pulling DTR lines to low and then back high again.)
// @todo Lower and raise DTR lines. Or close and re-open the serial
// port.
// Wait for two seconds to let the device reset itself and then
// clear the input buffer, both the hardware input buffer and
// the local input buffer.
vrpn_SleepMsecs(2100);
vrpn_flush_input_buffer(serial_fd);
m_buffer.clear();
// Send a command telling how many ports we want. Then make sure we
// get a response within a reasonable amount of time.
std::ostringstream msg;
msg << m_numchannels;
msg << "\n";
vrpn_write_characters(serial_fd,
static_cast<const unsigned char *>(
static_cast<const void*>(msg.str().c_str())), msg.str().size());
struct timeval timeout;
timeout.tv_sec = 0;
timeout.tv_usec = 10000;
unsigned char buffer;
int ret = vrpn_read_available_characters(serial_fd, &buffer, 1, &timeout);
if (ret != 1) {
std::cout << "vrpn_Streaming_Arduino: Could not reset" << std::endl;
return -1;
}
m_buffer += buffer;
// We already got the first byte of the record, so we drop directly
// into reading.
status = STATUS_READING;
vrpn_gettimeofday(&m_timestamp, NULL); // Set watchdog now
return 0;
}
int vrpn_ImmersionBox::sendIboxCommand (char cmd, char * returnString, double delay)
{
struct timeval timeout;
timeout.tv_sec = 0;
timeout.tv_usec = 15000;
char command[2];
char responseString [MAX_IBOX_STRING+1];
int result;
if (cmd) {
command[0] = cmd;
result = vrpn_write_characters(serial_fd, (const unsigned char *) command, 1);
if (delay > 0)
pause (delay);
if (NULL == returnString)
// if we are not anticipating a return, go back now
return result;
}
// read the response to the command
result = vrpn_read_available_characters(serial_fd, (unsigned char *) responseString, MAX_IBOX_STRING, &timeout );
if (result <= 0)
return 0;
// since we're looking for a response, we need to ensure the command was properly
// echoed back... To begin with, bit 7 must be set
if (!(responseString[0] & 0x80))
return 0;
// the bits, excepting bit 7 must match
if ((responseString[0] & 0x7f) != (cmd & 0x7f))
return 0;
// copy the remainder of the response into the response
strcpy (returnString, &responseString[1]);
return 1;
}
bool vrpn_Tracker_3DMouse::set_filtering_count(int count)
{
char sBuf[16];
sBuf[0] = 0x2a;
sBuf[1] = 0x24;
sBuf[2] = 2;
sBuf[3] = 7;
sBuf[4] = count;
if (vrpn_write_characters(serial_fd, (const unsigned char*)sBuf, 5) == 5)
{
vrpn_SleepMsecs(1000.0*1);
}
else
{
perror(" 3DMouse write filtering count failed");
status = vrpn_TRACKER_FAIL;
return false;
}
return true;
}
int vrpn_Tracker_Liberty::set_sensor_output_format(int sensor)
{
char outstring[64];
const char *timestring;
const char *buttonstring;
const char *analogstring;
// Set output format for the station to be position and quaternion,
// and any of the extended Liberty (stylus with button) or
// IS900 states (timestamp, button, analog).
// This command is a capitol 'o' followed by the number of the
// station, then comma-separated values (2 for xyz, 7 for quat, 8 for
// timestamp, 10 for buttons, 0 for space) that
// indicate data sets, followed by character 13 (octal 15).
// Note that the sensor number has to be bumped to map to station number.
timestring = ",8";
buttonstring = stylus_buttons[sensor] ? ",10" : "";
analogstring="";
sprintf(outstring, "O%d,2,7%s%s%s,0\015", sensor+1, timestring,
buttonstring, analogstring);
if (DEBUG) fprintf(stderr,"[DEBUG]: %s \n",outstring);
if (vrpn_write_characters(serial_fd, (const unsigned char *)outstring,
strlen(outstring)) == (int)strlen(outstring)) {
vrpn_SleepMsecs(50); // Sleep for a bit to let command run
} else {
FT_ERROR("Write failed on format command");
status = vrpn_TRACKER_FAIL;
return -1;
}
return 0;
}
void vrpn_Tracker_3DMouse::reset()
{
static int numResets = 0; // How many resets have we tried?
int ret, i;
numResets++; // We're trying another reset
clear_values();
fprintf(stderr, "Resetting the 3DMouse (attempt %d)\n", numResets);
if (vrpn_write_characters(serial_fd, (unsigned char*)"*R", 2) == 2)
{
fprintf(stderr,".");
vrpn_SleepMsecs(1000.0*2); // Wait after each character to give it time to respond
}
else
{
perror("3DMouse: Failed writing to 3DMouse");
status = vrpn_TRACKER_FAIL;
return;
}
fprintf(stderr,"\n");
// Get rid of the characters left over from before the reset
vrpn_flush_input_buffer(serial_fd);
// Make sure that the tracker has stopped sending characters
vrpn_SleepMsecs(1000.0*2);
if ( (ret = vrpn_read_available_characters(serial_fd, _buffer, 80)) != 0)
{
fprintf(stderr, "Got >=%d characters after reset\n", ret);
for (i = 0; i < ret; i++)
{
if (isprint(_buffer[i])) fprintf(stderr,"%c",_buffer[i]);
else fprintf(stderr,"[0x%02X]",_buffer[i]);
}
fprintf(stderr, "\n");
vrpn_flush_input_buffer(serial_fd); // Flush what's left
}
// Asking for tracker status
if (vrpn_write_characters(serial_fd, (const unsigned char *) "*\x05", 2) == 2)
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
else
{
perror(" 3DMouse write failed");
status = vrpn_TRACKER_FAIL;
return;
}
// Read Status
bool success = true;
ret = vrpn_read_available_characters(serial_fd, _buffer, 2);
if (ret != 2) fprintf(stderr, " Got %d of 5 characters for status\n",ret);
fprintf(stderr, " Control Unit test : ");
if (_buffer[0] & 1) fprintf(stderr, "success\n");
else
{
fprintf(stderr, "fail\n");
success = false;
}
fprintf(stderr, " Processor test : ");
if (_buffer[0] & 2) fprintf(stderr, "success\n");
else
{
fprintf(stderr, "fail\n");
success = false;
}
fprintf(stderr, " EPROM checksum test : ");
if (_buffer[0] & 4) fprintf(stderr, "success\n");
else
{
fprintf(stderr, "fail\n");
success = false;
}
fprintf(stderr, " RAM checksum test : ");
if (_buffer[0] & 8) fprintf(stderr, "success\n");
else
{
fprintf(stderr, "fail\n");
success = false;
}
fprintf(stderr, " Transmitter test : ");
if (_buffer[0] & 16) fprintf(stderr, "success\n");
else
{
fprintf(stderr, "fail\n");
success = false;
}
fprintf(stderr, " Receiver test : ");
if (_buffer[0] & 32) fprintf(stderr, "success\n");
else
{
fprintf(stderr, "fail\n");
success = false;
}
fprintf(stderr, " Serial Port test : ");
if (_buffer[1] & 1) fprintf(stderr, "success\n");
else
{
fprintf(stderr, "fail\n");
success = false;
}
fprintf(stderr, " EEPROM test : ");
if (_buffer[0] & 2) fprintf(stderr, "success\n");
else
{
fprintf(stderr, "fail\n");
success = false;
}
if (!success)
{
fprintf(stderr, "Bad status report from 3DMouse, retrying reset\n");
status = vrpn_TRACKER_FAIL;
return;
}
else
{
fprintf(stderr, "3DMouse gives status (this is good)\n");
numResets = 0; // Success, use simple reset next time
}
// Set filtering count if the constructor parameter said to.
if (_filtering_count > 1)
{
if (!set_filtering_count(_filtering_count)) return;
}
fprintf(stderr, "Reset Completed (this is good)\n");
status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
}
/* extern */ error_t
getCmd (handle_t fd, struct command_t* Cmd)
{
struct timeval time_wait;
int ret;
#ifdef VRPN_ATMEL_SERIAL_VRPN
unsigned char read_val;
#endif
// some adds to measure time for reading out
#ifdef VRPN_ATMELLIB_TIME_MEASURE
struct timeval start;
struct timeval end;
int sec, usec;
gettimeofday( &start , 0);
#endif
unsigned char PossibilityOne;
unsigned char PossibilityTwo;
unsigned char Reference;
/* check if the given parameters are valid */
if (handle_invalid(fd))
return ENXIO;
if ((*Cmd).addr < 128)
return ATMELLIB_ERROR_NOCOMVAL;
PossibilityOne = (*Cmd).addr;
PossibilityTwo = (*Cmd).addr;
Reference = 0x01;
if (Reference & PossibilityTwo)
/* the LSB of the Address is 1 -> make 0 -> use XOR operator */
PossibilityTwo ^= Reference;
else
/* the LSB of the address is 0 -> make 1 -> use OR operator */
PossibilityTwo |= Reference;
#ifdef VRPN_ATMEL_SERIAL_VRPN
vrpn_flush_input_buffer(fd);
vrpn_flush_output_buffer(fd);
#else
tcflush( fd , TCIOFLUSH );
#endif
time_wait.tv_sec = VRPN_ATMELLIB_SELECT_WAIT_SEC;
time_wait.tv_usec = VRPN_ATMELLIB_SELECT_WAIT_USEC;
#ifdef VRPN_ATMEL_SERIAL_VRPN
vrpn_write_characters(fd, (&((*Cmd).addr)) , 1);
#else
/* you have to send the address first */
if ( (ret = select_write_wrapper( fd ,
&(time_wait),
(&((*Cmd).addr)) ,
1 ))
!= 1 ) {
fprintf(stderr, "\n vrpn_atmellib::getCmd: Error while writing down. error=%i\n",
ret);
return ret;
}
#endif
while (time_wait.tv_usec!=0) {
#ifdef VRPN_ATMEL_SERIAL_VRPN
if (( vrpn_read_available_characters(fd, &(read_val), 1, &time_wait)) != 1) {
fprintf(stderr, "vrpn_atmellib::getCmd: Error vrpn_read_available_characters for first byte\n");
break;
}
// else
ret = read_val;
#else
if ((ret = select_read_wrapper(fd, &time_wait)) < 0) {
fprintf(stderr, "vrpn_atmellib::getCmd:\
Error select_read_wrapper for first byte: %i\n" , ret);
break;
}
#endif
// found expected first byte
if ((ret==PossibilityOne) || (ret==PossibilityTwo )) {
(*Cmd).addr = ret;
#ifdef VRPN_ATMEL_SERIAL_VRPN
if (( vrpn_read_available_characters(fd, &(read_val), 1, &time_wait)) != 1) {
fprintf(stderr, "vrpn_atmellib::getCmd: Error vrpn_read_available_characters.\n");;
break;
}
//else
ret = read_val;
#else
ret = select_read_wrapper(fd, &time_wait);
#endif
if ((ret < 0) || (ret > 128)) {
fprintf(stderr, "vrpn_atmellib::getCmd: Error reading second byte: %i\n\n" , ret);
break;
}
(*Cmd).value = ret;
#ifdef VRPN_ATMELLIB_TIME_MEASURE
// display time for
gettimeofday( &end , 0);
sec=end.tv_sec-start.tv_sec;
usec=end.tv_usec-start.tv_usec;
printf("Time for reading out: sec=%i , usec=%i\n", sec, usec);
#endif
return ATMELLIB_NOERROR;
}
}
int vrpn_Tracker_3DMouse::get_report(void)
{
int ret; // Return value from function call to be checked
static int count = 0;
timeval waittime;
waittime.tv_sec = 2;
if (status == vrpn_TRACKER_SYNCING) {
unsigned char tmpc;
if (vrpn_write_characters(serial_fd, (const unsigned char*)"*d", 2) !=2)
{
perror(" 3DMouse write command failed");
status = vrpn_TRACKER_RESETTING;
return 0;
}
ret = vrpn_read_available_characters(serial_fd, _buffer+count, 16-count, &waittime);
if (ret < 0)
{
perror(" 3DMouse read failed (disconnected)");
status = vrpn_TRACKER_RESETTING;
return 0;
}
count += ret;
if (count < 16) return 0;
if (count > 16)
{
perror(" 3DMouse read failed (wrong message)");
status = vrpn_TRACKER_RESETTING;
return 0;
}
count = 0;
tmpc = _buffer[0];
if (tmpc & 32)
{
//printf("port%d: Out of Range\n", i);
//ret |= 1 << (3-i);
} else
{
long ax, ay, az; // integer form of absolute translational data
short arx, ary, arz; // integer form of absolute rotational data
float p, y, r;
ax = (_buffer[1] & 0x40) ? 0xFFE00000 : 0;
ax |= (long)(_buffer[1] & 0x7f) << 14;
ax |= (long)(_buffer[2] & 0x7f) << 7;
ax |= (_buffer[3] & 0x7f);
ay = (_buffer[4] & 0x40) ? 0xFFE00000 : 0;
ay |= (long)(_buffer[4] & 0x7f) << 14;
ay |= (long)(_buffer[5] & 0x7f) << 7;
ay |= (_buffer[6] & 0x7f);
az = (_buffer[7] & 0x40) ? 0xFFE00000 : 0;
az |= (long)(_buffer[7] & 0x7f) << 14;
az |= (long)(_buffer[8] & 0x7f) << 7;
az |= (_buffer[9] & 0x7f);
pos[0] = static_cast<float>(ax / 100000.0 * 2.54);
pos[2] = static_cast<float>(ay / 100000.0 * 2.54);
pos[1] = -static_cast<float>(az / 100000.0f * 2.54);
arx = (_buffer[10] & 0x7f) << 7;
arx += (_buffer[11] & 0x7f);
ary = (_buffer[12] & 0x7f) << 7;
ary += (_buffer[13] & 0x7f);
arz = (_buffer[14] & 0x7f) << 7;
arz += (_buffer[15] & 0x7f);
p = static_cast<float>(arx / 40.0); // pitch
y = static_cast<float>(ary / 40.0); // yaw
r = static_cast<float>(arz / 40.0); // roll
p = static_cast<float>(p * M_PI / 180);
y = static_cast<float>(y * M_PI / 180);
r = static_cast<float>((360-r) * M_PI / 180);
float cosp2 = static_cast<float>(cos(p/2));
float cosy2 = static_cast<float>(cos(y/2));
float cosr2 = static_cast<float>(cos(r/2));
float sinp2 = static_cast<float>(sin(p/2));
float siny2 = static_cast<float>(sin(y/2));
float sinr2 = static_cast<float>(sin(r/2));
d_quat[0] = cosr2*sinp2*cosy2 + sinr2*cosp2*siny2;
d_quat[1] = sinr2*cosp2*cosy2 + cosr2*sinp2*siny2;
d_quat[2] = cosr2*cosp2*siny2 + sinr2*sinp2*cosy2;
d_quat[3] = cosr2*cosp2*cosy2 + sinr2*sinp2*siny2;
}
buttons[0] = tmpc & 16; // Mouse stand button
buttons[1] = tmpc & 8; // Suspend button
buttons[2] = tmpc & 4; // Left button
buttons[3] = tmpc & 2; // Middle button
buttons[4] = tmpc & 1; // Right button
}
vrpn_Button::report_changes();
status = vrpn_TRACKER_SYNCING;
bufcount = 0;
#ifdef VERBOSE2
print_latest_report();
#endif
return 1;
}
// slave resets are ONLY called by the master
void vrpn_Tracker_Flock_Parallel_Slave::reset()
{
// slaves just flush on a reset and then stream (or poll)
// master does all the real resetting
// set vars for error handling
// set them right away so they are set properly in the
// event that we fail during the reset.
cResets++;
cSyncs=0;
fFirstStatusReport=1;
// Get rid of the characters left over from before the reset
// (make sure they are processed)
vrpn_drain_output_buffer(serial_fd);
// put back into polled mode (need to stop stream mode
// before doing an auto-config)
int resetLen=0;
unsigned char reset[3];
reset[resetLen++]='B';
// send the poll mode command (cmd and cmd_size are args)
if (vrpn_write_characters(serial_fd, (const unsigned char *) reset, resetLen )!=resetLen) {
fprintf(stderr,"\nvrpn_Tracker_Flock_Parallel_Slave %d: "
"failed writing poll cmd to tracker", d_sensor);
status = vrpn_TRACKER_FAIL;
return;
}
// make sure the command is sent out
vrpn_drain_output_buffer( serial_fd );
// wait for tracker to respond and flush buffers
vrpn_SleepMsecs(500);
// clear the input buffer (it will contain a single point
// record from the poll command above and garbage from before reset)
vrpn_flush_input_buffer(serial_fd);
// now start it running
resetLen = 0;
// either stream or let poll take place later
if (fStream==1) {
// stream mode
reset[resetLen++] = '@';
if (vrpn_write_characters(serial_fd, (const unsigned char *) reset, resetLen )!=resetLen) {
fprintf(stderr,"\nvrpn_Tracker_Flock_Parallel_Slave %d: "
"failed writing set mode cmds to tracker", d_sensor);
status = vrpn_TRACKER_FAIL;
return;
}
// make sure the commands are sent out
vrpn_drain_output_buffer( serial_fd );
} else {
poll();
}
fprintf(stderr,"\nvrpn_Tracker_Flock_Parallel_Slave %d: "
"done with reset ... running.\n", d_sensor);
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
}
// sync the baud rate on the ibox.
// seconds determines how long the process is permitted to continue
int vrpn_ImmersionBox::syncBaudrate (double seconds) {
struct timeval miniDelay;
miniDelay.tv_sec = 0;
miniDelay.tv_usec = 50000;
unsigned long maxDelay = 1000000L * (long) seconds;
struct timeval start_time;
vrpn_gettimeofday(&start_time, NULL);
int loggedOn = 0;
unsigned char responseString[8];
const unsigned char * matchString = (unsigned char *) S_INITIALIZE ; // IMMC
int index, numRead;
if (serial_fd < 0)
return 0;
vrpn_flush_input_buffer(serial_fd);
vrpn_write_characters(serial_fd, (const unsigned char *)"E", 1);
pause (0.01);
while (!loggedOn) {
struct timeval current_time;
vrpn_gettimeofday(¤t_time, NULL);
if (vrpn_TimevalDuration(current_time, start_time) > maxDelay ) {
// if we've timed out, go back unhappy
fprintf(stderr,"vrpn_ImmersionBox::syncBaudrate timeout expired: %lf secs \n", seconds);
break; // out of while loop
}
// send "IMMC"
if (4 != vrpn_write_characters(serial_fd, matchString, 4)) {
fprintf(stderr,"vrpn_ImmersionBox::syncBaudrate could not write to serial port\n");
break; // out of while loop
}
pause (0.015);
// wait for 4 characters
numRead = vrpn_read_available_characters(serial_fd, responseString, 4, &miniDelay);
if (numRead <= 0)
continue;
// get 4 characters, hopefully "IMMC"
for (index = 0; index < 4; index++) {
// get a character, check for failure
if (responseString[index] != matchString[index])
break;
}
// if we got all four, we're done
if (4 == index)
loggedOn = 1;
}
if (!loggedOn)
return 0;
// now begin the session && ensure that its an ibox we're talking to
matchString = (const unsigned char *) "IBOX";
vrpn_write_characters(serial_fd, (const unsigned char *)"BEGIN", 5);
numRead = vrpn_read_available_characters(serial_fd, responseString, 4, &miniDelay);
if (numRead <= 0)
return 0;
// check 4 characters, hopefully "IBOX"
for (index = 0; index < 4; index++) {
// get a character, check for failure
if (responseString[index] != matchString[index])
return 0;
}
vrpn_flush_input_buffer(serial_fd);
return 1;
}
void vrpn_Tracker_Isotrak::reset()
{
static int numResets = 0; // How many resets have we tried?
int i,resetLen,ret;
unsigned char reset[10];
char errmsg[512];
//--------------------------------------------------------------------
// This section deals with resetting the tracker to its default state.
// Multiple attempts are made to reset, getting more aggressive each
// time. This section completes when the tracker reports a valid status
// message after the reset has completed.
//--------------------------------------------------------------------
// Send the tracker a string that should reset it. The first time we
// try this, just do the normal 'c' command to put it into polled mode.
// After a few tries with this, use a [return] character, and then use the ^Y to reset.
resetLen = 0;
numResets++;
// We're trying another reset
if (numResets > 1) { // Try to get it out of a query loop if its in one
reset[resetLen++] = (unsigned char) (13); // Return key -> get ready
}
if (numResets > 2) {
reset[resetLen++] = (unsigned char) (25); // Ctrl + Y -> reset the tracker
}
reset[resetLen++] = 'c'; // Put it into polled (not continuous) mode
sprintf(errmsg, "Resetting the tracker (attempt %d)", numResets);
VRPN_MSG_WARNING(errmsg);
for (i = 0; i < resetLen; i++) {
if (vrpn_write_characters(serial_fd, &reset[i], 1) == 1) {
fprintf(stderr,".");
vrpn_SleepMsecs(1000.0*2); // Wait after each character to give it time to respond
} else {
perror("Isotrack: Failed writing to tracker");
status = vrpn_TRACKER_FAIL;
return;
}
}
//XXX Take out the sleep and make it keep spinning quickly
if (numResets > 2) {
vrpn_SleepMsecs(1000.0*20); // Sleep to let the reset happen, if we're doing ^Y
}
fprintf(stderr,"\n");
// Get rid of the characters left over from before the reset
vrpn_flush_input_buffer(serial_fd);
// Make sure that the tracker has stopped sending characters
vrpn_SleepMsecs(1000.0*2);
unsigned char scrap[80];
if ( (ret = vrpn_read_available_characters(serial_fd, scrap, 80)) != 0) {
sprintf(errmsg,"Got >=%d characters after reset",ret);
VRPN_MSG_WARNING(errmsg);
for (i = 0; i < ret; i++) {
if (isprint(scrap[i])) {
fprintf(stderr,"%c",scrap[i]);
} else {
fprintf(stderr,"[0x%02X]",scrap[i]);
}
}
fprintf(stderr, "\n");
vrpn_flush_input_buffer(serial_fd); // Flush what's left
}
// Asking for tracker status
if (vrpn_write_characters(serial_fd, (const unsigned char *) "S", 1) == 1) {
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
} else {
perror(" Isotrack write failed");
status = vrpn_TRACKER_FAIL;
return;
}
// Read Status
unsigned char statusmsg[22];
// Attempt to read 21 characters.
ret = vrpn_read_available_characters(serial_fd, statusmsg, 21);
if ( (ret != 21) ) {
fprintf(stderr,
" Got %d of 21 characters for status\n",ret);
VRPN_MSG_ERROR("Bad status report from Isotrack, retrying reset");
return;
}
else if ( (statusmsg[0]!='2') ) {
int i;
statusmsg[sizeof(statusmsg) - 1] = '\0'; // Null-terminate the string
fprintf(stderr, " Isotrack: bad status (");
for (i = 0; i < ret; i++) {
if (isprint(statusmsg[i])) {
fprintf(stderr,"%c",statusmsg[i]);
} else {
fprintf(stderr,"[0x%02X]",statusmsg[i]);
}
}
fprintf(stderr,")\n");
VRPN_MSG_ERROR("Bad status report from Isotrack, retrying reset");
return;
} else {
VRPN_MSG_WARNING("Isotrack gives correct status (this is good)");
numResets = 0; // Success, use simple reset next time
}
//--------------------------------------------------------------------
// Now that the tracker has given a valid status report, set all of
// the parameters the way we want them. We rely on power-up setting
// based on the receiver select switches to turn on the receivers that
// the user wants.
//--------------------------------------------------------------------
// Set output format. This is done once for the Isotrak, not per channel.
if (set_sensor_output_format(0)) {
return;
}
// Enable filtering if the constructor parameter said to.
// Set filtering for both position (x command) and orientation (v command)
// to the values that are recommended as a "jumping off point" in the
// Isotrack manual.
if (do_filter) {
if (vrpn_write_characters(serial_fd, (const unsigned char *)"x0.2,0.2,0.8,0.8\015", 17) == 17) {
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
} else {
perror(" Isotrack write position filter failed");
status = vrpn_TRACKER_FAIL;
return;
}
if (vrpn_write_characters(serial_fd, (const unsigned char *)"v0.2,0.2,0.8,0.8\015", 17) == 17) {
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
} else {
perror(" Isotrack write orientation filter failed");
status = vrpn_TRACKER_FAIL;
return;
}
}
// RESET Alignment reference frame
if (vrpn_write_characters(serial_fd, (const unsigned char *) "R1\r", 3) != 3) {
perror(" Isotrack write failed");
status = vrpn_TRACKER_FAIL;
return;
} else {
VRPN_MSG_WARNING("Isotrack reset ALIGNMENT reference frame (this is good)");
}
// reset BORESIGHT
if (vrpn_write_characters(serial_fd, (const unsigned char *) "b1\r", 3) != 3) {
perror(" Isotrack write failed");
status = vrpn_TRACKER_FAIL;
return;
} else {
VRPN_MSG_WARNING("Isotrack reset BORESIGHT (this is good)");
}
// Set data format to METRIC mode
if (vrpn_write_characters(serial_fd, (const unsigned char *) "u", 1) != 1) {
perror(" Isotrack write failed");
status = vrpn_TRACKER_FAIL;
return;
} else {
VRPN_MSG_WARNING("Isotrack set to metric units (this is good)");
}
// Send the additional reset commands, if any, to the tracker.
// These commands come in lines, with character \015 ending each
// line. If a line start with an asterisk (*), treat it as a pause
// command, with the number of seconds to wait coming right after
// the asterisk. Otherwise, the line is sent directly to the tracker.
// Wait a while for them to take effect, then clear the input
// buffer.
if (strlen(add_reset_cmd) > 0) {
char *next_line;
char add_cmd_copy[sizeof(add_reset_cmd)];
char string_to_send[sizeof(add_reset_cmd)];
int seconds_to_wait;
printf(" Isotrack writing extended reset commands...\n");
// Make a copy of the additional reset string, since it is consumed
strncpy(add_cmd_copy, add_reset_cmd, sizeof(add_cmd_copy));
// Pass through the string, testing each line to see if it is
// a sleep command or a line to send to the tracker. Continue until
// there are no more line delimiters ('\015'). Be sure to write the
// \015 to the end of the string sent to the tracker.
// Note that strok() puts a NULL character in place of the delimiter.
next_line = strtok(add_cmd_copy, "\015");
while (next_line != NULL) {
if (next_line[0] == '*') { // This is a "sleep" line, see how long
seconds_to_wait = atoi(&next_line[1]);
fprintf(stderr," ...sleeping %d seconds\n",seconds_to_wait);
vrpn_SleepMsecs(1000.0*seconds_to_wait);
} else { // This is a command line, send it
sprintf(string_to_send, "%s\015", next_line);
fprintf(stderr, " ...sending command: %s\n", string_to_send);
vrpn_write_characters(serial_fd,
(const unsigned char *)string_to_send,strlen(string_to_send));
}
next_line = strtok(next_line+strlen(next_line)+1, "\015");
}
// Sleep a little while to let this finish, then clear the input buffer
vrpn_SleepMsecs(1000.0*2);
vrpn_flush_input_buffer(serial_fd);
}
// Set data format to BINARY mode
// F = ASCII, f = binary
if (vrpn_write_characters(serial_fd, (const unsigned char *) "f", 1) != 1) {
perror(" Isotrack write failed");
status = vrpn_TRACKER_FAIL;
return;
} else {
VRPN_MSG_WARNING("Isotrack set to BINARY mode (this is good)");
}
// Set tracker to continuous mode
if (vrpn_write_characters(serial_fd, (const unsigned char *) "C", 1) != 1) {
perror(" Isotrack write failed");
status = vrpn_TRACKER_FAIL;
return;
} else {
VRPN_MSG_WARNING("Isotrack set to continuous mode (this is good)");
}
VRPN_MSG_WARNING("Reset Completed.");
status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
// Ok, device is ready, we want to calibrate to sensor 1 current position/orientation
while(get_report() != 1);
// Done with reset.
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
}
int vrpn_ImmersionBox::reset(void)
{
//-----------------------------------------------------------------------
// Set the values back to zero for all buttons, analogs and encoders
clear_values();
//-----------------------------------------------------------------------
// sending an end at this time will force the ibox into the reset mode, if it
// was not already. if the ibox is in the power up mode, nothing will happen because
// it 'should' be waiting to sync up the baudrate
// try to synchronize for 2 seconds
if (syncBaudrate (10.0)) {
printf("vrpn_ImmersionBox found\n");
} else {
return -1;
}
if (0 == sendIboxCommand((char) CMD_GETNAME, (char *) &iname, .1)) {
fprintf(stderr,"problems with ibox command CMD_GETNAME\n");
return -1;
}
if (0 == sendIboxCommand((char) CMD_GETPRID, (char *) &id, .1)) {
fprintf(stderr,"problems with ibox command CMD_GETPRID\n");
return -1;
}
if (0 == sendIboxCommand((char) CMD_GETMODL, (char *) &model, .1)){
fprintf(stderr,"problems with ibox command CMD_GETMODL\n");
return -1;
}
if (0 == sendIboxCommand((char) CMD_GETSERN, (char *) &serial, .1)){
fprintf(stderr,"problems with ibox command CMD_GETSERN\n");
return -1;
}
if (0 == sendIboxCommand((char) CMD_GETCOMM, (char *) &comment, .1)){
fprintf(stderr,"problems with ibox command CMD_GETCOMM\n");
return -1;
}
if (0 == sendIboxCommand((char) CMD_GETPERF, (char *) &parmf, .1)){
fprintf(stderr,"problems with ibox command CMD_GETPERF\n");
return -1;
}
if (0 == sendIboxCommand((char) CMD_GETVERS, (char *) &vers, .1)){
fprintf(stderr,"problems with ibox command CMD_GETVERS\n");
return -1;
}
#ifdef VERBOSE
printf("%s\n%s\n%s\n%s\n%s\n%s\n", iname, id, serial, comment, parmf, vers);
#endif
//-----------------------------------------------------------------------
// Compute the proper string to initialize the device based on how many
// of each type of input/output that is selected.
setupCommand (0, _numchannels, _numencoders);
unsigned char command[26] = "";
command[0] = 0xcf;
command[1] = 0x0;
command[2] = (unsigned char) 10; // milliseconds between reports
command[3] = commandByte;
for (int i = 4; i < 25;i++)
command[i] = 0x0;
//-----------------------------------------------------------------------
int result = vrpn_write_characters(serial_fd, (const unsigned char *) command, 25);
// Ask the box to send a report, ensure echo received
if (result < 25) {
fprintf(stderr,"vrpnImmersionBox::reset: could not write command\n");
return -1;
}
pause (.1);
// look for command echo
result = vrpn_read_available_characters(serial_fd, (unsigned char *) command, 1);
if (result <= 0 || command[0] != 0xcf) {
fprintf(stderr,"vrpnImmersionBox::reset: no command echo\n");
return -1;
}
// flush the input buffer
vrpn_flush_input_buffer(serial_fd);
printf("ImmersionBox reset complete.\n");
status = STATUS_SYNCING;
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
return 0;
}
int vrpn_Tracker_Crossbow::get_report() {
struct timeval timeout;
timeout.tv_sec = 0;
timeout.tv_usec = 500000; // Half a second
switch (status) {
case vrpn_TRACKER_AWAITING_STATION:
fprintf(stderr, "vrpn_Tracker_Crossbow: sanity: should never enter AWAITING_STATION state\n");
return 0;
case vrpn_TRACKER_SYNCING: {
int rv;
// Quit early if we just got a packet
if (just_read_something) {
just_read_something = 0;
return 0;
}
// Request a packet from the device
unsigned char echo = 'G';
vrpn_write_characters(serial_fd, &echo, 1);
rv = vrpn_read_available_characters(serial_fd, buffer, 1, &timeout);
// Return early if no characters are available
if (!rv)
return 0;
// Return early if we aren't at the start of a packet header
if (*buffer != 0xAA) {
return 0;
}
bufcount = 1;
status = vrpn_TRACKER_PARTIAL;
// Fall through to next state
}
case vrpn_TRACKER_PARTIAL: {
int rv;
if (bufcount == 1) {
rv = vrpn_read_available_characters(serial_fd, buffer + 1, 1, &timeout);
if (!rv || (buffer[1] != 0x55)) {
buffer[0] = 0;
bufcount = 0;
status = vrpn_TRACKER_SYNCING;
return 0;
}
bufcount++;
}
// Try to read the rest of the packet. Return early if we haven't got a full packet yet.
rv = vrpn_read_available_characters(serial_fd, buffer + bufcount, 24 - bufcount, &timeout);
bufcount += rv;
if (bufcount < 24)
return 0;
raw_packet new_data;
unbuffer_packet(new_data, &buffer[0]);
// Ensure the packet is valid
if (validate_packet(new_data)) {
vrpn_flush_input_buffer(serial_fd);
status = vrpn_TRACKER_SYNCING;
return 0;
}
// Prepare the new report
process_packet(new_data);
bufcount = 0;
memset(buffer, 0, 24);
status = vrpn_TRACKER_SYNCING;
just_read_something = 1;
return 1;
}
default:
fprintf(stderr, "vrpn_Tracker_Crossbow: sanity: unknown tracker state\n");
return 0;
}
}
void vrpn_Tracker_Liberty::reset()
{
int i,resetLen,ret;
char reset[10];
char errmsg[512];
char outstring1[64],outstring3[64];
//--------------------------------------------------------------------
// This section deals with resetting the tracker to its default state.
// Multiple attempts are made to reset, getting more aggressive each
// time. This section completes when the tracker reports a valid status
// message after the reset has completed.
//--------------------------------------------------------------------
// Send the tracker a string that should reset it. The first time we
// try this, just do the normal 'c' command to put it into polled mode.
// after a few tries with this, use the ^Y reset. Later, try to reset
// to the factory defaults. Then toggle the extended mode.
// Then put in a carriage return to try and break it out of
// a query mode if it is in one. These additions are cumulative: by the
// end, we're doing them all.
fprintf(stderr,"[DEBUG] Beginning Reset");
resetLen = 0;
num_resets++; // We're trying another reset
if (num_resets > 0) { // Try to get it out of a query loop if its in one
reset[resetLen++] = (char) (13); // Return key -> get ready
reset[resetLen++] = 'F';
reset[resetLen++] = '0';
reset[resetLen++] = (char) (13); // Return key -> get ready
// reset[resetLen++] = (char) (13);
// reset[resetLen++] = (char) (13);
}
/* XXX These commands are probably never needed, and can cause real
headaches for people who are keeping state in their trackers (especially
the InterSense trackers). Taking them out in version 05.01; you can put
them back in if your tracker isn't resetting as well.
if (num_resets > 3) { // Get a little more aggressive
reset[resetLen++] = 'W'; // Reset to factory defaults
reset[resetLen++] = (char) (11); // Ctrl + k --> Burn settings into EPROM
}
*/
if (num_resets > 2) {
reset[resetLen++] = (char) (25); // Ctrl + Y -> reset the tracker
reset[resetLen++] = (char) (13); // Return Key
}
reset[resetLen++] = 'P'; // Put it into polled (not continuous) mode
sprintf(errmsg, "Resetting the tracker (attempt %d)", num_resets);
VRPN_MSG_WARNING(errmsg);
for (i = 0; i < resetLen; i++) {
if (vrpn_write_characters(serial_fd, (unsigned char*)&reset[i], 1) == 1) {
fprintf(stderr,".");
vrpn_SleepMsecs(1000.0*2); // Wait after each character to give it time to respond
} else {
perror("Liberty: Failed writing to tracker");
status = vrpn_TRACKER_FAIL;
return;
}
}
//XXX Take out the sleep and make it keep spinning quickly
// You only need to sleep 10 seconds for an actual Liberty.
// For the Intersense trackers, you need to sleep 20. So,
// sleeping 20 is the more general solution...
if (num_resets > 2) {
vrpn_SleepMsecs(1000.0*20); // Sleep to let the reset happen, if we're doing ^Y
}
fprintf(stderr,"\n");
// Get rid of the characters left over from before the reset
vrpn_flush_input_buffer(serial_fd);
// Make sure that the tracker has stopped sending characters
vrpn_SleepMsecs(1000.0*2);
unsigned char scrap[80];
if ( (ret = vrpn_read_available_characters(serial_fd, scrap, 80)) != 0) {
sprintf(errmsg,"Got >=%d characters after reset",ret);
VRPN_MSG_WARNING(errmsg);
for (i = 0; i < ret; i++) {
if (isprint(scrap[i])) {
fprintf(stderr,"%c",scrap[i]);
} else {
fprintf(stderr,"[0x%02X]",scrap[i]);
}
}
fprintf(stderr, "\n");
vrpn_flush_input_buffer(serial_fd); // Flush what's left
}
// Asking for tracker status. S not implemented in Liberty and hence
// ^V (WhoAmI) is used. It retruns 196 bytes
char statusCommand[2];
statusCommand[0]=(char)(22); // ^V
statusCommand[1]=(char)(13); // Return Key
if (vrpn_write_characters(serial_fd, (const unsigned char *) &statusCommand[0], 2) == 2) {
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
} else {
perror(" Liberty write failed");
status = vrpn_TRACKER_FAIL;
return;
}
// Read Status
unsigned char statusmsg[vrpn_LIBERTY_MAX_WHOAMI_LEN+1];
// Attempt to read whoami_len characters.
ret = vrpn_read_available_characters(serial_fd, statusmsg, whoami_len);
if (ret != whoami_len) {
fprintf(stderr," Got %d of %d characters for status\n",ret, whoami_len);
}
if (ret != -1) {
statusmsg[ret] = '\0'; // Null-terminate the string
}
// It seems like some versions of the tracker report longer
// messages; so we reduced this check so that it does not check for the
// appropriate length of message or for the last character being a 10,
// so that it works more generally. The removed tests are:
// || (ret!=whoami_len) || (statusmsg[ret-1]!=(char)(10))
if ( (statusmsg[0]!='0') ) {
int i;
fprintf(stderr, " Liberty: status is (");
for (i = 0; i < ret; i++) {
if (isprint(statusmsg[i])) {
fprintf(stderr,"%c",statusmsg[i]);
} else {
fprintf(stderr,"[0x%02X]",statusmsg[i]);
}
}
fprintf(stderr,"\n)\n");
VRPN_MSG_ERROR("Bad status report from Liberty, retrying reset");
return;
} else {
VRPN_MSG_WARNING("Liberty/Isense gives status (this is good)");
printf("LIBERTY LATUS STATUS (whoami):\n%s\n\n",statusmsg);
num_resets = 0; // Success, use simple reset next time
}
//--------------------------------------------------------------------
// Now that the tracker has given a valid status report, set all of
// the parameters the way we want them. We rely on power-up setting
// based on the receiver select switches to turn on the receivers that
// the user wants.
//--------------------------------------------------------------------
// Set output format for each of the possible stations.
for (i = 0; i < num_stations; i++) {
if (set_sensor_output_format(i)) {
return;
}
}
// Enable filtering if the constructor parameter said to.
// Set filtering for both position (X command) and orientation (Y command)
// to the values that are recommended as a "jumping off point" in the
// Liberty manual.
if (do_filter) {
if (VRPN_LIBERTY_DEBUG) fprintf(stderr,"[DEBUG]: Enabling filtering\n");
if (vrpn_write_characters(serial_fd,
(const unsigned char *)"X0.2,0.2,0.8,0.8\015", 17) == 17) {
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
} else {
perror(" Liberty write position filter failed");
status = vrpn_TRACKER_FAIL;
return;
}
if (vrpn_write_characters(serial_fd,
(const unsigned char *)"Y0.2,0.2,0.8,0.8\015", 17) == 17) {
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
} else {
perror(" Liberty write orientation filter failed");
status = vrpn_TRACKER_FAIL;
return;
}
} else {
if (VRPN_LIBERTY_DEBUG) fprintf(stderr,"[DEBUG]: Disabling filtering\n");
if (vrpn_write_characters(serial_fd,
(const unsigned char *)"X0,1,0,0\015", 9) == 9) {
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
} else {
perror(" Liberty write position filter failed");
status = vrpn_TRACKER_FAIL;
return;
}
if (vrpn_write_characters(serial_fd,
(const unsigned char *)"Y0,1,0,0\015", 9) == 9) {
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
} else {
perror(" Liberty write orientation filter failed");
status = vrpn_TRACKER_FAIL;
return;
}
}
// Send the additional reset commands, if any, to the tracker.
// These commands come in lines, with character \015 ending each
// line. If a line start with an asterisk (*), treat it as a pause
// command, with the number of seconds to wait coming right after
// the asterisk. Otherwise, the line is sent directly to the tracker.
// Wait a while for them to take effect, then clear the input
// buffer.
if (strlen(add_reset_cmd) > 0) {
char *next_line;
char add_cmd_copy[sizeof(add_reset_cmd)];
char string_to_send[sizeof(add_reset_cmd)];
int seconds_to_wait;
printf(" Liberty writing extended reset commands...\n");
// Make a copy of the additional reset string, since it is consumed
vrpn_strcpy(add_cmd_copy, add_reset_cmd);
// Pass through the string, testing each line to see if it is
// a sleep command or a line to send to the tracker. Continue until
// there are no more line delimiters ('\015'). Be sure to write the
// \015 to the end of the string sent to the tracker.
// Note that strok() puts a NULL character in place of the delimiter.
next_line = strtok(add_cmd_copy, "\015");
while (next_line != NULL) {
if (next_line[0] == '*') { // This is a "sleep" line, see how long
seconds_to_wait = atoi(&next_line[1]);
fprintf(stderr," ...sleeping %d seconds\n",seconds_to_wait);
vrpn_SleepMsecs(1000.0*seconds_to_wait);
} else { // This is a command line, send it
sprintf(string_to_send, "%s\015", next_line);
fprintf(stderr, " ...sending command: %s\n", string_to_send);
vrpn_write_characters(serial_fd,
(const unsigned char *)string_to_send,strlen(string_to_send));
}
next_line = strtok(next_line+strlen(next_line)+1, "\015");
}
// Sleep a little while to let this finish, then clear the input buffer
vrpn_SleepMsecs(1000.0*2);
vrpn_flush_input_buffer(serial_fd);
}
// Set data format to BINARY mode
sprintf(outstring1, "F1\r");
if (vrpn_write_characters(serial_fd, (const unsigned char *)outstring1,
strlen(outstring1)) == (int)strlen(outstring1)) {
fprintf(stderr, " Liberty set to binary mode\n");
}
// Set tracker to continuous mode
sprintf(outstring3, "C\r");
if (vrpn_write_characters(serial_fd, (const unsigned char *)outstring3,
strlen(outstring3)) != (int)strlen(outstring3)) {
perror(" Liberty write failed");
status = vrpn_TRACKER_FAIL;
return;
} else {
fprintf(stderr, " Liberty set to continuous mode\n");
}
// If we are using the Liberty timestamps, clear the timer on the device and
// store the time when we cleared it. First, drain any characters in the output
// buffer to ensure we're sending right away. Then, send the reset command and
// store the time that we sent it, plus the estimated time for the characters to
// get across the serial line to the device at the current baud rate.
// Set time units to milliseconds (MT) and reset the time (MZ).
char clear_timestamp_cmd[] = "Q0\r";
vrpn_drain_output_buffer(serial_fd);
if (vrpn_write_characters(serial_fd, (const unsigned char *)clear_timestamp_cmd,
strlen(clear_timestamp_cmd)) != (int)strlen(clear_timestamp_cmd)) {
VRPN_MSG_ERROR("Cannot send command to clear timestamp");
status = vrpn_TRACKER_FAIL;
return;
}
// Drain the output buffer again, then record the time as the base time from
// the tracker.
vrpn_drain_output_buffer(serial_fd);
vrpn_gettimeofday(&liberty_zerotime, NULL);
// Done with reset.
vrpn_gettimeofday(&watchdog_timestamp, NULL); // Set watchdog now
VRPN_MSG_WARNING("Reset Completed (this is good)");
status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
}
void vrpn_Tracker_NDI_Polaris::sendCommand(const char* commandString ){
vrpn_write_characters(serialFd,(const unsigned char* )commandString,strlen(commandString));
vrpn_write_characters(serialFd,(const unsigned char* )"\r",1); //send the CR
vrpn_flush_output_buffer(serialFd);
}
int vrpn_CerealBox::get_report(void)
{
int ret; // Return value from function call to be checked
int i; // Loop counter
int nextchar = 1; // Index of the next character to read
//--------------------------------------------------------------------
// The reports are each _expected_chars characters long, and each start with an
// ASCII 'p' character. If we're synching, read a byte at a time until we
// find a 'p' character.
//--------------------------------------------------------------------
if (status == STATUS_SYNCING) {
// Try to get a character. If none, just return.
if (vrpn_read_available_characters(serial_fd, _buffer, 1) != 1) {
return 0;
}
// If it is not a 'p', we don't want it but we
// need to look at the next one, so just return and stay
// in Syncing mode so that we will try again next time through.
if ( _buffer[0] != 'p') {
fprintf(stderr,"vrpn_CerealBox: Syncing (looking for 'p', "
"got '%c')\n", _buffer[0]);
return 0;
}
// Got the first character of a report -- go into READING mode
// and record that we got one character at this time. The next
// bit of code will attempt to read the rest of the report.
// The time stored here is as close as possible to when the
// report was generated.
_bufcount = 1;
vrpn_gettimeofday(×tamp, NULL);
status = STATUS_READING;
#ifdef VERBOSE
printf("... Got the 1st char\n");
#endif
}
//--------------------------------------------------------------------
// Read as many bytes of this report as we can, storing them
// in the buffer. We keep track of how many have been read so far
// and only try to read the rest. The routine that calls this one
// makes sure we get a full reading often enough (ie, it is responsible
// for doing the watchdog timing to make sure the device hasn't simply
// stopped sending characters).
//--------------------------------------------------------------------
ret = vrpn_read_available_characters(serial_fd, &_buffer[_bufcount],
_expected_chars-_bufcount);
if (ret == -1) {
send_text_message("vrpn_CerealBox: Error reading", timestamp, vrpn_TEXT_ERROR);
status = STATUS_RESETTING;
return 0;
}
_bufcount += ret;
#ifdef VERBOSE
if (ret != 0) printf("... got %d characters (%d total)\n",ret, _bufcount);
#endif
if (_bufcount < _expected_chars) { // Not done -- go back for more
return 0;
}
//--------------------------------------------------------------------
// We now have enough characters to make a full report. Check to make
// sure that its format matches what we expect. If it does, the next
// section will parse it. If it does not, we need to go back into
// synch mode and ignore this report. A well-formed report has the
// first character 'p', and the last character is '\n'.
//--------------------------------------------------------------------
if (_buffer[0] != 'p') {
status = STATUS_SYNCING;
fprintf(stderr,"vrpn_CerealBox: Not 'p' in record\n");
return 0;
}
if (_buffer[_expected_chars-1] != '\n') {
status = STATUS_SYNCING;
fprintf(stderr,"vrpn_CerealBox: No carriage return in record\n");
return 0;
}
#ifdef VERBOSE
printf("got a complete report (%d of %d)!\n", _bufcount, _expected_chars);
#endif
//--------------------------------------------------------------------
// Ask the device to send us another report. Ideally, this would be
// sent earlier so that we can overlap with the previous report. However,
// when we ask after the first character we start losing parts of the
// reports when we've turned on a lot of inputs. So, we do it here
// after the report has come in.
//--------------------------------------------------------------------
vrpn_write_characters(serial_fd, (unsigned char *)"pE", 2);
//--------------------------------------------------------------------
// Decode the report and store the values in it into the parent classes
// (analog, button and encoder). This code is modelled on the routines
// convert_serial() and unpack_encoders() in the BG systems code.
//--------------------------------------------------------------------
{ // Digital code. There appear to be 4 bits (four buttons) stored
// in each byte, in the low-order 4 bits after the offset of 0x21
// has been removed from each byte. They seem to come in highest-
// buttons first, with the highest within each bank in the leftmost
// bit. This assumes we are not using MP for digital inputs.
int i;
int numbuttonchars;
// Read two characters for each eight buttons that are on, from the
// highest bank down.
numbuttonchars = ((_numbuttons+7) / 8) * 2;
for (i = numbuttonchars-1; i >= 0; i--) {
// Find the four bits for these buttons by subtracting
// the offset to get them into the low-order 4 bits
char bits = (char)(_buffer[nextchar++] - offset);
// Set the buttons for each bit
buttons[ i*4 + 3 ] = ( (bits & 0x08) != 0);
buttons[ i*4 + 2 ] = ( (bits & 0x04) != 0);
buttons[ i*4 + 1 ] = ( (bits & 0x02) != 0);
buttons[ i*4 + 0 ] = ( (bits & 0x01) != 0);
}
}
{// Analog code. Looks like there are two characters for each
// analog value; this conversion code grabbed right from the
// BG code. They seem to come in lowest-numbered first.
int intval, i;
double realval;
for (i = 0; i < _numchannels; i++) {
intval = (0x3f & (_buffer[nextchar++]-offset)) << 6;
intval |= (0x3f & (_buffer[nextchar++]-offset));
realval = -1.0 + (2.0 * intval/4095.0);
channel[i] = realval;
}
}
{ // Encoders. They come packed as 24-bit values with 6 bits in
// each byte (offset by 0x21). They seem to come least-significant
// part first. This decoding is valid only for incremental
// encoders. Remember to convert the encoder values into fractions
// of a revolution.
for (i = 0; i < _numencoders; i++) {
int enc0, enc1, enc2, enc3;
long increment;
enc0 = (_buffer[nextchar++]-offset) & 0x3f;
increment = enc0;
enc1 = (_buffer[nextchar++]-offset) & 0x3f;
increment |= enc1 << 6;
enc2 = (_buffer[nextchar++]-offset) & 0x3f;
increment |= enc2 << 12;
enc3 = (_buffer[nextchar++]-offset) & 0x3f;
increment |= enc3 << 18;
if ( increment & 0x800000 ) {
dials[i] = (int)(increment - 16777216) * REV_PER_TICK;
} else {
dials[i] = (int)(increment) * REV_PER_TICK;
}
}
}
//--------------------------------------------------------------------
// Done with the decoding, send the reports and go back to syncing
//--------------------------------------------------------------------
report_changes();
status = STATUS_SYNCING;
_bufcount = 0;
return 1;
}
int vrpn_CerealBox::reset(void)
{
struct timeval timeout;
unsigned char inbuf[45];
const char *Cpy = "Copyright (c), BG Systems";
int major, minor, bug; // Version of the EEPROM
unsigned char reset_str[32]; // Reset string sent to box
int ret;
//-----------------------------------------------------------------------
// Set the values back to zero for all buttons, analogs and encoders
clear_values();
//-----------------------------------------------------------------------
// Check that the box exists and has the correct EEPROM version. The
// "T" command to the box should cause the 44-byte EEPROM string to be
// returned. This string defines the version and type of the box.
// Give it a reasonable amount of time to finish (2 seconds), then timeout
vrpn_flush_input_buffer(serial_fd);
vrpn_write_characters(serial_fd, (const unsigned char *)"T", 1);
timeout.tv_sec = 2;
timeout.tv_usec = 0;
ret = vrpn_read_available_characters(serial_fd, inbuf, 44, &timeout);
inbuf[44] = 0; // Make sure string is NULL-terminated
if (ret < 0) {
perror("vrpn_CerealBox: Error reading from box\n");
return -1;
}
if (ret == 0) {
fprintf(stderr,"vrpn_CerealBox: No response from box\n");
return -1;
}
if (ret != 44) {
fprintf(stderr,"vrpn_CerealBox: Got %d of 44 expected characters\n",ret);
return -1;
}
// Parse the copyright string for the version and other information
// Code here is similar to check_rev() function in the BG example code.
if (strncmp((char *)inbuf, Cpy, strlen(Cpy))) {
fprintf(stderr,"vrpn_CerealBox: Copyright string mismatch: %s\n",inbuf);
return -1;
}
major = inbuf[38] - '0';
minor = inbuf[40] - '0';
bug = inbuf[41] - '0';
if ( (3 != major) || (0 != minor) || (7 != bug) ) {
fprintf(stderr, "vrpn_CerealBox: Bad EEPROM version (want 3.07, got %d.%d%d)\n",
major, minor, bug);
return -1;
}
printf("vrpn_CerealBox: Box of type %c found\n",inbuf[42]);
//-----------------------------------------------------------------------
// Compute the proper string to initialize the device based on how many
// of each type of input/output that is selected. This follows init_cereal()
// in BG example code.
{ int i;
char ana1_4 = 0; // Bits 1-4 do analog channels 1-4
char ana5_8 = 0; // Bits 1-4 do analog channels 5-8
char dig1_3 = 0; // Bits 1-3 enable groups of 8 inputs
char enc1_4 = 0; // Bits 1-4 enable encoders 1-4
char enc5_8 = 0; // Bits 1-4 enable encoders 5-8
// Figure out which analog channels to use and set them
for (i = 0; i < 4; i++) {
if (i < _numchannels) {
ana1_4 |= (1<<i);
}
}
for (i = 0; i < 4; i++) {
if (i+4 < _numchannels) {
ana5_8 |= (1<<i);
}
}
// Figure out which banks of digital inputs to use and set them
for (i = 0; i < _numbuttons; i++) {
dig1_3 |= (1 << (i/8));
}
// Figure out which encoder channels to use and set them
for (i = 0; i < 4; i++) {
if (i < _numencoders) {
enc1_4 |= (1<<i);
}
}
for (i = 0; i < 4; i++) {
if (i+4 < _numencoders) {
enc5_8 |= (1<<i);
}
}
reset_str[0] = 'c';
reset_str[1] = (unsigned char)(ana1_4 + offset); // Hope we don't need to set baud rate
reset_str[2] = (unsigned char)((ana5_8 | (dig1_3 << 4)) + offset);
reset_str[3] = (unsigned char)(0 + offset);
reset_str[4] = (unsigned char)(0 + offset);
reset_str[5] = (unsigned char)(enc1_4 + offset);
reset_str[6] = (unsigned char)(enc1_4 + offset); // Set encoders 1-4 for incremental
reset_str[7] = (unsigned char)(enc5_8 + offset);
reset_str[8] = (unsigned char)(enc5_8 + offset); // Set encoders 5-8 for incremental
reset_str[9] = '\n';
reset_str[10] = 0;
}
// Send the string and then wait for an acknowledgement from the box.
vrpn_write_characters(serial_fd, reset_str, 10);
timeout.tv_sec = 2;
timeout.tv_usec = 0;
ret = vrpn_read_available_characters(serial_fd, inbuf, 2, &timeout);
if (ret < 0) {
perror("vrpn_CerealBox: Error reading ack from box\n");
return -1;
}
if (ret == 0) {
fprintf(stderr,"vrpn_CerealBox: No ack from box\n");
return -1;
}
if (ret != 2) {
fprintf(stderr,"vrpn_CerealBox: Got %d of 2 expected ack characters\n",ret);
return -1;
}
if (inbuf[0] != 'a') {
fprintf(stderr,"vrpn_CerealBox: Bad ack: wanted 'a', got '%c'\n",inbuf[0]);
return -1;
}
//-----------------------------------------------------------------------
// Ask the box to send a report, and go into SYNCING mode to get it.
vrpn_write_characters(serial_fd, (unsigned char *)"pE", 2);
status = STATUS_SYNCING;
printf("CerealBox reset complete.\n");
//-----------------------------------------------------------------------
// Figure out how many characters to expect in each report from the device
// There is a 'p' to start and '\n' to finish, two bytes for each analog
// value, 4 bytes for each encoder. Buttons are enabled in banks of 8,
// but each bank of 8 is returned in 2 bytes (4 bits each).
_expected_chars = 2 + 2*_numchannels + _numencoders*4 +
((_numbuttons+7) / 8) * 2;
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
return 0;
}
void vrpn_Tracker_3Space::reset()
{
static int numResets = 0; // How many resets have we tried?
int i,resetLen,ret;
unsigned char reset[10];
// Send the tracker a string that should reset it. The first time we
// try this, just do the normal ^Y reset. Later, try to reset
// to the factory defaults. Then toggle the extended mode.
// Then put in a carriage return to try and break it out of
// a query mode if it is in one. These additions are cumulative: by the
// end, we're doing them all.
resetLen = 0;
numResets++; // We're trying another reset
if (numResets > 1) { // Try to get it out of a query loop if its in one
reset[resetLen++] = (char) (13); // Return key -> get ready
}
if (numResets > 7) {
reset[resetLen++] = 'Y'; // Put tracker into tracking (not point) mode
}
if (numResets > 3) { // Get a little more aggressive
if (numResets > 4) { // Even more aggressive
reset[resetLen++] = 't'; // Toggle extended mode (in case it is on)
}
reset[resetLen++] = 'W'; // Reset to factory defaults
reset[resetLen++] = (char) (11); // Ctrl + k --> Burn settings into EPROM
}
reset[resetLen++] = (char) (25); // Ctrl + Y -> reset the tracker
send_text_message("Resetting", timestamp, vrpn_TEXT_ERROR, numResets);
for (i = 0; i < resetLen; i++) {
if (vrpn_write_characters(serial_fd, &reset[i], 1) == 1) {
sleep(2); // Wait 2 seconds each character
} else {
send_text_message("Failed writing to tracker", timestamp, vrpn_TEXT_ERROR, numResets);
perror("3Space: Failed writing to tracker");
status = vrpn_TRACKER_FAIL;
return;
}
}
sleep(10); // Sleep to let the reset happen
// Get rid of the characters left over from before the reset
vrpn_flush_input_buffer(serial_fd);
// Make sure that the tracker has stopped sending characters
sleep(2);
unsigned char scrap[80];
if ( (ret = vrpn_read_available_characters(serial_fd, scrap, 80)) != 0) {
fprintf(stderr," 3Space warning: got >=%d characters after reset:\n",ret);
for (i = 0; i < ret; i++) {
if (isprint(scrap[i])) {
fprintf(stderr,"%c",scrap[i]);
} else {
fprintf(stderr,"[0x%02X]",scrap[i]);
}
}
fprintf(stderr, "\n");
vrpn_flush_input_buffer(serial_fd); // Flush what's left
}
// Asking for tracker status
if (vrpn_write_characters(serial_fd, (const unsigned char *) "S", 1) == 1) {
sleep(1); // Sleep for a second to let it respond
} else {
perror(" 3Space write failed");
status = vrpn_TRACKER_FAIL;
return;
}
// Read Status
unsigned char statusmsg[56];
if ( (ret = vrpn_read_available_characters(serial_fd, statusmsg, 55)) != 55){
fprintf(stderr, " Got %d of 55 characters for status\n",ret);
}
if ( (statusmsg[0]!='2') || (statusmsg[54]!=(char)(10)) ) {
int i;
statusmsg[55] = '\0'; // Null-terminate the string
fprintf(stderr, " Tracker: status is (");
for (i = 0; i < 55; i++) {
if (isprint(statusmsg[i])) {
fprintf(stderr,"%c",statusmsg[i]);
} else {
fprintf(stderr,"[0x%02X]",statusmsg[i]);
}
}
fprintf(stderr, ")\n Bad status report from tracker, retrying reset\n");
return;
} else {
send_text_message("Got status (tracker back up)!", timestamp, vrpn_TEXT_ERROR, 0);
numResets = 0; // Success, use simple reset next time
}
// Set output format to be position,quaternion
// These are a capitol 'o' followed by comma-separated values that
// indicate data sets according to appendix F of the 3Space manual,
// then followed by character 13 (octal 15).
if (vrpn_write_characters(serial_fd, (const unsigned char *)"O2,11\015", 6) == 6) {
sleep(1); // Sleep for a second to let it respond
} else {
perror(" 3Space write failed");
status = vrpn_TRACKER_FAIL;
return;
}
// Set data format to BINARY mode
vrpn_write_characters(serial_fd, (const unsigned char *)"f", 1);
// Set tracker to continuous mode
if (vrpn_write_characters(serial_fd,(const unsigned char *) "C", 1) != 1)
perror(" 3Space write failed");
else {
fprintf(stderr, " 3Space set to continuous mode\n");
}
fprintf(stderr, " (at the end of 3Space reset routine)\n");
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
}
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;
}