コード例 #1
0
ファイル: vrpn_Analog_5dt.C プロジェクト: Progga1/vrpn
/******************************************************************************
 * NAME      : vrpn_5dt::mainloop
 * ROLE      :  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 device,
 *              either trying to reset it or trying to get a reading from it.  It will
 *              try to reset the device if no data has come from it for a couple of
 *              seconds
 * ARGUMENTS : 
 * RETURN    : void
 ******************************************************************************/
void vrpn_5dt::mainloop ()
{
  char l_errmsg[256];

  server_mainloop();
  if (_wireless) {
    static bool announced = false;
    if (!announced) {
      VRPN_MSG_INFO ("Will connect to a receive-only 'wireless-type' glove - there may be a few warnings before we succeed.");
      announced = true;
    }
  }
  switch (_status)
    {
    case STATUS_RESETTING:
      if (reset()== -1)
	{
	  VRPN_MSG_ERROR ("vrpn_Analog_5dt: Cannot reset the glove!");
	}
      break;

    case STATUS_SYNCING:
      syncing ();
      break;

    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).
        get_report();
        struct timeval current_time;
        vrpn_gettimeofday (&current_time, NULL);
        if (vrpn_TimevalDuration (current_time, timestamp) > MAX_TIME_INTERVAL) {
          sprintf (l_errmsg, "vrpn_5dt::mainloop: Timeout... current_time=%ld:%ld, timestamp=%ld:%ld",
                   current_time.tv_sec,
                   static_cast<long> (current_time.tv_usec),
                   timestamp.tv_sec,
                   static_cast<long> (timestamp.tv_usec));
          VRPN_MSG_ERROR (l_errmsg);
          _status = STATUS_RESETTING;
        }
      }
      break;

    default:
      VRPN_MSG_ERROR ("vrpn_5dt::mainloop: Unknown mode (internal error)");
      break;
    }
}
コード例 #2
0
ファイル: vrpn_Analog_5dt.C プロジェクト: Progga1/vrpn
/******************************************************************************
 * 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) {
      VRPN_MSG_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) {
      VRPN_MSG_INFO ("Got capability byte as expected - switching into read mode.");
      _bufcount = 0;
      _status = STATUS_READING;
    } else {
      VRPN_MSG_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 :
      VRPN_MSG_ERROR ("vrpn_5dt::syncing : internal error : unknown state");
      printf ("mode %d\n", _mode);
      break;
    }
  _bufcount = 0;
  _status = STATUS_READING;
}
コード例 #3
0
ファイル: vrpn_Tracker_Liberty.C プロジェクト: vrpn/vrpn
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(&timestamp, 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;
}
コード例 #4
0
ファイル: vrpn_Tracker_isense.C プロジェクト: vrpn/vrpn
vrpn_Tracker_InterSense::vrpn_Tracker_InterSense(const char *name, 
                                         vrpn_Connection *c,
                                         int commPort, const char *additional_reset_commands, 
										 int is900_timestamps, int reset_at_start) :
vrpn_Tracker(name,c), do_is900_timestamps(is900_timestamps), 
m_reset_at_start(reset_at_start)
{
#ifdef  VRPN_INCLUDE_INTERSENSE
  char errStr[1024];
  int i;

  register_server_handlers();

	if (additional_reset_commands == NULL) {
		sprintf(add_reset_cmd, "");
	} else {
		vrpn_strcpy(add_reset_cmd, additional_reset_commands);
	}

	// Initially, set to no buttons or analogs on the stations.  The
	// methods to add buttons and analogs must be called to add them.
	for (i = 0; i < ISD_MAX_STATIONS; i++) {
	    is900_buttons[i] = NULL;
	    is900_analogs[i] = NULL;
	}

  m_CommPort = commPort;
  m_Handle = ISD_OpenTracker(NULL, commPort, FALSE, FALSE);

  if(m_Handle == -1)
  {
    sprintf(errStr,"Failed to open tracker '%s' on COM%d: ISLIB_OpenTracker returned -1",name,commPort);
    status = vrpn_TRACKER_FAIL;
    return;
  }

  //  ISD_TRACKER_INFO_TYPE trackerInfo;

  ISD_GetTrackerConfig(m_Handle,&m_TrackerInfo,FALSE);

  for (i = 0; i < ISD_MAX_STATIONS; i++) {
       if (set_sensor_output_format(i)) {
		    sprintf(errStr,"Failed to reset sensor %d on tracker '%s' on COM%d",i, name,commPort);
			status = vrpn_TRACKER_FAIL;
     		return;
       }
  }


  //what is the update rate of this tracker?
  //we might want to set the update rate of the mainloop to based on this value.
  //for now we just print it out.
  getTrackerInfo(errStr);
  vrpn_gettimeofday(&timestamp, NULL);
  VRPN_MSG_INFO(errStr);
  fprintf(stderr,errStr);	
  
  status =   vrpn_TRACKER_SYNCING;
#else
  fprintf(stderr,"Intersense library not compiled into this version.  Use Fastrak driver for IS-600/900 or recompile with VRPN_INCLUDE_INTERSENSE defined\n");
  status = vrpn_TRACKER_FAIL;
#endif
}
コード例 #5
0
ファイル: vrpn_Analog_5dt.C プロジェクト: Progga1/vrpn
/******************************************************************************
 * 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(&timestamp, 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) {
      VRPN_MSG_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(&timestamp, 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) {
    VRPN_MSG_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!
      VRPN_MSG_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) {
      VRPN_MSG_INFO ("A 'wireless-type' right glove is ready and reporting");
    } else {
      VRPN_MSG_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 :
      VRPN_MSG_ERROR ("vrpn_5dt::get_report : internal error : unknown state");
      break;
  }
}
コード例 #6
0
ファイル: vrpn_Analog_5dt.C プロジェクト: Progga1/vrpn
/******************************************************************************
 * NAME      : vrpn_5dt::reset
 * ROLE      : This routine will reset the 5DT
 * ARGUMENTS : 
 * RETURN    : 0 else -1 in case of error
 ******************************************************************************/
int
vrpn_5dt::reset (void)
{
  struct timeval l_timeout;
  unsigned char	l_inbuf [45];
  int            l_ret;
  char           l_errmsg[256];

  if (_wireless) {
    // Wireless gloves can't be reset, but we do need to wait for a header byte.
    // Then, syncing will wait to see if we get a capability byte as expected
    // at the end of a report.
    _gotInfo = false;
    vrpn_SleepMsecs (100);  //Give it time to respond

    // Will wait at most 2 seconds
    l_timeout.tv_sec = 2;
    l_timeout.tv_usec = 0;
    l_ret = vrpn_read_available_characters (serial_fd, l_inbuf, 1, &l_timeout);
    if (l_ret != 1) {
      VRPN_MSG_ERROR ("vrpn_5dt: Unable to read from the glove\n");
      return -1;
    }
    if (l_inbuf[0] == 0x80) {
      _status = STATUS_SYNCING;
      _buffer[0] = l_inbuf[0];
      _bufcount = 1;
      vrpn_gettimeofday (&timestamp, NULL);	// Set watchdog now
      VRPN_MSG_INFO ("vrpn_5dt: Got a possible header byte!");
      return 0;
    }
    return 0;
  }
  vrpn_flush_input_buffer (serial_fd);
  send_command ((unsigned char *) "A", 1); // Command to init the glove
  vrpn_SleepMsecs (100);  //Give it time to respond
  l_timeout.tv_sec = 2;
  l_timeout.tv_usec = 0;
  l_ret = vrpn_read_available_characters (serial_fd, l_inbuf, 1, &l_timeout);

  if (l_ret != 1) {
    VRPN_MSG_ERROR ("vrpn_5dt: Unable to read from the glove\n");
    return -1;
  }

  if (l_inbuf[0] != 85) {
    VRPN_MSG_ERROR ("vrpn_5dt: Cannot get response on init command");
    return -1;
  } else {
    vrpn_flush_input_buffer (serial_fd);
    send_command ( (unsigned char *) "G", 1); //Command to Query informations from the glove
    vrpn_SleepMsecs (100);  //Give it time to respond
    l_timeout.tv_sec = 2;
    l_timeout.tv_usec = 0;
    l_ret = vrpn_read_available_characters (serial_fd, l_inbuf, 32, &l_timeout);

    if (l_ret != 32) {
      VRPN_MSG_ERROR ("vrpn_5dt: Cannot get info. from the glove");
      return -1;
    }
    if ( (l_inbuf[0] != 66) || (l_inbuf[1] != 82)) {
      VRPN_MSG_ERROR ("vrpn_5dt: Cannot get good header on info command");
      return -1;
    }

    sprintf (l_errmsg, "vrpn_5dt: glove \"%s\"version %d.%d\n", &l_inbuf [16], l_inbuf [2], l_inbuf [3]);
    VRPN_MSG_INFO (l_errmsg);

    if (l_inbuf[4] | 1) {
      VRPN_MSG_INFO ("A right glove is ready");
    } else {
      VRPN_MSG_INFO ("A left glove is ready");
    }
    if (l_inbuf[5] | 16) {
      VRPN_MSG_INFO ("Pitch and Roll are available");
    } else {
      VRPN_MSG_INFO ("Pitch and Roll are not available");
    }
  }

  // If we're in continuous mode, request continuous sends
  if (_mode == 2) {
    send_command ( (unsigned char *) "D", 1); // Command to query streaming data from the glove
  }

  // We're now entering the syncing mode which send the read command to the glove
  _status = STATUS_SYNCING;

  vrpn_gettimeofday (&timestamp, NULL);	// Set watchdog now
  return 0;
}
コード例 #7
0
ファイル: vrpn_Tracker_GPS.C プロジェクト: ASPePeX/vrpn
//-----------------------------------------------------------------
// This function will read characters until it has a full report, then
// put that report into the time, sensor, pos and quat fields so that it can
// be sent the next time through the loop.
int vrpn_Tracker_GPS::get_report(void)
{
	//printf("getting report\n");
	char errmsg[512];	// Error message to send to VRPN
	//int ret;		// Return value from function call to be checked
	int ret;
    
    //	unsigned char *bufptr;	// Points into buffer at the current value to read
	int done=0;
    
	//char speed[256];
	//char course[256];
	//	int buflen = 0;
	int charRead = 0;
    //	char temp [256];
    
	//--------------------------------------------------------------------
	// Each report starts with an ASCII '$' character. If we're synching,
	// read a byte at a time until we find a '$' character.
	//--------------------------------------------------------------------
    
	//printf("STATUS =%d\n", status);
	if(status == vrpn_TRACKER_SYNCING)
	{
		
		// If testfile is live; read data from that
		if (testfile != NULL) { 
            
			//Read one char to see if test file is active
			if(fread(buffer,sizeof(char),1,testfile) != 1)  
			{
				return 0;
			}
            
         //   Else read from the serial port 
		} else {
			
            charRead = vrpn_read_available_characters(serial_fd, buffer, 1);
            
            
			if( charRead == -1)
			{			
				printf("fail to read first bit\n");
				return 0;
			} else if (charRead == 0) {
				//case of successful read but no new bits, take a quick nap, wait for new info.
				
#ifdef _WIN32
                Sleep(10);
#else
                usleep(10000);
#endif
				return 0;
			}
        }
			//if it returns 0 (very likely - happens on interrupt or no new bits) then we have to prevent it from reading the same buffer over and over.
            
            // If it's not = to $, keep going until the beginning of a packet starts
            if( buffer[0] != '$') 
            {
                sprintf(errmsg,"While syncing (looking for '$', got '%c')", buffer[0]);
                
                
                VRPN_MSG_INFO(errmsg);
                vrpn_flush_input_buffer(serial_fd);
                
                return 0;
            }
            
            //printf("We have a $, setting to partial and moving on.\n");
            bufcount = 1;  // external GPS parser lib expects "$" at start
            
            vrpn_gettimeofday(&timestamp, NULL);
            
            status = vrpn_TRACKER_PARTIAL;
        }//if syncing
        
        while (!done)
        {
            if (testfile != NULL) {
                ret = static_cast<int>(fread(&buffer[bufcount],sizeof(char),1,testfile)); 
            } else {
                ret = vrpn_read_available_characters(serial_fd, &buffer[bufcount], 1);
            }
            
            if (ret == -1) {
                VRPN_MSG_ERROR("Error reading report");
                status = vrpn_TRACKER_FAIL;
                return 0;
            } else if (ret == 0) {
                return 0;
            }
            
            bufcount += ret;
            if (bufcount >= VRPN_TRACKER_BUF_SIZE*10)
            {
                status = vrpn_TRACKER_SYNCING;
                return 0;
            }
            if(buffer[bufcount-1] == '\n')
            {
                buffer[bufcount-1] = '\0';
                done = 1;
            }
        }
        if (nmeaParser.parseSentence((char*)buffer) == SENTENCE_VALID) 
        {
            nmeaData = nmeaParser.getData();
            //printf("Raw lastfixQuality: %d anyValid: %d LAT: %f LONG: %f ALT: %f\n", nmeaData.lastFixQuality, nmeaData.hasCoordEverBeenValid, nmeaData.lat, nmeaData.lon, nmeaData.altitude);
            //required RMC doesn't do altitude, so this will be needlessly false half of the time.
            //if (nmeaData.isValidLat && nmeaData.isValidLon && nmeaData.isValidAltitude) 
            if (nmeaData.isValidLat && nmeaData.isValidLon)
            { 
                if(nmeaData.isValidAltitude){
                
                if (useUTM) 
                {
                    utmCoord.setLatLonCoord (nmeaData.lat, nmeaData.lon);
                    if (!utmCoord.isOutsideUTMGrid ())
                    {
                        double x, y;
                        utmCoord.getXYCoord (x,y);
                        // Christopher 07/25/04: We flip to be x = East <-> West and y = North <-> South
                        //Should this be changed to match rtk precision vrpn_float64?
                        pos[0] = (float)(y);
                        pos[1] = (float)(x);
                        pos[2] = (float)(nmeaData.altitude);
                    }
                } else {
                    
                    pos[0] = (vrpn_float64)(nmeaData.lat);
                    pos[1] = (vrpn_float64)(nmeaData.lon);
                    pos[2] = (vrpn_float64)(nmeaData.altitude);
                }//use utm d           
                /*
                 vel[0] = vel_data[0];
                 vel[1] = vel_data[1];
                 vel[2] = vel_data[2];
                 */
//#ifdef VERBOSE	
                printf("GPS pos: %f, %f, %f\n",pos[0],pos[1],pos[2]);
//#endif
                nmeaParser.reset();
                
                //send report -eb
                //-----------------------------
                //printf("tracker report ready\n",status);
                
                //vrpn_gettimeofday(&timestamp, NULL);	// Set watchdog now
                
                /*
                 // Send the message on the connection
                 if (NULL != vrpn_Tracker::d_connection) 
                 {
                 char	msgbuf[1000];
                 
                 
                 fprintf(stderr, "position id = %d, sender id = %d", position_m_id, d_sender_id); 
                 //MessageBox(NULL, temp,"GPS Testing",0);
                 
                 // Pack position report
                 int	len = encode_to(msgbuf);
                 if (d_connection->pack_message(len, timestamp,
                 position_m_id, d_sender_id, msgbuf,
                 vrpn_CONNECTION_LOW_LATENCY)) 
                 {
                 
                 fprintf(stderr,"GPS: cannot write message: tossing\n");
                 }
                 else 
                 {
                 fprintf(stderr,"packed a message\n\n");
                 }
                 
                 
                 } 
                 else 
                 {
                 fprintf(stderr,"Tracker Fastrak: No valid connection\n");
                 }
                 
                 //-----------------------------*/
                
                //printf("%s\n", buffer);
                //printf("before first sync status is %d\n",status);
                status = vrpn_TRACKER_SYNCING;
                //printf("after first set sync status is %d\n",status);
                
                return 1;
            }
                //no valid alt probably RMC, safe to ignore if we care about alt
            } else {//valid lat lon alt fail
                printf("GPS cannot determine position (empty fields). Wait for satellites or reposition GPS.\n");
                //printf("GPS cannot determine position (empty fields).");
                status = vrpn_TRACKER_SYNCING;
                return 0;
            }
		} else {//valid sentence fail
            //status = vrpn_TRACKER_FAIL;
            status = vrpn_TRACKER_RESETTING;
            //is this a good place to put it?  If it's not a valid sentence? maybe it should be in reset.
            printf("Sentence Invalid.  Resetting tracker and resyncing.\n");
            return 0;
        }
        // failed valid sentence
        status = vrpn_TRACKER_SYNCING;
        return 0;
        
//#ifdef VERBOSE2
        //      print_latest_report();
//#endif
    }