// vrpn_ImmersionBox.C // This is a driver for an Immersion Corporation Immersion Box controller. // This box is a serial-line device that allows the user to connect // analog inputs, digital inputs, digital outputs, and digital // encoders and read from them over RS-232. // // This code is based on the ImmersionBox code previously written as part // of the vrpn library #include <math.h> // for floor #include <stdio.h> // for fprintf, stderr, printf #include <string.h> // for NULL, strcpy #include "vrpn_BaseClass.h" // for ::vrpn_TEXT_ERROR #include "vrpn_ImmersionBox.h" #include "vrpn_Serial.h" #include "vrpn_Shared.h" // for timeval, vrpn_gettimeofday #undef VERBOSE // low-level stuff //static const unsigned char CMD_BASIC = static_cast<unsigned char>(0xC0); // mask for command //static const unsigned char CMD_HOMEREF = static_cast<unsigned char>(0xC1); //static const unsigned char CMD_HOMEPOS = static_cast<unsigned char>(0xC2); //static const unsigned char CMD_SETHOME = static_cast<unsigned char>(0xC3); //static const unsigned char CMD_BAUDSET = static_cast<unsigned char>(0xC4); //static const unsigned char CMD_ENDSESS = static_cast<unsigned char>(0xC5); //static const unsigned char CMD_GETMAXS = static_cast<unsigned char>(0xC6); //static const unsigned char CMD_SETPARM = static_cast<unsigned char>(0xC7); static const unsigned char CMD_GETNAME = static_cast<unsigned char>(0xC8); static const unsigned char CMD_GETPRID = static_cast<unsigned char>(0xC9); static const unsigned char CMD_GETMODL = static_cast<unsigned char>(0xCA); static const unsigned char CMD_GETSERN = static_cast<unsigned char>(0xCB); static const unsigned char CMD_GETCOMM = static_cast<unsigned char>(0xCC); static const unsigned char CMD_GETPERF = static_cast<unsigned char>(0xCD); static const unsigned char CMD_GETVERS = static_cast<unsigned char>(0xCE); //static const unsigned char CMD_GETMOTN = static_cast<unsigned char>(0xCF); //static const unsigned char CMD_SETHREF = static_cast<unsigned char>(0xD0); //static const unsigned char CMD_FACREST = static_cast<unsigned char>(0xD1); //static const unsigned char CMD_INSMARK = static_cast<unsigned char>(0xD2); //static const double PAUSE_RESET = .015; //static const double PAUSE_END = .015; //static const double PAUSE_RESTORE = 2.0; //static const double PAUSE_BYTE = .015; // Defines the modes in which the box can find itself. #define STATUS_RESETTING (-1) // Resetting the box #define STATUS_SYNCING (0) // Looking for the first character of report #define STATUS_READING (1) // Looking for the rest of the report /* Strings for establishing connection */ char S_INITIALIZE[5] = "IMMC"; char S_START[6] = "BEGIN"; char S_END[4] = "END"; #define MAX_TIME_INTERVAL (2000000) // max time between reports (usec) static void pause (double delay) { if (delay < 0) delay = 0; unsigned long interval = (long) floor(1000000.0 * delay); struct timeval start, now; vrpn_gettimeofday (&start, NULL); do { vrpn_gettimeofday (&now, NULL); } while (vrpn_TimevalDuration(now, start) < interval); } // This creates a vrpn_ImmersionBox and sets it to reset mode. It opens // the serial device using the code in the vrpn_Serial_Analog constructor. // The box will autodetect the baud rate when the "IMMC" command is sent // to it. vrpn_ImmersionBox::vrpn_ImmersionBox (const char * name, vrpn_Connection * c, const char * port, int baud, const int numbuttons, const int numchannels, const int numencoders): vrpn_Serial_Analog(name, c, port, baud), vrpn_Button_Filter(name, c), vrpn_Dial(name, c), _numbuttons(numbuttons), _numchannels(numchannels), _numencoders(numencoders) { // Verify the validity of the parameters if (_numbuttons > MAX_IBUTTONS) { fprintf(stderr,"vrpn_ImmersionBox: Can only support %d buttons, not %d\n", MAX_IBUTTONS, _numbuttons); _numbuttons = MAX_IBUTTONS; } if (_numchannels > MAX_ICHANNELS) { fprintf(stderr,"vrpn_ImmersionBox: Can only support %d analog channels, not %d\n", MAX_ICHANNELS, _numchannels); _numchannels = MAX_ICHANNELS; } if (_numencoders > MAX_IENCODERS) { fprintf(stderr,"vrpn_ImmersionBox: Can only support %d encoders, not %d\n", MAX_IENCODERS, _numencoders); _numencoders = MAX_IENCODERS; } // explicitly clear the identification variables iname[0] = 0; comment[0] = 0; serial[0] = 0; id[0] = 0; model[0] = 0; vers[0] = 0; parmf[0] = 0; // Set the parameters in the parent classes vrpn_Button::num_buttons = _numbuttons; vrpn_Analog::num_channel = _numchannels; vrpn_Dial::num_dials = _numencoders; vrpn_gettimeofday(×tamp, NULL); // Set watchdog now // Set the status of the buttons, analogs and encoders to 0 to start clear_values(); // Set the mode to reset _status = STATUS_RESETTING; } void vrpn_ImmersionBox::clear_values(void) { int i; for (i = 0; i < _numbuttons; i++) { vrpn_Button::buttons[i] = vrpn_Button::lastbuttons[i] = 0; } for (i = 0; i < _numchannels; i++) { vrpn_Analog::channel[i] = vrpn_Analog::last[i] = 0; } for (i = 0; i < _numencoders; i++) { vrpn_Dial::dials[i] = 0.0; } } // This routine will reset the ImmersionBox, asking it to send the requested number // of analogs, buttons and encoders. // It verifies that it can communicate with the device and checks the version of // the device. 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; } // This function will read characters until it has a full report, then // put that report into the time, analog, button and encoder fields and call // the report methods on these. The time stored is that of // the first character received as part of the report. // // Reports start with the header packetHeader followed by dataRecordLength bytes // // If we get a report that is not valid, we assume that we have lost a // character or something and re-synchronize by flushing the buffer // // 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 box hasn't simply // stopped sending characters). // Returns 1 if got a full report, 0 otherwise. 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; } void vrpn_ImmersionBox::report_changes(vrpn_uint32 class_of_service) { vrpn_Analog::timestamp = timestamp; vrpn_Button::timestamp = timestamp; vrpn_Dial::timestamp = timestamp; vrpn_Analog::report_changes(class_of_service); vrpn_Button::report_changes(); vrpn_Dial::report_changes(); } void vrpn_ImmersionBox::report(vrpn_uint32 class_of_service) { vrpn_Analog::timestamp = timestamp; vrpn_Button::timestamp = timestamp; vrpn_Dial::timestamp = timestamp; vrpn_Analog::report(class_of_service); vrpn_Button::report_changes(); vrpn_Dial::report(); } // This routine is called each time through the server's main loop. It will // take a course of action depending on the current status of the cerealbox, // either trying to reset it or trying to get a reading from it. void vrpn_ImmersionBox::mainloop(void) { server_mainloop(); switch(status) { case STATUS_RESETTING: reset(); break; case STATUS_SYNCING: case STATUS_READING: { // It turns out to be important to get the report before checking // to see if it has been too long since the last report. This is // because there is the possibility that some other device running // in the same server may have taken a long time on its last pass // through mainloop(). Trackers that are resetting do this. When // this happens, you can get an infinite loop -- where one tracker // resets and causes the other to timeout, and then it returns the // favor. By checking for the report here, we reset the timestamp // if there is a report ready (ie, if THIS device is still operating). while (get_report()) {}; // Get multiple reports if available struct timeval current_time; vrpn_gettimeofday(¤t_time, NULL); if ( vrpn_TimevalDuration(current_time,timestamp) > MAX_TIME_INTERVAL) { fprintf(stderr,"Tracker failed to read... current_time=%ld:%ld, timestamp=%ld:%ld\n", current_time.tv_sec, static_cast<long>(current_time.tv_usec), timestamp.tv_sec, static_cast<long>(timestamp.tv_usec)); send_text_message("Too long since last report, resetting", current_time, vrpn_TEXT_ERROR); status = STATUS_RESETTING; } } break; default: fprintf(stderr,"vrpn_ImmersionBox: Unknown mode (internal error)\n"); break; } } // utility routine (optionally) to send commands to the ibox, // (optionally) to return results, // (optionally) delaying between sending the command and receiving the results 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; } // 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; }