/****************************************************************************** * 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 (¤t_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; } }
/****************************************************************************** * 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; }
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; }
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(×tamp, 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 }
/****************************************************************************** * 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) { 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(×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) { 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; } }
/****************************************************************************** * 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 (×tamp, 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 (×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, 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(×tamp, 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(×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; } //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 }