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; }
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(×tamp, 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 }
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(×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 (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; }
//----------------------------------------------------------------- // 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(×tamp, 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(×tamp, 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 }