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 }