Exemplo n.º 1
0
int main(int argc, char const *argv[])
{
	bool ok;
	struct ft_context context;

	// compile message to print
	int msglen = 1;
	for(int i=1; i<argc; i+=1)
	{
		msglen += 1;
		msglen += strlen(argv[i]);
	}

	if (msglen == 1) return EXIT_SUCCESS;

	char msg[msglen];
	char * c = msg;
	for(int i=1; i<argc; i+=1)
	{
		strcpy(c, argv[i]);
		c += strlen(argv[i]);
		*c = ' ';
		c += 1;
		*c = '\0';
	}	

	ft_initialise();
	
	// Initialize a context
	ok = ft_context_init(&context);
	if (!ok) return EXIT_FAILURE;

	ok = ft_log_syslog_backend_init(&context);
	if (!ok) return EXIT_FAILURE;	

	ft_log_backend_switch(&ft_log_syslog_backend);

	FT_INFO("%s", msg);

	// Enter event loop
	ft_context_run(&context);

	// Clean up
	ft_context_fini(&context);

	return EXIT_SUCCESS;
}
Exemplo n.º 2
0
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 {
		strncpy(add_reset_cmd, additional_reset_commands, sizeof(add_reset_cmd)-1);
	}

	// 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);
  FT_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
}
Exemplo n.º 3
0
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
   static int singleSyncChar = 0; // set if we only get a single sync char

   //--------------------------------------------------------------------
   // 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'.

   if (status == vrpn_TRACKER_SYNCING) {

     // Try to get the first sync character if don't already have it. 
     // If none, just return.
     if (singleSyncChar != 1) {
       ret = vrpn_read_available_characters(serial_fd, buffer, 1);
       if (ret != 1) {
	 //if (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
       singleSyncChar = 0;
     }
     else if (ret != -1) {
       if (DEBUG) fprintf(stderr,"[DEBUG]: Missed Second Sync Char\n");
       singleSyncChar = 1;
       return 0;
     }

 

      // If it is not an 'LY', 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)
      ) 
      {
      	sprintf(errmsg,"While syncing (looking for 'LY' or 'PA', "
		"got '%c')", buffer[0]);
	FT_INFO(errmsg);
	vrpn_flush_input_buffer(serial_fd);
	if (DEBUG) fprintf(stderr,"[DEBUGA]: Getting Report - Not LY or PA, Got Character %c %c \n",buffer[0],buffer[1]);
      	return 0;
      }

        if (DEBUG) fprintf(stderr,"[DEBUG]: Getting Report - Got LY or PA\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 (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);
	   FT_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 (DEBUGA) fprintf(stderr,"[DEBUG]: Error Reading Report\n");
	FT_ERROR("Error reading report");
	status = vrpn_TRACKER_FAIL;
	return 0;
   }
   bufcount += ret;
   if (bufcount < REPORT_LEN) {	// Not done -- go back for more
     if (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'))
   ) {
     if (DEBUGA)	fprintf(stderr,"[DEBUG]: Don't have LY or PA at beginning");
	   status = vrpn_TRACKER_SYNCING;
	   FT_INFO("Not 'LY' or 'PA' in record, re-syncing");
	   vrpn_flush_input_buffer(serial_fd);
	   return 0;
   }

   if (buffer[bufcount-1] != ' ') {
	   status = vrpn_TRACKER_SYNCING;
	   FT_INFO("No space character at end of report, re-syncing\n");
	   vrpn_flush_input_buffer(serial_fd);
	   if (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 (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_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;
}
Exemplo n.º 4
0
//-----------------------------------------------------------------
// 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;
	unsigned int numparameter=0;
	unsigned int index=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.
	//--------------------------------------------------------------------


	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 {


			//if(vrpn_read_available_characters(serial_fd, &buffer[0], 1) != 1)
			if(vrpn_read_available_characters(serial_fd, buffer, 1) != 1)
			{			
				return 0;
			}
		} 

		// 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]);
	        FT_INFO(errmsg);
	        vrpn_flush_input_buffer(serial_fd);

			return 0;
		}

      bufcount = 1;  // external GPS parser lib expects "$" at start
	  
	  gettimeofday(&timestamp, NULL);
      status = vrpn_TRACKER_PARTIAL;
   }//if syncing

   while (!done)
   {
 		if (testfile != NULL) {
			ret = fread(&buffer[bufcount],sizeof(char),1,testfile); 
		} else {
			ret = vrpn_read_available_characters(serial_fd, &buffer[bufcount], 1);
		}

	    if (ret == -1) {
			FT_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();
		if (nmeaData.isValidLat && nmeaData.isValidLon && 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

					pos[0] = (float)(y);
					pos[1] = (float)(x);
					pos[2] = (float)(nmeaData.altitude);
				}
			} else {
				
				pos[0] = (float)(nmeaData.lat);
				pos[1] = (float)(nmeaData.lon);
				pos[2] = (float)(nmeaData.altitude);
			}//use utm d           
			/*
			vel[0] = vel_data[0];
			vel[1] = vel_data[1];
			vel[2] = vel_data[2];
			*/
			nmeaParser.reset();
			
			//send report -eb
			//-----------------------------
			//printf("tracker report ready\n",status);
			
			//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;
		}//valid lat lon alt
	}//valid sentence

	// failed valid sentence
	//printf("invalid sentence status is %d\n",status);
    status = vrpn_TRACKER_SYNCING;
	//printf("invalid sentence status syncing\n",status);
	return 0;

#ifdef VERBOSE2
	//      print_latest_report();
#endif
}