int vrpn_Nikon_Controls::reset(void)
{
unsigned char inbuf[256];
int ret;
char errmsg[256];
char cmd[256];
double response_pos; //< Where the focus says it is.
//-----------------------------------------------------------------------
// Sleep a second and then drain the input buffer to make sure we start
// with a fresh slate.
vrpn_SleepMsecs(1000);
vrpn_flush_input_buffer(serial_fd);
//-----------------------------------------------------------------------
// Send the command to request the focus. Then wait 1 second for a response
sprintf(cmd, "rSPR\r");
if (vrpn_write_characters(serial_fd, (unsigned char *)cmd, strlen(cmd)) != (int)strlen(cmd)) {
fprintf(stderr,"vrpn_Nikon_Controls::reset(): Cannot send focus request\n");
return -1;
}
vrpn_SleepMsecs(1000);
//-----------------------------------------------------------------------
// Read the response from the camera and then see if it is a good response,
// an error message, or nothing.
ret = vrpn_read_available_characters(serial_fd, inbuf, sizeof(inbuf));
if (ret < 0) {
perror("vrpn_Nikon_Controls::reset(): Error reading position from device");
return -1;
}
if (ret == 0) {
fprintf(stderr, "vrpn_Nikon_Controls::reset(): No characters when reading position from device\n");
return -1;
}
inbuf[ret] = '\0'; //< Null-terminate the input string
ret = parse_focus_position_response((char *)inbuf, response_pos);
if (ret < 0) {
fprintf(stderr,"vrpn_Nikon_Controls::reset(): Error reading focus: %s\n",
error_code_to_string((int)(response_pos)));
return -1;
}
if (ret != 1) {
fprintf(stderr,"vrpn_Nikon_Controls::reset(): Unexpected response to focus request\n");
return -1;
}
channel[0] = response_pos;
sprintf(errmsg,"Focus reported (this is good)");
VRPN_MSG_WARNING(errmsg);
// We're now waiting for any responses from devices
status = STATUS_SYNCING;
VRPN_MSG_WARNING("reset complete (this is good)");
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
return 0;
}
void vrpn_Tracker_Crossbow::recalibrate(vrpn_uint16 num_samples) {
if (num_samples < 100) {
fprintf(stderr, "vrpn_Tracker_Crossbow: Must recalibrate using at least 100 samples\n");
return;
}
else if (num_samples > 25599) {
fprintf(stderr, "vrpn_Tracker_Crossbow: Capping recalibration at 25,500 samples\n");
num_samples = 25500;
}
vrpn_drain_output_buffer(serial_fd);
vrpn_flush_input_buffer(serial_fd);
// Prepare zero command
unsigned char buffer[2];
buffer[0] = 'z';
buffer[1] = (unsigned char) (num_samples / 100);
vrpn_write_characters(serial_fd, buffer, 2);
vrpn_drain_output_buffer(serial_fd);
vrpn_SleepMsecs(50);
// Wait for affirmative response.
// Allow two minutes before timing out.
// Even 25500 samples should make it with a few seconds to spare.
struct timeval timeout;
timeout.tv_sec = 120;
timeout.tv_usec = 0;
if (!vrpn_read_available_characters(serial_fd, buffer, 1, &timeout) || *buffer != 'Z') {
fprintf(stderr, "vrpn_Tracker_Crossbow: Failed to recalibrate device\n");
}
}
int vrpn_SerialPort::read_available_characters(unsigned char * buffer, int count, struct timeval & timeout) {
requiresOpen();
int ret = vrpn_read_available_characters(_comm, buffer, count, &timeout);
if (ret == -1) {
throw ReadFailure();
}
return ret;
}
int vrpn_Tracker_Dyna::get_status()
{
int bytesRead;
unsigned char statusBuffer[256];
my_flush();
/* send request for status record */
vrpn_write_characters(serial_fd,(const unsigned char *) "\021", 1);
vrpn_drain_output_buffer(serial_fd);
vrpn_SleepMsecs(1000.0*2);
/* do non-blocking read of status record */
bytesRead = vrpn_read_available_characters(serial_fd, statusBuffer, 8);
// T_PDYN_STATUS_RECORD_LENGTH =8;
if ( bytesRead == 8 )
{
/* we have correct length- check a few chars to make sure this is a valid
* record
*/
if ( ((statusBuffer[0] & lOOO_OOOO) != lOOO_OOOO) ||
((statusBuffer[1] & lOOO_OOOO) != lOOO_OOOO) )
return(T_ERROR);
/* otherwise, all is well */
return(T_OK);
}
/* if we get here, we either got too much data or not enough */
/* no data means it's probably disconnected or not turned on */
if ( bytesRead == 0 )
{
// fprintf(stderr, "No data\n");
return(T_PDYN_NO_DATA);
}
/* if we got too much data, chances are that it's in continuous mode */
if ( bytesRead > 8)
{
fprintf(stderr, "3\n");
return(T_PDYN_SPEW_MODE);
}
/* if we get here, i have no idea what's going on- could be garbage on the
* serial line, wrong baud rate, or that the Dynasight is flaking out.
*/
return(T_ERROR);
} /* t_pdyn_get_status */
int vrpn_Tracker_Dyna::get_report(void) {
int ret;
if (status == vrpn_TRACKER_SYNCING) {
if ((ret=vrpn_read_available_characters(serial_fd, buffer, 1)) != 1 ||
(buffer[0] & llll_OOOO) != lOOO_OOOO) {
return 0;
}
vrpn_gettimeofday(×tamp, NULL);
status = vrpn_TRACKER_PARTIAL;
bufcount= ret;
}
if (status == vrpn_TRACKER_PARTIAL) {
ret=vrpn_read_available_characters(serial_fd, &(buffer[bufcount]),
reportLength-bufcount);
if (ret < 0) {
fprintf(stderr,"%s@%d: Error reading\n", __FILE__, __LINE__);
status = vrpn_TRACKER_FAIL;
return 0;
}
bufcount += ret;
if (bufcount < reportLength) { // Not done -- go back for more
return 0;
}
}
if (!valid_report()) {
//fprintf(stderr,"no valid report");
bufcount = 0;
status = vrpn_TRACKER_SYNCING;
return 0;
}
//else fprintf(stderr,"got valid report");
decode_record();
status = vrpn_TRACKER_SYNCING;
bufcount=0;
return 1;
}
int vrpn_read_available_characters(int comm, unsigned char *buffer,
size_t bytes, struct timeval *timeout)
{
#ifdef VERBOSE
printf("vrpn_read_available_characters(timeout): Entering\n");
#endif
struct timeval start, finish, now;
int sofar = 0, ret; // How many characters we have read so far
unsigned char *where = buffer;
// Find out what time it is at the start, and when we should end
// (unless the timeout is NULL)
if (timeout == NULL) {
// Set up so that it will never be that now > finish
// This prevents the while() loop below from stopping looping
vrpn_gettimeofday(&now, NULL);
memcpy(&finish, &now, sizeof(finish));
vrpn_gettimeofday(&finish, NULL);
}
else {
vrpn_gettimeofday(&start, NULL);
memcpy(&now, &start, sizeof(now));
time_add(start, *timeout, finish);
}
// Keep reading until we timeout. Exit from the middle of this by
// returning if we complete or find an error so that the loop only has
// to check for timeout, not other terminating conditions.
do {
ret = vrpn_read_available_characters(comm, where, bytes - sofar);
if (ret == -1) {
return -1;
}
sofar += ret;
if (sofar == bytes) {
break;
}
where += ret;
if (timeout != NULL) { // Update the time if we are checking timeout
vrpn_gettimeofday(&now, NULL);
}
} while (!(time_greater(now, finish)));
#ifdef VERBOSE
printf("vrpn_read_available_characters(timeout): Exiting\n");
#endif
return sofar;
}
void vrpn_Tracker_Crossbow::ping() {
unsigned char buffer = 'R';
struct timeval timeout;
vrpn_write_characters(serial_fd, &buffer, 1);
timeout.tv_sec = 1;
timeout.tv_usec = 0;
while (vrpn_read_available_characters(serial_fd, &buffer, 1, &timeout) == 1) {
if (buffer == 'H')
return;
}
fprintf(stderr, "vrpn_Tracker_Crossbow: Crossbow device not responding to ping\n");
}
int vrpn_GlobalHapticsOrb::reset(void)
{
struct timeval timeout;
unsigned char inbuf[1]; // Response from the Orb
int ret;
//-----------------------------------------------------------------------
// Set the values back to zero for all buttons, analogs and encoders
clear_values();
//-----------------------------------------------------------------------
// Clear the input buffer to make sure we're starting with a clean slate.
// Send the "reset" command to the box, then wait for a response and
// make sure it matches what we expect.
vrpn_flush_input_buffer(serial_fd);
vrpn_write_characters(serial_fd, &reset_char, 1);
timeout.tv_sec = 2;
timeout.tv_usec = 0;
ret = vrpn_read_available_characters(serial_fd, inbuf, 1, &timeout);
if (ret < 0) {
perror("vrpn_GlobalHapticsOrb::reset(): Error reading from Orb\n");
return -1;
}
if (ret == 0) {
send_text_message("vrpn_GlobalHapticsOrb::reset(): No response from Orb", d_timestamp, vrpn_TEXT_ERROR);
return -1;
}
if (inbuf[0] != 0xfc) {
char message[1024];
sprintf(message, "vrpn_GlobalHapticsOrb::reset(): Bad response from Orb (%02X)", inbuf[0]);
send_text_message(message, d_timestamp, vrpn_TEXT_ERROR);
return -1;
}
//-----------------------------------------------------------------------
// Figure out how many characters to expect in each report from the device,
// which is just 1 for the Orb.
d_expected_chars = 1;
vrpn_gettimeofday(&d_timestamp, NULL); // Set watchdog now
send_text_message("vrpn_GlobalHapticsOrb::reset(): Reset complete (this is good)", d_timestamp, vrpn_TEXT_ERROR);
// Set the mode to synchronizing
d_status = STATUS_SYNCING;
return 0;
}
/******************************************************************************
* NAME : vrpn_5dt::syncing
* ROLE : Send the "C" command to ask for new data from the glove
* ARGUMENTS : void
* RETURN : void
******************************************************************************/
void vrpn_5dt::syncing (void)
{
if (_wireless) {
// For a wireless glove, syncing means we got a header byte and need
// to wait for the end of the report to see if we guessed right and
// will get a capability byte.
int l_ret;
l_ret = vrpn_read_available_characters (serial_fd, &_buffer [_bufcount],
_expected_chars - _bufcount);
if (l_ret == -1) {
_5DT_ERROR ("Error reading the glove");
_status = STATUS_RESETTING;
return;
}
_bufcount += l_ret;
if (_bufcount < _expected_chars) { // Not done -- go back for more
return;
}
if (_buffer[_bufcount - 1] == 0x40 || _buffer[_bufcount - 1] == 0x01) {
_5DT_INFO ("Got capability byte as expected - switching into read mode.");
_bufcount = 0;
_status = STATUS_READING;
} else {
_5DT_WARNING ("Got a header byte, but capability byte not found - resetting.");
_status = STATUS_RESETTING;
}
return;
}
switch (_mode)
{
case 1:
send_command ((unsigned char *) "C", 1); // Command to query data from the glove
break;
case 2:
// Nothing to be done here -- continuous mode was requested in the reset.
break;
default :
_5DT_ERROR ("vrpn_5dt::syncing : internal error : unknown state");
printf ("mode %d\n", _mode);
break;
}
_bufcount = 0;
_status = STATUS_READING;
}
// Read a fully formed responses from the NDI tracker, (including the 4-byte CRC at the end)
// and copies it to latestResponseString.
// Returns the number of characters in the response (not including the CR),
// or -1 in case of an error
// NDI responses are all terminated with a CR, which this function replace with a end-of-string char.
//
// This function blocks until the CR has been received.
// FIXME: add a timeout parameter, and timeout if it's been too long
int vrpn_Tracker_NDI_Polaris::readResponse(){
//read in characters one-at-a-time until we come across a CR
int charIndex=0;
bool foundCR=false;
unsigned char c;
while (!foundCR) {
if (vrpn_read_available_characters(serialFd,&c,1)>0) {
latestResponseStr[charIndex]=c;
if (c=='\r') { //found CR
latestResponseStr[charIndex]='\0';
foundCR=true;
} else {
charIndex++;
}
}
}
return (charIndex);
}
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;
}
int vrpn_ImmersionBox::get_report(void)
{
unsigned char responseString[MAX_IBOX_STRING];
int i;
unsigned int buttonBits = 0;
struct timeval timeout;
timeout.tv_sec = 0;
timeout.tv_usec = 100000;
status = STATUS_SYNCING;
// process as long as we can get characters
while (1 == vrpn_read_available_characters(serial_fd, buffer, 1)) {
// if not a record start, skip it
if (buffer[0] != dataPacketHeader) {
continue;
}
// we got a good one... we're reading now
status = STATUS_READING;
break;
}
// we broke out.. if we're not reading, then we have nothing to do
if (STATUS_READING != status) {
return 0;
}
// we're reading now, get the report
// get the expected number of data record bytes
int result = vrpn_read_available_characters(serial_fd,
(unsigned char *) responseString,
dataRecordLength,
&timeout );
if (result < dataRecordLength) {
status = STATUS_SYNCING;
return 0;
}
// parse the report here
buttonBits = responseString[0];
for (i = 0; i < _numbuttons; i++) {
vrpn_Button::lastbuttons[i] = vrpn_Button::buttons[i];
vrpn_Button::buttons[i] = static_cast<unsigned char>(buttonBits & (1 << i));
}
#if VERBOSE
if (vrpn_Button::buttons[3])
cerr << "left button pressed " << endl;
#endif
// if we processed timer bits we would do so here
// here is where we decode the analog stuff
for (i = 0; i < _numchannels; i++) {
vrpn_Analog::last[i] = vrpn_Analog::channel[i];
}
// here is where we convert the angle encoders
for (i = 0; i < _numencoders; i++) {
vrpn_Dial::dials[i] = 0.0;
}
report_changes();
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
return 1;
}
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_Magellan::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
//--------------------------------------------------------------------
// If we're SYNCing, then the next character we get should be the start
// of a report. If we recognize it, go into READing mode and tell how
// many characters we expect total. If we don't recognize it, then we
// must have misinterpreted a command or something; reset the Magellan
// and start over
//--------------------------------------------------------------------
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;
}
switch (_buffer[0]) {
case 'k':
_expected_chars = 5; status = STATUS_READING; break;
case 'b':
_expected_chars = 2; status = STATUS_READING; break;
case 'm':
_expected_chars = 3; status = STATUS_READING; break;
case 'd':
_expected_chars = 26; status = STATUS_READING; break;
case 'n':
_expected_chars = 3; status = STATUS_READING; break;
case 'q':
_expected_chars = 4; status = STATUS_READING; break;
case 'z':
_expected_chars = 2; status = STATUS_READING; break;
case 'p':
_expected_chars = 4; status = STATUS_READING; break;
case 'c':
_expected_chars = 4; status = STATUS_READING; break;
default:
fprintf(stderr,"vrpn_Magellan: Unknown command (%c), resetting\n", _buffer[0]);
status = STATUS_RESETTING;
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.
//--------------------------------------------------------------------
ret = vrpn_read_available_characters(serial_fd, &_buffer[_bufcount],
_expected_chars-_bufcount);
if (ret == -1) {
send_text_message("vrpn_Magellan: 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
// last character '\r'
//--------------------------------------------------------------------
if (_buffer[_expected_chars-1] != '\r') {
status = STATUS_SYNCING;
send_text_message("vrpn_Magellan: No carriage return in record", timestamp, vrpn_TEXT_ERROR);
return 0;
}
#ifdef VERBOSE
printf("got a complete report (%d of %d)!\n", _bufcount, _expected_chars);
#endif
//--------------------------------------------------------------------
// Decode the report and store the values in it into the parent classes
// (analog or button) if appropriate.
//--------------------------------------------------------------------
switch ( _buffer[0] ) {
case 'k':
// This is a button command from the device. It gives us the state
// of each of the buttons on the device. Buttons 1-4 are encoded
// in the 4 LSBs of the first byte (key 1 in the LSB); buttons 5-8
// are in the 4 LSBs of the second byte; the * button (which we'll
// call button 0) is in the LSB of the third byte (the other 3 bits
// don't seem to be used).
buttons[0] = ( (_buffer[3] & 0x01) != 0);
buttons[1] = ( (_buffer[1] & 0x01) != 0);
buttons[2] = ( (_buffer[1] & 0x02) != 0);
buttons[3] = ( (_buffer[1] & 0x04) != 0);
buttons[4] = ( (_buffer[1] & 0x08) != 0);
buttons[5] = ( (_buffer[2] & 0x01) != 0);
buttons[6] = ( (_buffer[2] & 0x02) != 0);
buttons[7] = ( (_buffer[2] & 0x04) != 0);
buttons[8] = ( (_buffer[2] & 0x08) != 0);
break;
case 'b':
// Beep command received. We don't care.
break;
case 'm':
// Mode set command. We really only care that it is still in
// 3D mode (as opposed to Mouse mode); the other fields tell
// whether it is in dominant axis mode, and whether translations
// and rotations are being sent. We can handle any of these without
// incident.
if ( (_buffer[1] & 0x08) != 0) {
send_text_message("vrpn_Magellan: Was put into mouse mode, resetting", timestamp, vrpn_TEXT_ERROR);
status = STATUS_RESETTING;
return 1;
}
break;
case 'd':
// Axis data is being returned (telling what the X,Y,Z and A,B,C axes
// are currently set to). This data is put into the range [-1,1] and
// put into the analog channels (0=X, 1=Y, 2=Z, 3=A, 4=B, 5=C). It comes
// from the device with each axis packed into the lower nybble of 4
// consecutive bytes; the translation back to a signed 16-bit integer
// is done with (N0 * 4096) + (N1 * 256) + (N2 * 16) + (N3) - 32768
// for each value; this is then scaled to [-1,1].
nextchar = 1; // Skip the zeroeth character (the command)
for (i = 0; i < _numchannels; i++) {
long intval;
intval = (0x0f & _buffer[nextchar++]) << 12;
intval += (0x0f & _buffer[nextchar++]) << 8;
intval += (0x0f & _buffer[nextchar++]) << 4;
intval += (0x0f & _buffer[nextchar++]);
intval -= 32768;
// If the absolute value of the integer is <= the NULL radius, it should
// be set to zero.
if ( (intval <= _null_radius) && (intval >= - _null_radius) ) {
intval = 0;
}
// The largest values that seem to come out of the Magellan I've got
// even under the maximum acceleration are absolute value 7200 or so.
// We'll divide by 7500 to keep it safe.
double realval = intval / 7500.0;
channel[i] = realval;
}
break;
case 'n':
// NULL radius set. This is the number of ticks around zero that should
// count as zero, to allow a "dead zone" for the user near the center.
// We store this for the analog parsing code. The low nybble in the data
// word holds the new value
_null_radius = 0x0f & _buffer[1];
break;
case 'q':
// Sensitivity set. We don't care.
break;
case 'z':
// The device was zeroed. We don't care.
break;
case 'p':
// The min/max periods were set. We don't care.
break;
case 'c':
// Some extended command was sent. I hope we don't care.
// XXX Should check to make sure compression is not on.
break;
default:
fprintf(stderr,"vrpn_Magellan: Unknown [internal] command (%c), resetting\n", _buffer[0]);
status = STATUS_RESETTING;
return 1;
}
//--------------------------------------------------------------------
// Done with the decoding, send the reports and go back to syncing
//--------------------------------------------------------------------
report_changes();
status = STATUS_SYNCING;
_bufcount = 0;
return 1; // We got a full report.
}
int vrpn_Magellan::reset(void)
{
struct timeval timeout, now;
unsigned char inbuf[45];
const char *reset_str = "z\rm3\rc30\rnH\rbH\r"; // Reset string sent to box
const char *expect_back = "z\rm3\rc30\rnH\rb\r"; // What we expect back
int ret;
//-----------------------------------------------------------------------
// See if we should be using the alternative reset string.
// XXX The "expect_back" string here is almost certainly wrong. Waiting
// to hear back what the correct one should be.
if (_altreset) {
reset_str = "z\rm3\rnH\rp?0\rq00\r";
expect_back = "z\rm3\rnH\rp?0\rq00\r";
}
//-----------------------------------------------------------------------
// Set the values back to zero for all buttons, analogs and encoders
clear_values();
//-----------------------------------------------------------------------
// Send the list of commands to the device to cause it to reset and beep.
// Read back the response and make sure it matches what we expect.
// Give it a reasonable amount of time to finish, then timeout
vrpn_flush_input_buffer(serial_fd);
vrpn_write_slowly(serial_fd, (unsigned char *)reset_str, strlen(reset_str), 5);
timeout.tv_sec = 1;
timeout.tv_usec = 0;
ret = vrpn_read_available_characters(serial_fd, inbuf, strlen(expect_back), &timeout);
inbuf[strlen(expect_back)] = 0; // Make sure string is NULL-terminated
vrpn_gettimeofday(&now, NULL);
if (ret < 0) {
send_text_message("vrpn_Magellan reset: Error reading from device", now);
return -1;
}
if (ret == 0) {
send_text_message("vrpn_Magellan reset: No response from device", now);
return -1;
}
if (ret != (int)strlen(expect_back)) {
send_text_message("vrpn_Magellan reset: Got less than expected number of characters", now);
//,ret, strlen(expect_back));
return -1;
}
// Make sure the string we got back is what we expected
if ( strcmp((char *)inbuf, expect_back) != 0 ) {
send_text_message("vrpn_Magellan reset: Bad reset string", now);
//(want %s, got %s)\n", expect_back, inbuf);
return -1;
}
// The NULL radius is now set to 8
_null_radius = 8;
// We're now waiting for a response from the box
status = STATUS_SYNCING;
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
return 0;
}
int vrpn_BiosciencesTools::get_report(void)
{
int ret; // Return value from function call to be checked
//--------------------------------------------------------------------
// If we're SYNCing, then the next character we get should be the start
// of a report. If we recognize it, go into READing mode and tell how
// many characters we expect total. If we don't recognize it, then we
// must have misinterpreted a command or something; reset
// and start over
//--------------------------------------------------------------------
if (status == STATUS_SYNCING) {
// Try to get a character. If none, just return.
if (vrpn_read_available_characters(serial_fd, (unsigned char *)(d_buffer), 1) != 1) {
return 0;
}
d_expected_chars = 8;
// 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.
d_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.
//--------------------------------------------------------------------
ret = vrpn_read_available_characters(serial_fd, (unsigned char *)(&d_buffer[d_bufcount]),
d_expected_chars-d_bufcount);
if (ret == -1) {
DO_ERROR("Error reading");
status = STATUS_RESETTING;
return 0;
}
d_bufcount += ret;
#ifdef VERBOSE
if (ret != 0) printf("... got %d characters (%d total)\n",ret, d_bufcount);
#endif
if (d_bufcount < d_expected_chars) { // Not done -- go back for more
return 0;
}
d_buffer[d_expected_chars] = '\0'; // NULL terminate.
//--------------------------------------------------------------------
// 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.
// Store the report into the appropriate analog channel.
//--------------------------------------------------------------------
float value = convert_bytes_to_reading(d_buffer);
if (value == -1000) {
char msg[256];
sprintf(msg,"Invalid report, channel %d, resetting", d_next_channel_to_read);
DO_ERROR(msg);
status = STATUS_RESETTING;
}
channel[d_next_channel_to_read] = value;
#ifdef VERBOSE
printf("got a complete report (%d of %d)!\n", d_bufcount, d_expected_chars);
#endif
//--------------------------------------------------------------------
// Request a reading from the next channe.
//--------------------------------------------------------------------
d_next_channel_to_read = (d_next_channel_to_read + 1) % 6;
if (!request_temperature(d_next_channel_to_read)) {
char msg[256];
sprintf(msg,"Can't request reading, channel %d, resetting", d_next_channel_to_read);
DO_ERROR(msg);
status = STATUS_RESETTING;
}
//--------------------------------------------------------------------
// Done with the decoding, send the reports and go back to syncing
//--------------------------------------------------------------------
report_changes();
status = STATUS_SYNCING;
d_bufcount = 0;
return 1;
}
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
}
int vrpn_Tracker_3Space::get_report(void)
{
int ret;
// The reports are each 20 characters long, and each start with a
// byte that has the high bit set and no other bytes have the high
// bit set. If we're synching, read a byte at a time until we find
// one with the high bit set.
if (status == vrpn_TRACKER_SYNCING) {
// Try to get a character. If none, just return.
if (vrpn_read_available_characters(serial_fd, buffer, 1) != 1) {
return 0;
}
// If the high bit isn't set, we don't want it we
// need to look at the next one, so just return
if ( (buffer[0] & 0x80) == 0) {
send_text_message("Syncing (high bit not set)", timestamp, vrpn_TEXT_WARNING);
return 0;
}
// Got the first character of a report -- go into PARTIAL mode
// and say that we got one character at this time.
bufcount = 1;
vrpn_gettimeofday(×tamp, NULL);
status = vrpn_TRACKER_PARTIAL;
}
// Read as many bytes of this 20 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 tracker hasn't simply
// stopped sending characters).
ret = vrpn_read_available_characters(serial_fd, &buffer[bufcount],
20-bufcount);
if (ret == -1) {
send_text_message("Error reading, resetting", timestamp, vrpn_TEXT_ERROR);
status = vrpn_TRACKER_FAIL;
return 0;
}
bufcount += ret;
if (bufcount < 20) { // Not done -- go back for more
return 0;
}
{ // Decode the report
unsigned char decode[17];
int i;
static unsigned char mask[8] = {0x01, 0x02, 0x04, 0x08,
0x10, 0x20, 0x40, 0x80 };
// Clear the MSB in the first byte
buffer[0] &= 0x7F;
// Decode the 3Space binary representation into standard
// 8-bit bytes. This is done according to page 4-4 of the
// 3Space user's manual, which says that the high-order bits
// of each group of 7 bytes is packed into the 8th byte of the
// group. Decoding involves setting those bits in the bytes
// iff their encoded counterpart is set and then skipping the
// byte that holds the encoded bits.
// We decode from buffer[] into decode[] (which is 3 bytes
// shorter due to the removal of the bit-encoding bytes).
// decoding from buffer[0-6] into decode[0-6]
for (i=0; i<7; i++) {
decode[i] = buffer[i];
if ( (buffer[7] & mask[i]) != 0) {
decode[i] |= (unsigned char)(0x80);
}
}
// decoding from buffer[8-14] into decode[7-13]
for (i=7; i<14; i++) {
decode[i] = buffer[i+1];
if ( (buffer[15] & mask[i-7]) != 0) {
decode[i] |= (unsigned char)(0x80);
}
}
// decoding from buffer[16-18] into decode[14-16]
for (i=14; i<17; i++) {
decode[i] = buffer[i+2];
if ( (buffer[19] & mask[i-14]) != 0) {
decode[i] |= (unsigned char)(0x80);
}
}
// Parse out sensor number, which is the second byte and is
// stored as the ASCII number of the sensor, with numbers
// starting from '1'. We turn it into a zero-based unit number.
d_sensor = decode[1] - '1';
// Position
for (i=0; i<3; i++) {
pos[i] = (* (short*)(&decode[3+2*i])) * T_3_BINARY_TO_METERS;
}
// Quarternion orientation. The 3Space gives quaternions
// as w,x,y,z while the VR code handles them as x,y,z,w,
// so we need to switch the order when decoding. Also the
// tracker does not normalize the quaternions.
d_quat[3] = (* (short*)(&decode[9]));
for (i=0; i<3; i++) {
d_quat[i] = (* (short*)(&decode[11+2*i]));
}
//Normalize quaternion
double norm = sqrt ( d_quat[0]*d_quat[0] + d_quat[1]*d_quat[1]
+ d_quat[2]*d_quat[2] + d_quat[3]*d_quat[3]);
for (i=0; i<4; i++) {
d_quat[i] /= norm;
}
// Done with the decoding, set the report to ready
// Ready for another report
status = vrpn_TRACKER_SYNCING;
bufcount = 0;
}
return 1; // Got a report.
#ifdef VERBOSE
print_latest_report();
#endif
}
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
}
int vrpn_Tracker_Isotrak::get_report(void)
{
char errmsg[512]; // Error message to send to VRPN
int ret; // Return value from function call to be checked
// The first byte of a binary record has the high order bit set
if (status == vrpn_TRACKER_SYNCING) {
// Try to get a character. If none, just return.
if (vrpn_read_available_characters(serial_fd, buffer, 1) != 1) {
return 0;
}
// The first byte of a record has the high order bit set
if(!(buffer[0] & 0x80)) {
sprintf(errmsg,"While syncing (looking for byte with high order bit set, "
"got '%x')", buffer[0]);
VRPN_MSG_WARNING(errmsg);
vrpn_flush_input_buffer(serial_fd);
return 0;
}
// Got the first byte of a report -- go into TRACKER_PARTIAL mode
// and record that we got one character at this time.
bufcount = 1;
vrpn_gettimeofday(×tamp, NULL);
status = vrpn_TRACKER_PARTIAL;
}
//--------------------------------------------------------------------
// 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 tracker hasn't simply
// stopped sending characters).
//--------------------------------------------------------------------
ret = vrpn_read_available_characters(serial_fd, &buffer[bufcount],
BINARY_RECORD_SIZE - bufcount);
if (ret == -1) {
VRPN_MSG_ERROR("Error reading report");
status = vrpn_TRACKER_FAIL;
return 0;
}
bufcount += ret;
if (bufcount < BINARY_RECORD_SIZE) { // Not done -- go back for more
return 0;
}
// We now have enough characters for a full report
// Check it to ensure we do not have a high bit set other
// than on the first byte
for(int i=1; i<BINARY_RECORD_SIZE; i++)
{
if (buffer[i] & 0x80) {
status = vrpn_TRACKER_SYNCING;
sprintf(errmsg,"Unexpected sync character in record");
VRPN_MSG_WARNING(errmsg);
//VRPN_MSG_WARNING("Not '0' in record, re-syncing");
vrpn_flush_input_buffer(serial_fd);
return 0;
}
}
// Create a buffer for the decoded message
unsigned char decoded[BINARY_RECORD_SIZE];
int d = 0;
int fullgroups = BINARY_RECORD_SIZE / 8;
// The following decodes the Isotrak binary format. It consists of
// 7 byte values plus an extra byte of the high bit for these
// 7 bytes. First, loop over the 7 byte ranges (8 bytes in binary)
int i;
for(i = 0; i<fullgroups; i++)
{
vrpn_uint8 *group = &buffer[i * 8];
vrpn_uint8 high = buffer[i * 8 + 7];
for(int j=0; j<7; j++)
{
decoded[d] = *group++;
if(high & 1)
decoded[d] |= 0x80;
d++;
high >>= 1;
}
}
// We'll have X bytes left at the end
int left = BINARY_RECORD_SIZE - fullgroups * 8;
vrpn_uint8 *group = &buffer[fullgroups * 8];
vrpn_uint8 high = buffer[fullgroups * 8 + left - 1];
for(int j=0; j<left-1; j++)
{
decoded[d] = *group++;
if(high & 1)
decoded[d] |= 0x80;
d++;
high >>= 1;
}
// ASCII value of 1 == 49 subtracing 49 gives the sensor number
d_sensor = decoded[1] - 49; // Convert ASCII 1 to sensor 0 and so on.
if ( (d_sensor < 0) || (d_sensor >= num_stations) ) {
status = vrpn_TRACKER_SYNCING;
sprintf(errmsg,"Bad sensor # (%d) in record, re-syncing", d_sensor);
VRPN_MSG_WARNING(errmsg);
vrpn_flush_input_buffer(serial_fd);
return 0;
}
// Extract the important information
vrpn_uint8 *item = &decoded[3];
// This is a scale factor from the Isotrak manual
// This will convert the values to meters, the standard vrpn format
double mul = 1.6632 / 32767.;
float div = 1.f / 32767.f; // Fractional amount for angles
pos[0] = ( (vrpn_int8(item[1]) << 8) + item[0]) * mul; item += 2;
pos[1] = ( (vrpn_int8(item[1]) << 8) + item[0]) * mul; item += 2;
pos[2] = ( (vrpn_int8(item[1]) << 8) + item[0]) * mul; item += 2;
d_quat[3] = ( (vrpn_int8(item[1]) << 8) + item[0]) * div; item += 2;
d_quat[0] = ( (vrpn_int8(item[1]) << 8) + item[0]) * div; item += 2;
d_quat[1] = ( (vrpn_int8(item[1]) << 8) + item[0]) * div; item += 2;
d_quat[2] = ( (vrpn_int8(item[1]) << 8) + item[0]) * div;
//--------------------------------------------------------------------
// If this sensor has button on it, decode the button values
// into the button device and mainloop the button device so that
// it will report any changes.
//--------------------------------------------------------------------
if(stylus_buttons[d_sensor] != NULL)
{
char button = decoded[2];
if(button == '@' || button == '*')
{
stylus_buttons[d_sensor]->set_button(0, button == '@');
}
stylus_buttons[d_sensor]->mainloop();
}
//--------------------------------------------------------------------
// Done with the decoding,
// set the report to ready
//--------------------------------------------------------------------
status = vrpn_TRACKER_SYNCING;
bufcount = 0;
#ifdef VERBOSE2
print_latest_report();
#endif
return 1;
}
// 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;
}
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;
}
// Fill in the buttons[] array with the current value of each of the
// buttons For a description of the protocols for a Microsoft 3button
// mouse and a MouseSystems mouse, see http://www.hut.fi/~then/mytexts/mouse.html
void vrpn_Button_SerialMouse::read(void)
{
// Make sure we're ready to read
if (status != BUTTON_READY) {
return;
}
unsigned char buffer;
// process as long as we can get characters
int num = 1;
int debounce = 0;
while (num)
{
num = vrpn_read_available_characters(serial_fd, &buffer, 1);
if (num <= 0) {
if (debounce) {
#ifdef VERBOSE
fprintf (stderr,"state: %d %d %d last: %d %d %d\n",
buttons[0],buttons[1],buttons[2],
lastL, lastM, lastR);
#endif
lastL = buttons[0];
lastM = buttons[1];
lastR = buttons[2];
}
return; // nothing there or error, so return
}
switch (mousetype) {
case THREEBUTTON_EMULATION:
// a mouse capable of 3 button emulation
// this mouse encodes its buttons in a byte that is one of
// 0xc0 0xd0 0xe0 0xf0.
// Throw away all bytes that are not one of C0, D0, E0 or F0.
if ( (buffer != 0xc0) && (buffer != 0xd0) &&
(buffer != 0xe0) && (buffer != 0xf0) ) {
continue;
}
buttons[0] = (unsigned char)( (buffer & 0x20)?1:0 );
buttons[2] = (unsigned char)( (buffer & 0x10)?1:0 );
// middle button check:: we get here without a change in left or right
// This means that we toggle the middle button by moving the mouse
// around while not pressing or releasing the other buttons!
if ((buttons[0] == lastL) && (buttons[2] == lastR) && !debounce) {
buttons[1] = (unsigned char)( lastM?0:1 );
}
debounce = 1;
break;
case MOUSESYSTEMS:
// mousesystems (real PC 3 button mouse) protocol
// The pc three button mouse encodes its buttons in a byte
// that looks like 1 0 0 0 0 lb mb rb
if ((buffer & 0xf8) != 0x80) { // Ignore all bytes but first in record
continue;
}
debounce = 1;
buttons[0] = (unsigned char)( (buffer & 4)?0:1 );
buttons[1] = (unsigned char)( (buffer & 2)?0:1 );
buttons[2] = (unsigned char)( (buffer & 1)?0:1 );
break;
default:
printf("vrpn_Button_SerialMouse::read(): Unknown mouse type\n");
break;
} // switch
} // while (num)
}
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
}
/******************************************************************************
* NAME : vrpn_5dt::get_report
* ROLE : This function will read characters until it has a full report, then
* put that report into analog fields and call the report methods on these.
* ARGUMENTS : void
* RETURN : void
******************************************************************************/
void vrpn_5dt::get_report (void)
{
int l_ret; // Return value from function call to be checked
// XXX This should be called when the first character of a report is read.
vrpn_gettimeofday(×tamp, NULL);
//--------------------------------------------------------------------
// 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.
//--------------------------------------------------------------------
l_ret = vrpn_read_available_characters (serial_fd, &_buffer [_bufcount],
_expected_chars - _bufcount);
if (l_ret == -1) {
_5DT_ERROR ("Error reading the glove");
_status = STATUS_RESETTING;
return;
}
#ifdef VERBOSE
if (l_ret != 0) printf("... got %d characters (%d total)\n",l_ret, _bufcount);
#endif
//--------------------------------------------------------------------
// The time of the report is the time at which the first character for
// the report is read.
//--------------------------------------------------------------------
if ( (l_ret > 0) && (_bufcount == 0) ) {
vrpn_gettimeofday(×tamp, NULL);
}
//--------------------------------------------------------------------
// We keep track of how many characters we have received and keep
// going back until we get as many as we expect.
//--------------------------------------------------------------------
_bufcount += l_ret;
if (_bufcount < _expected_chars) { // Not done -- go back for more
return;
}
//--------------------------------------------------------------------
// We now have enough characters to make a full report. First check to
// make sure that the first one is what we expect.
if (_buffer[0] != 128) {
_5DT_WARNING ("Unexpected first character in report, resetting");
_status = STATUS_RESETTING;
_bufcount = 0;
return;
}
if (_wireless) {
if (_buffer[_bufcount - 1] != 0x40 && _buffer[_bufcount - 1] != 0x01) {
// The last byte wasn't a capability byte, so this report is invalid.
// Reset!
_5DT_WARNING ("Unexpected last character in report, resetting");
_status = STATUS_RESETTING;
_bufcount = 0;
return;
}
}
#ifdef VERBOSE
printf ("Got a complete report (%d of %d)!\n", _bufcount, _expected_chars);
#endif
//--------------------------------------------------------------------
// Decode the report and store the values in it into the analog values
// if appropriate.
//--------------------------------------------------------------------
channel[1] = _buffer[1] / 255.0; //Thumb
channel[2] = _buffer[2] / 255.0;
channel[3] = _buffer[3] / 255.0;
channel[4] = _buffer[4] / 255.0;
channel[5] = _buffer[5] / 255.0; // Pinkie
channel[6] = 180 * _buffer[6] / 255.0; // Pitch
channel[7] = 180 * _buffer[7] / 255.0; // Roll
if (_wireless && !_gotInfo) {
_gotInfo = true;
// Bit 0 set in the capability byte implies a right-hand glove.
if (_buffer[9] == 0x01) {
_5DT_INFO ("A 'wireless-type' right glove is ready and reporting");
} else {
_5DT_INFO ("A 'wireless-type' left glove is ready and reporting");
}
}
//--------------------------------------------------------------------
// Done with the decoding, send the reports and go back to syncing
//--------------------------------------------------------------------
report_changes();
switch (_mode) {
case 1:
_status = STATUS_SYNCING;
break;
case 2: // Streaming Mode, just go back for the next report.
_bufcount = 0;
break;
default :
_5DT_ERROR ("vrpn_5dt::get_report : internal error : unknown state");
break;
}
}
int vrpn_Tracker_Liberty::get_report(void)
{
char errmsg[512]; // Error message to send to VRPN
int ret; // Return value from function call to be checked
unsigned char *bufptr; // Points into buffer at the current value to read
//--------------------------------------------------------------------
// Each report starts with the ASCII 'LY' characters. If we're synching,
// read a byte at a time until we find a 'LY' characters.
//--------------------------------------------------------------------
// For the Patriot this is 'PA'.
// For the (high speed) Liberty Latus this is 'LU'.
if (status == vrpn_TRACKER_SYNCING) {
// Try to get the first sync character if don't already have it.
// If none, just return.
if (got_single_sync_char != 1) {
ret = vrpn_read_available_characters(serial_fd, buffer, 1);
if (ret != 1) {
//if (VRPN_LIBERTY_DEBUG) fprintf(stderr,"[DEBUG]: Missed First Sync Char, ret= %i\n",ret);
return 0;
}
}
// Try to get the second sync character. If none, just return
ret = vrpn_read_available_characters(serial_fd, &buffer[1], 1);
if (ret == 1) {
//Got second sync Char
got_single_sync_char = 0;
}
else if (ret != -1) {
if (VRPN_LIBERTY_DEBUG) fprintf(stderr,"[DEBUG]: Missed Second Sync Char\n");
got_single_sync_char = 1;
return 0;
}
// If it is not 'LY' or 'PA' or 'LU' , 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.
// Also, flush the buffer so that it won't take as long to catch up.
if (
((( buffer[0] == 'L') && (buffer[1] == 'Y')) != 1)
&&
((( buffer[0] == 'P') && (buffer[1] == 'A')) != 1)
&&
((( buffer[0] == 'L') && (buffer[1] == 'U')) != 1)
)
{
sprintf(errmsg,"While syncing (looking for 'LY' or 'PA' or 'LU', "
"got '%c%c')", buffer[0], buffer[1]);
VRPN_MSG_INFO(errmsg);
vrpn_flush_input_buffer(serial_fd);
if (VRPN_LIBERTY_DEBUG) fprintf(stderr,"[DEBUGA]: Getting Report - Not LY or PA or LU, Got Character %c %c \n",buffer[0],buffer[1]);
return 0;
}
if (VRPN_LIBERTY_DEBUG) fprintf(stderr,"[DEBUG]: Getting Report - Got LY or PA or LU\n");
// Got the first character of a report -- go into AWAITING_STATION mode
// and record that we got one character at this time. The next
// bit of code will attempt to read the station.
// The time stored here is as close as possible to when the
// report was generated. For the InterSense 900 in timestamp
// mode, this value will be overwritten later.
bufcount = 2;
// vrpn_gettimeofday(×tamp, NULL);
status = vrpn_TRACKER_AWAITING_STATION;
}
//--------------------------------------------------------------------
// The third character of each report is the station number. Once
// we know this, we can compute how long the report should be for the
// given station, based on what values are in its report.
// The station number is converted into a VRPN sensor number, where
// the first Liberty station is '1' and the first VRPN sensor is 0.
//--------------------------------------------------------------------
if (status == vrpn_TRACKER_AWAITING_STATION) {
// Try to get a character. If none, just return.
if (vrpn_read_available_characters(serial_fd, &buffer[bufcount], 1) != 1) {
return 0;
}
if (VRPN_LIBERTY_DEBUG) fprintf(stderr,"[DEBUG]: Awaiting Station - Got Station (%i) \n",buffer[2]);
d_sensor = buffer[2] - 1; // Convert ASCII 1 to sensor 0 and so on.
if ( (d_sensor < 0) || (d_sensor >= num_stations) ) {
status = vrpn_TRACKER_SYNCING;
sprintf(errmsg,"Bad sensor # (%d) in record, re-syncing", d_sensor);
VRPN_MSG_INFO(errmsg);
vrpn_flush_input_buffer(serial_fd);
return 0;
}
// Figure out how long the current report should be based on the
// settings for this sensor.
REPORT_LEN = report_length(d_sensor);
// Got the station report -- to into PARTIAL 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.
bufcount++;
status = vrpn_TRACKER_PARTIAL;
}
//--------------------------------------------------------------------
// 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 tracker hasn't simply
// stopped sending characters).
//--------------------------------------------------------------------
ret = vrpn_read_available_characters(serial_fd, &buffer[bufcount],
REPORT_LEN-bufcount);
if (ret == -1) {
if (VRPN_LIBERTY_DEBUGA) fprintf(stderr,"[DEBUG]: Error Reading Report\n");
VRPN_MSG_ERROR("Error reading report");
status = vrpn_TRACKER_FAIL;
return 0;
}
bufcount += ret;
if (bufcount < REPORT_LEN) { // Not done -- go back for more
if (VRPN_LIBERTY_DEBUG) fprintf(stderr,"[DEBUG]: Don't have full report (%i of %i)\n",bufcount,REPORT_LEN);
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 '0', the next character is the ASCII station
// number, and the third character is either a space or a letter.
//--------------------------------------------------------------------
// fprintf(stderr,"[DEBUG]: Got full report\n");
if (
((buffer[0] != 'L') || (buffer[1] != 'Y'))
&&
((buffer[0] != 'P') || (buffer[1] != 'A'))
&&
((buffer[0] != 'L') || (buffer[1] != 'U'))
) {
if (VRPN_LIBERTY_DEBUGA) fprintf(stderr,"[DEBUG]: Don't have LY or PA or 'LU' at beginning");
status = vrpn_TRACKER_SYNCING;
VRPN_MSG_INFO("Not 'LY' or 'PA' or 'LU' in record, re-syncing");
vrpn_flush_input_buffer(serial_fd);
return 0;
}
if (buffer[bufcount-1] != ' ') {
status = vrpn_TRACKER_SYNCING;
VRPN_MSG_INFO("No space character at end of report, re-syncing\n");
vrpn_flush_input_buffer(serial_fd);
if (VRPN_LIBERTY_DEBUGA) fprintf(stderr,"[DEBUG]: Don't have space at end of report, got (%c) sensor %i\n",buffer[bufcount-1], d_sensor);
return 0;
}
//Decode the error status and output a debug message
if (buffer[4] != ' ') {
// An error has been flagged
if (VRPN_LIBERTY_DEBUGA) fprintf(stderr,"[DEBUG]:Error Flag %i\n",buffer[4]);
}
//--------------------------------------------------------------------
// Decode the X,Y,Z of the position and the W,X,Y,Z of the quaternion
// (keeping in mind that we store quaternions as X,Y,Z, W).
//--------------------------------------------------------------------
// The reports coming from the Liberty are in little-endian order,
// which is the opposite of the network-standard byte order that is
// used by VRPN. Here we swap the order to big-endian so that the
// routines below can pull out the values in the correct order.
// This is slightly inefficient on machines that have little-endian
// order to start with, since it means swapping the values twice, but
// that is more than outweighed by the cleanliness gained by keeping
// all architecture-dependent code in the vrpn_Shared.C file.
//--------------------------------------------------------------------
// Point at the first value in the buffer (position of the X value)
bufptr = &buffer[8];
// When copying the positions, convert from inches to meters, since the
// Liberty reports in inches and VRPN reports in meters.
pos[0] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr) * INCHES_TO_METERS;
pos[1] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr) * INCHES_TO_METERS;
pos[2] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr) * INCHES_TO_METERS;
// Change the order of the quaternion fields to match quatlib order
d_quat[Q_W] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
d_quat[Q_X] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
d_quat[Q_Y] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
d_quat[Q_Z] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
//--------------------------------------------------------------------
// Decode the time from the Liberty system (unsigned 32bit int), add it to the
// time we zeroed the tracker, and update the report time. Remember
// to convert the MILLIseconds from the report into MICROseconds and
// seconds.
//--------------------------------------------------------------------
struct timeval delta_time; // Time since the clock was reset
// Read the integer value of the time from the record.
vrpn_uint32 read_time = vrpn_unbuffer_from_little_endian<vrpn_uint32>(bufptr);
// Convert from the float in MILLIseconds to the struct timeval
delta_time.tv_sec = (long)(read_time / 1000); // Integer trunction to seconds
vrpn_uint32 read_time_milliseconds = read_time - delta_time.tv_sec * 1000; // Subtract out what we just counted
delta_time.tv_usec = (long)(read_time_milliseconds * 1000); // Convert remainder to MICROseconds
// The time that the report was generated
timestamp = vrpn_TimevalSum(liberty_zerotime, delta_time);
vrpn_gettimeofday(&watchdog_timestamp, NULL); // Set watchdog now
//--------------------------------------------------------------------
// If this sensor has button on it, decode the button values
// into the button device and mainloop the button device so that
// it will report any changes.
//--------------------------------------------------------------------
if (stylus_buttons[d_sensor]) {
// Read the integer value of the bytton status from the record.
vrpn_uint32 button_status = vrpn_unbuffer_from_little_endian<vrpn_uint32>(bufptr);
stylus_buttons[d_sensor]->set_button(0, button_status);
stylus_buttons[d_sensor]->mainloop();
}
//--------------------------------------------------------------------
// Done with the decoding, set the report to ready
//--------------------------------------------------------------------
status = vrpn_TRACKER_SYNCING;
bufcount = 0;
#ifdef VERBOSE2
print_latest_report();
#endif
return 1;
}
int vrpn_inertiamouse::get_report(void)
{
int ret; // Return value from function call to be checked
//
// If we're SYNCing, then the next character we get should be the
// start of a report. If we recognize it, go into READing mode
// and tell how many characters we expect total. If we don't
// recognize it, then we must have misinterpreted a command or
// something; reset the intertiamouse and start over
//
if (status_ == STATUS_SYNCING) {
// Try to get a character. If none, just return.
ret = vrpn_read_available_characters (serial_fd, &buffer_[0], 1);
if (ret != 1) { return 0; }
switch (buffer_[0]) {
case 'D':
expected_chars_ = 25;
break;
case 'B':
expected_chars_ = 4;
break;
default:
fprintf(stderr, "vrpn_inertiamouse: Unknown command (%c), resetting\n", buffer_[0]);
status_ = STATUS_RESETTING;
return 0;
}
bufcount_ = 1;
vrpn_gettimeofday (×tamp, NULL);
status_ = STATUS_READING;
}
ret = vrpn_read_available_characters(serial_fd,
&buffer_[bufcount_],
expected_chars_ - bufcount_);
if (ret == -1) {
send_text_message("vrpn_inertiamouse: Error reading",
timestamp,
vrpn_TEXT_ERROR);
status_ = STATUS_RESETTING;
return 0;
}
bufcount_ += ret;
if (bufcount_ < expected_chars_) { // Not done -- go back for more
return 0;
}
if (buffer_[expected_chars_ - 1] != '\n') {
status_ = STATUS_SYNCING;
send_text_message("vrpn_inertiamouse: No newline in record",
timestamp,
vrpn_TEXT_ERROR);
return 0;
}
switch ( buffer_[0] ) {
case 'D':
{
int i;
int nextchar;
for (i = 0, nextchar = 0; i < numchannels_; ++i) {
int packet;
packet = (buffer_[++nextchar] & 0xf0) << 8;
packet |= (buffer_[++nextchar] & 0xf0) << 4;
packet |= (buffer_[++nextchar] & 0xf0);
packet |= (buffer_[++nextchar] & 0xf0) >> 4;
int chnl = (packet >> 10) & 7;
if (chnl >= Channels) {
status_ = STATUS_SYNCING;
send_text_message("vrpn_inertiamouse: Too-large channel value",
timestamp,
vrpn_TEXT_ERROR);
return 0;
}
int acc = packet & 0x3ff; // 10 bits
// normalize to interval [-1,1]
// just a guess, block dc later
double normval = ((double)(acc - 256) /
(double)256);
// normval *= 1.5;
// update rotation data
if ( (chnl == 4) || (chnl == 5) ) {
channel[chnl] = normval;
break;
}
normval = dcb_[chnl].filter (normval);
normval = lp_[chnl].filter (normval);
double dt = 0.25;
// update velocity and position only when button[0] pressed
if (buttons[0]) {
double pos = vel_[chnl] * dt + normval * dt * dt / 2;
vel_[chnl] += normval*dt;
if(fabs (vel_[chnl]) < dt/2.0)
vel_[chnl] *= 0.90;
// else
// if (fabs (vel_[chnl]) > 1.0)
// vel_[chnl] *= 1.0 / fabs (vel_[chnl]);
channel[chnl] = pos;
// channel[chnl] *= 0.95;
} else {
vel_[chnl] = 0.0;
channel[chnl] = 0.0;
}
}
}
break;
case 'B':
buttons[0] = ((buffer_[1] & 1) != 0);
buttons[1] = ((buffer_[1] & 2) != 0);
break;
default:
fprintf(stderr, "vrpn_inertiamouse: Unknown [internal] command (%c), resetting\n", buffer_[0]);
status_ = STATUS_RESETTING;
return 0;
}
//
// Done with the decoding, send the reports and go back to syncing
//
report_changes();
status_ = STATUS_SYNCING;
bufcount_ = 0;
return 1; // We got a full report.
}
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
}
int vrpn_Spaceball::get_report(void)
{
unsigned char rawbuf[1024]; // raw unprocessed incoming characters
int i, num, packs;
#if defined(DEBUG)
int j;
#endif
packs = 0; /* no packs received yet */
// read up to 1023 unprocessed characters from the serial device at once
num = vrpn_read_available_characters(serial_fd, rawbuf, 1023);
// if we receive 1 or more chars, we will see if this completes any
// pending packets we're trying to process.
if (num > 0) {
for (i=0; i<num; i++) {
/* process potentially occurring escaped character sequences */
if (rawbuf[i] == '^') {
if (!escapedchar) {
escapedchar = 1;
continue; /* eat the escape character from buffer */
}
}
/* if in escaped mode, we convert the escaped char to final form */
if (escapedchar) {
escapedchar = 0; /* we're back out of escape mode after this */
switch (rawbuf[i]) {
case '^': /* leave char in buffer unchanged */
break;
case 'Q':
case 'S':
case 'M':
rawbuf[i] &= 0x1F; /* convert character to unescaped form */
break;
default:
#if defined(DEBUG)
printf("\nGot a bad escape sequence! 0x%02x", rawbuf[i]);
if (isprint(rawbuf[i]))
printf(" (%c)", rawbuf[i]);
else
printf(" (unprintable)");
printf("\n");
#endif
break;
}
}
/* figure out what kind of packet we received */
if (bufpos == 0) {
status = STATUS_SYNCING; /* update our status */
switch(rawbuf[i]) {
case 'D': /* Displacement packet */
packtype = 'D';
packlen = 16; /* D packets are 15 bytes long */
break;
case 'K': /* Button/Key packet */
packtype = 'K';
packlen = 4; /* K packets are 3 bytes long */
break;
case '.': /* Spaceball 4000 FLX "advanced" button press event */
packtype = '.';
packlen = 4; /* . packets are 3 bytes long */
break;
case 'C': /* Communications mode packet */
packtype = 'C';
packlen = 4;
break;
case 'F': /* Spaceball sensitization mode packet */
packtype = 'F';
packlen = 4;
break;
case 'M': /* Movement mode packet */
packtype = 'M';
packlen = 5;
break;
case 'N': /* Null region packet */
packtype = 'N';
packlen = 3;
break;
case 'P': /* Update rate packet */
packtype = 'P';
packlen = 6;
break;
case '\v': /* XON at poweron */
packtype = '\v';
packlen = 1;
break;
case '\n': /* carriage return at poweron */
case '\r': /* carriage return at poweron */
packtype = '\r';
packlen = 1;
break;
case '@': /* Spaceball Hard/Soft Reset packet */
resetoccured=1;
packtype = '@';
packlen = 62; /* Resets aren't longer than 62 chars */
break;
case 'E': /* Error packet */
packtype = 'E';
packlen = 8; /* E packets are up to 7 bytes long */
break;
case 'Z': /* Zero packet (Spaceball 2003/3003/4000 FLX) */
packtype = 'Z';
packlen = 14; /* Z packets are hardware dependent */
break;
default: /* Unknown packet! */
#if defined(DEBUG)
printf("\nUnknown packet (1) [%d]: 0x%02x \n ", i, rawbuf[i]);
printf(" char: ");
if (isprint(rawbuf[i]))
printf("%c", rawbuf[i]);
else
printf(" (unprintable)");
printf("\n");
#endif
continue;
}
}
buf[bufpos] = rawbuf[i]; /* copy processed chars into long-term buffer */
bufpos++; /* go to next buffer slot */
/* Reset packet processing */
if (packtype == '@') {
if (rawbuf[i] != '\r')
continue;
else
packlen = bufpos;
}
/* Error packet processing */
if (packtype == 'E') {
if (rawbuf[i] != '\r')
continue;
else
packlen = bufpos;
} else if (bufpos != packlen)
continue;
status = STATUS_READING; // ready to process event packet
vrpn_gettimeofday(×tamp, NULL); // set timestamp of this event
switch (packtype) {
case 'D': /* ball displacement event */
{
int nextchar, chan;
/* number of 1/16ths of milliseconds since last */
/* ball displacement packet */
nextchar = 1; // this is where the timer data is, if we want it.
nextchar = 3; // Skip the zeroeth character (the command)
for (chan = 0; chan < _numchannels; chan++) {
vrpn_int16 intval;
intval = (buf[nextchar++]) << 8;
intval |= (buf[nextchar++]);
// If the absolute value of the integer is <= the NULL
// radius, it should be set to zero.
if ( (intval <= null_radius) && (intval >= - null_radius) ) {
intval = 0;
}
// maximum possible values per axis are +/- 32768, although the
// largest value I've ever observed among several devices
// is only 20,000. For now, we'll divide by 32768, and if this is
// too insensitive, we can do something better later.
double realval = intval / 32768.0;
channel[chan] = realval;
// printf("XXX Channel[%d] = %f %d \n", z, realval, intval);
}
channel[2] = -channel[2]; // Negate Z translation
channel[5] = -channel[5]; // Negate Z rotation
}
break;
case 'K': /* button press event */
/* Spaceball 2003A, 2003B, 2003 FLX, 3003 FLX, 4000 FLX */
/* button packet. (4000 only for backwards compatibility) */
/* The lowest 5 bits of the first byte are buttons 5-9 */
/* Button '8' on a Spaceball 2003 is the rezero button */
/* The lowest 4 bits of the second byte are buttons 1-4 */
/* For Spaceball 2003, we'll map the buttons 1-7 normally */
/* skip 8, as its a hardware "rezero button" on that device */
/* and call the "pick" button "8". */
/* On the Spaceball 3003, the "right" button also triggers */
/* the "pick" bit. We OR the 2003/3003 rezero bits together */
/* if we have found a Spaceball 4000, then we ignore the 'K' */
/* packets entirely, and only use the '.' packets. */
if (spaceball4000)
break;
// XXX my original libsball button decoding code, for reference
// buttons =
// ((buf[1] & 0x10) << 3) | /* 2003 pick button is "8" */
// ((buf[1] & 0x20) << 9) | /* 3003 rezero button */
// ((buf[1] & 0x08) << 11) | /* 2003 rezero button */
// ((buf[1] & 0x07) << 4) | /* 5,6,7 (2003/4000) */
// ((buf[2] & 0x30) << 8) | /* 3003 Left/Right buttons */
// ((buf[2] & 0x0F)); /* 1,2,3,4 (2003/4000) */
// Mapping the buttons is tricky since different models
// of the Spaceball have different button sets.
// 2003 has 9 buttons (rezero included)
// 3003 has 3 buttons (rezero included)
// 4000 has 12 buttons, and can be in "lefty" or "righty" mode.
// We'll skip reporting lefty/righty mode for now though.
// Spaceball 2003/4000 buttons 1-4 mapped to slots 0-3
// Spaceball 3003 L/R buttons are mapped as slots 0/1
buttons[0] = static_cast<unsigned char>(((buf[2] & 0x01) != 0) | ((buf[2] & 0x10) != 0));
buttons[1] = static_cast<unsigned char>(((buf[2] & 0x02) != 0) | ((buf[2] & 0x20) != 0));
buttons[2] = static_cast<unsigned char>(((buf[2] & 0x04) != 0));
buttons[3] = static_cast<unsigned char>(((buf[2] & 0x08) != 0));
// Spaceball 2003/4000 buttons 5,6,7 mapped to slots 4-6
buttons[4] = static_cast<unsigned char>(((buf[1] & 0x01) != 0));
buttons[5] = static_cast<unsigned char>(((buf[1] & 0x02) != 0));
buttons[6] = static_cast<unsigned char>(((buf[1] & 0x04) != 0));
// Spaceball 2003/3003 rezero buttons are mapped to slot 7
// The rezero button's function is sometimes hard-wired,
// and may or may not be used by applications, this is up
// in the air still. For now, I'll map it to slot 7 which
// keeps the numbering in a sane order on the 2003 device anyway.
buttons[7] = static_cast<unsigned char>(((buf[1] & 0x20) != 0) | ((buf[1] & 0x08) != 0));
// Spaceball 2003 pick button mapped to slot 8
// Note: the pick button is the button embedded into the front
// surface of the control sphere.
buttons[8] = static_cast<unsigned char>(((buf[1] & 0x10) != 0));
break;
case '.': /* button press event (4000) */
/* Spaceball 4000 FLX "expanded" button packet, with 12 buttons */
/* extra packet validity check, since we use this packet type */
/* to override the 'K' button packets, and determine if its a */
/* Spaceball 4000 or not... */
if (buf[3] != '\r') {
break; /* if not terminated with a '\r', probably garbage */
}
/* if we got a valid '.' packet, this must be a Spaceball 4000 */
#if defined(DEBUG)
if (!spaceball4000)
printf("\nDetected a Spaceball 4000 FLX\n");
#endif
spaceball4000 = 1; /* Must be talking to a Spaceball 4000 */
// XXX my original libsball button decoding code, for reference
// buttons =
// (((~buf[1]) & 0x20) << 10) | /* "left handed" mode */
// ((buf[1] & 0x1F) << 7) | /* 8,9,10,11,12 */
// ((buf[2] & 0x3F) ) | /* 1,2,3,4,5,6 */
// ((buf[2] & 0x80) >> 1); /* 7 */
/* Spaceball 4000 series "expanded" button press event */
/* includes data for 12 buttons, and left/right orientation */
buttons[0] = ((buf[2] & 0x01) != 0); // SB 4000 button 1
buttons[1] = ((buf[2] & 0x02) != 0); // SB 4000 button 2
buttons[2] = ((buf[2] & 0x04) != 0); // SB 4000 button 3
buttons[3] = ((buf[2] & 0x08) != 0); // SB 4000 button 4
buttons[4] = ((buf[2] & 0x10) != 0); // SB 4000 button 5
buttons[5] = ((buf[2] & 0x20) != 0); // SB 4000 button 6
buttons[6] = ((buf[2] & 0x80) != 0); // SB 4000 button 7
buttons[7] = ((buf[1] & 0x01) != 0); // SB 4000 button 8
buttons[8] = ((buf[1] & 0x02) != 0); // SB 4000 button 9
buttons[9] = ((buf[1] & 0x04) != 0); // SB 4000 button 10
buttons[10] = ((buf[1] & 0x08) != 0); // SB 4000 button 11
buttons[11] = ((buf[1] & 0x10) != 0); // SB 4000 button 12
// XXX lefty/righty mode handling goes here if we wish to
// represent it as a "button" etc..
//buttons[??] = ((~buf[1]) & 0x20) != 0) // SB 4000 "lefty" mode bit
#if defined(DEBUG)
if (leftymode4000 != ((buf[1] & 0x20) == 0))
printf("\nSpaceball 4000 mode changed to: %s\n",
(((buf[1] & 0x20) == 0) ? "left handed" : "right handed"));
#endif
/* set "lefty" orientation mode if "lefty bit" is _clear_ */
if ((buf[1] & 0x20) == 0)
leftymode4000 = 1; /* left handed mode */
else
leftymode4000 = 0; /* right handed mode */
break;
case 'C': /* Communications mode packet */
case 'F': /* Spaceball sensitization packet */
case 'P': /* Spaceball update rate packet */
case 'M': /* Spaceball movement mode packet */
case 'N': /* Null region packet */
case '\r': /* carriage return at poweron */
case '\v': /* XON at poweron */
/* eat and ignore these packets */
break;
case '@': /* Reset packet */
#if defined(DEBUG)
printf("Spaceball reset: ");
for (j=0; j<packlen; j++) {
if (isprint(buf[j]))
printf("%c", buf[j]);
}
printf("\n");
#endif
/* if we get a reset packet, we have to re-initialize */
/* the device, and assume that its completely schizophrenic */
/* at this moment, we must reset it again at this point */
resetoccured=1;
reset(); // was sball_hwreset()
break;
case 'E': /* Error packet, hardware/software problem */
erroroccured++;
#if defined(DEBUG)
printf("\nSpaceball Error!! ");
printf("Error code: ");
for (j=0; j<packlen; j++) {
printf(" 0x%02x ", buf[j]);
}
printf("\n");
#endif
break;
case 'Z': /* Zero packet (Spaceball 2003/3003/4000 FLX) */
/* We just ignore these... */
break;
default:
#if defined(DEBUG)
printf("Unknown packet (2): 0x%02x\n", packtype);
printf(" char: ");
if (isprint(packtype))
printf("%c", packtype);
else
printf(" (unprintable)");
printf("\n");
#endif
break;
}
/* reset */
bufpos = 0;
packtype = 0;
packlen = 1;
packs++;
}
}
report_changes(); // Report updates to VRPN
if (packs > 0)
return 1; // got at least 1 full report
else
return 0; // didn't get any full reports
}
