/******************************************************************************
* 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
}
