void vrpn_Nikon_Controls::mainloop()
{
char errmsg[256];
server_mainloop();
switch(status) {
case STATUS_RESETTING:
reset();
break;
case STATUS_SYNCING:
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).
while (get_report()) {}; // Keep getting reports so long as there are more
struct timeval current_time;
vrpn_gettimeofday(¤t_time, NULL);
if ( vrpn_TimevalDuration(current_time,timestamp) > POLL_INTERVAL) {
static struct timeval last_poll = {0, 0};
if (vrpn_TimevalDuration(current_time, last_poll) > TIMEOUT_TIME_INTERVAL) {
// Ask the unit for its current focus location, which will cause it to respond.
char msg[256];
sprintf(msg, "rSPR\r");
if (vrpn_write_characters(serial_fd, (unsigned char *)msg, strlen(msg)) != (int)strlen(msg)) {
fprintf(stderr, "vrpn_Nikon_Controls::mainloop(): Can't write focus request command\n");
status = STATUS_RESETTING;
return;
}
vrpn_gettimeofday(&last_poll, NULL);
} else {
return;
}
}
if ( vrpn_TimevalDuration(current_time,timestamp) > TIMEOUT_TIME_INTERVAL) {
sprintf(errmsg,"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(errmsg);
status = STATUS_RESETTING;
}
}
break;
default:
VRPN_MSG_ERROR("Unknown mode (internal error)");
break;
}
}
/******************************************************************************
* 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;
}
}
int vrpn_Tracker_InterSense::add_is900_analog(const char *analog_device_name, int sensor,
double c0Min, double c0Low, double c0Hi, double c0Max,
double c1Min, double c1Low, double c1Hi, double c1Max)
{
// Make sure this is a valid sensor
if ( (sensor < 0) || (sensor >= ISD_MAX_STATIONS) ) {
return -1;
}
// Add a new analog device and set the pointer to point at it.
try { is900_analogs[sensor] = new vrpn_Clipping_Analog_Server(analog_device_name, d_connection); }
catch (...) {
VRPN_MSG_ERROR("Cannot open analog device");
return -1;
}
// Set the analog to have two channels, and set its channels to 0 to start with
is900_analogs[sensor]->setNumChannels(2);
is900_analogs[sensor]->setChannelValue(0, 0.0);
is900_analogs[sensor]->setChannelValue(1, 0.0);
// Set the scaling on the two channels.
is900_analogs[sensor]->setClipValues(0, c0Min, c0Low, c0Hi, c0Max);
is900_analogs[sensor]->setClipValues(1, c1Min, c1Low, c1Hi, c1Max);
// Send a new station-format command to the tracker so it will report the analog status
return set_sensor_output_format(sensor);
}
int vrpn_Tracker_Liberty::set_sensor_output_format(int sensor)
{
char outstring[64];
const char *timestring;
const char *buttonstring;
const char *analogstring;
// Set output format for the station to be position and quaternion,
// and any of the extended Liberty (stylus with button) or
// IS900 states (timestamp, button, analog).
// This command is a capitol 'o' followed by the number of the
// station, then comma-separated values (2 for xyz, 7 for quat, 8 for
// timestamp, 10 for buttons, 0 for space) that
// indicate data sets, followed by character 13 (octal 15).
// Note that the sensor number has to be bumped to map to station number.
timestring = ",8";
buttonstring = stylus_buttons[sensor] ? ",10" : "";
analogstring="";
sprintf(outstring, "O%d,2,7%s%s%s,0\015", sensor+1, timestring,
buttonstring, analogstring);
if (VRPN_LIBERTY_DEBUG) fprintf(stderr,"[DEBUG]: %s \n",outstring);
if (vrpn_write_characters(serial_fd, (const unsigned char *)outstring,
strlen(outstring)) == (int)strlen(outstring)) {
vrpn_SleepMsecs(50); // Sleep for a bit to let command run
} else {
VRPN_MSG_ERROR("Write failed on format command");
status = vrpn_TRACKER_FAIL;
return -1;
}
return 0;
}
/******************************************************************************
* 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;
}
void vrpn_3DMicroscribe::report_changes(vrpn_uint32 class_of_service)
{
vrpn_Button::timestamp = timestamp;
vrpn_Tracker::timestamp = timestamp;
vrpn_Button::report_changes();
if (d_connection) {
char msgbuf[1000];
int len = vrpn_Tracker::encode_to(msgbuf);
if (d_connection->pack_message(len, timestamp,
position_m_id, d_sender_id, msgbuf,
class_of_service)) {
VRPN_MSG_ERROR("Tracker: cannot write message: tossing\n");
}
} else {
VRPN_MSG_ERROR("Tracker: No valid connection\n");
}
}
void vrpn_OmegaTemperature::mainloop()
{
char errmsg[256];
server_mainloop();
switch(status) {
case STATUS_RESETTING:
reset();
break;
case STATUS_SYNCING:
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).
while (get_report()) {}; // Keep getting reports so long as there are more
struct timeval current_time;
vrpn_gettimeofday(¤t_time, NULL);
if ( vrpn_TimevalDuration(current_time,timestamp) > TIMEOUT_TIME_INTERVAL) {
sprintf(errmsg,"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(errmsg);
status = STATUS_RESETTING;
}
}
break;
default:
VRPN_MSG_ERROR("Unknown mode (internal error)");
break;
}
}
// This routine will reset the 3DMicroscribe, zeroing the Origin position,
// mode.
int vrpn_3DMicroscribe::reset(void)
{
#ifdef VRPN_USE_MICROSCRIBE
int iResult;
// ARM_FULL: ArmDll32 calculates and updates the Cartesian position and orientation of the stylus tip.
iResult = ArmSetUpdate(ARM_FULL);
if(iResult != ARM_SUCCESS)
{
//error setting the update type, disconnect and end the thread
VRPN_MSG_ERROR( "Unable to set the update type for the MicroScribe." );
return -1;
}
//use mm instead of inches
ArmSetLengthUnits(ARM_MM);
//use radians instead of degrees
//ArmSetAngleUnits(ARM_RADIANS);
//get the position of the tip
length_3D tipPosition;
angle_3D tipVector;
iResult = ArmGetTipPosition(&tipPosition); //retrieves the current stylus tip position in Cartesian coordinates
iResult = ArmGetTipOrientationUnitVector(&tipVector); //retrieves the current stylus tip's unit vector orientation
if(iResult == ARM_NOT_CONNECTED)
{
//error connecting
VRPN_MSG_ERROR( "MicroScribe connection lost!" );
return -1;
}
#endif
// We're now waiting for a response from the box
status = STATUS_SYNCING;
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
return 0;
}
// this routine is called when an "Stylus" button is encountered
// by the tracker init string parser it sets up the VRPN button
// device
int vrpn_Tracker_Isotrak::add_stylus_button(const char *button_device_name, int sensor)
{
// Make sure this is a valid sensor
if ( (sensor < 0) || (sensor >= num_stations) ) {
return -1;
}
// Add a new button device and set the pointer to point at it.
try { stylus_buttons[sensor] = new vrpn_Button_Server(button_device_name, d_connection, 1); }
catch (...) {
VRPN_MSG_ERROR("Cannot open button device");
return -1;
}
return 0;
}
// this routine is called when an "Stylus" button is encountered
// by the tracker init string parser it sets up the VRPN button
// device
int vrpn_Tracker_Liberty::add_stylus_button(const char *button_device_name, int sensor, int numbuttons)
{
// Make sure this is a valid sensor
if ( (sensor < 0) || (sensor >= num_stations) ) {
return -1;
}
// Add a new button device and set the pointer to point at it.
try { stylus_buttons[sensor] = new vrpn_Button_Server(button_device_name, d_connection, numbuttons); }
catch (...) {
VRPN_MSG_ERROR("Cannot open button device");
return -1;
}
// Send a new station-format command to the tracker so it will report the button states.
return set_sensor_output_format(sensor);
}
int vrpn_Tracker_Isotrak::set_sensor_output_format(int /*sensor*/)
{
char outstring[16];
// Set output format for the station to be position, quaternion
// Don't need the space anymore, though
sprintf(outstring, "O2,11\r");
if (vrpn_write_characters(serial_fd, (const unsigned char *)outstring,
strlen(outstring)) == (int)strlen(outstring)) {
vrpn_SleepMsecs(50); // Sleep for a bit to let command run
} else {
VRPN_MSG_ERROR("Write failed on format command");
status = vrpn_TRACKER_FAIL;
return -1;
}
return 0;
}
int vrpn_3DMicroscribe::get_report(void)
{
#ifdef VRPN_USE_MICROSCRIBE
length_3D tipPosition;
angle_3D tipOri;
DWORD buts;
int iResult = ArmGetTipPosition(&tipPosition); //retrieves the current stylus tip position in Cartesian coordinates
iResult = ArmGetTipOrientation(&tipOri); //retrieves the current stylus tip's unit vector orientation
iResult = ArmGetButtonsState(&buts);
if(iResult == ARM_NOT_CONNECTED)
{
//error connecting
VRPN_MSG_ERROR( "MicroScribe connection lost!" );
return 0;
}
//set the position, considering the scale, offset and origin matrix
pos[0] = (tipPosition.y * m_Scale + m_OffSet[0])* MM_TO_METERS ;
pos[1] = (tipPosition.z * m_Scale + m_OffSet[1])* MM_TO_METERS;
pos[2] = (tipPosition.x * m_Scale + m_OffSet[2])* MM_TO_METERS;
//vPosition = m_Matrix * vPosition + m_vPlaneOffset; extending the microscribe onto a plane
//set the orientation, considering the origin matrix
float ori[3]={tipOri.y, tipOri.z, tipOri.x};
ConvertOriToQuat(ori);
status = STATUS_READING; // ready to process event packet
vrpn_gettimeofday(×tamp, NULL); // set timestamp of this event
buttons[0] = ((buts & 0x02) != 0); // button 1
buttons[1] = ((buts & 0x01) != 0); // button 2
#endif
report_changes(); // Report updates to VRPN
return 0;
}
int vrpn_OmegaTemperature::get_report(void)
{
int ret; // Return value from function call to be checked
//--------------------------------------------------------------------
// If we're SYNCing, then the next character we get should be the start
// of a report. If we recognize it, go into READing mode and tell how
// many characters we expect total. If we don't recognize it, then we
// must have misinterpreted a command or something; reset
// and start over
//--------------------------------------------------------------------
if (status == STATUS_SYNCING) {
// Try to get a character. If none, just return.
if (vrpn_read_available_characters(serial_fd, (unsigned char *)(d_buffer), 1) != 1) {
return 0;
}
// Got the first character of a report -- go into READING mode
// and record that we got one character at this time. Clear the
// rest of the buffer to 0's so that we won't be looking at old
// data when we parse.
// The time stored here is as close as possible to when the
// report was generated.
d_bufcount = 1;
vrpn_gettimeofday(×tamp, NULL);
status = STATUS_READING;
size_t i;
for (i = 1; i < sizeof(d_buffer); i++) {
d_buffer[i] = 0;
}
#ifdef VERBOSE
printf("... Got the 1st char\n");
#endif
}
//--------------------------------------------------------------------
// Read as many bytes of this report as we can, storing them
// in the buffer.
//--------------------------------------------------------------------
while ( 1 == (ret = vrpn_read_available_characters(serial_fd, (unsigned char *)(&d_buffer[d_bufcount]), 1))) {
d_bufcount++;
}
if (ret == -1) {
VRPN_MSG_ERROR("Error reading");
status = STATUS_RESETTING;
return 0;
}
#ifdef VERBOSE
if (ret != 0) printf("... got %d total characters\n", d_bufcount);
#endif
if (d_buffer[d_bufcount-1] != '\r') { // Not done -- go back for more
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.
// Store the report into the appropriate analog channel.
//--------------------------------------------------------------------
#ifdef VERBOSE
printf(" Complete report: \n%s\n",d_buffer);
#endif
float value = convert_bytes_to_reading(d_buffer);
if (value == -1000) {
char msg[256];
sprintf(msg,"Invalid report, channel %d, resetting", d_next_channel_to_read);
VRPN_MSG_ERROR(msg);
status = STATUS_RESETTING;
}
channel[d_next_channel_to_read] = value;
#ifdef VERBOSE
printf("got a complete report (%d chars)!\n", d_bufcount);
#endif
//--------------------------------------------------------------------
// Request a reading from the next channe.
//--------------------------------------------------------------------
d_next_channel_to_read = (d_next_channel_to_read + 1) % 6;
if (!request_temperature(d_next_channel_to_read)) {
char msg[256];
sprintf(msg,"Can't request reading, channel %d, resetting", d_next_channel_to_read);
VRPN_MSG_ERROR(msg);
status = STATUS_RESETTING;
}
//--------------------------------------------------------------------
// Done with the decoding, send the reports and go back to syncing
//--------------------------------------------------------------------
report_changes();
status = STATUS_SYNCING;
d_bufcount = 0;
return 1;
}
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;
}
void vrpn_Tracker_Liberty::reset()
{
int i,resetLen,ret;
char reset[10];
char errmsg[512];
char outstring1[64],outstring3[64];
//--------------------------------------------------------------------
// This section deals with resetting the tracker to its default state.
// Multiple attempts are made to reset, getting more aggressive each
// time. This section completes when the tracker reports a valid status
// message after the reset has completed.
//--------------------------------------------------------------------
// Send the tracker a string that should reset it. The first time we
// try this, just do the normal 'c' command to put it into polled mode.
// after a few tries with this, use the ^Y reset. Later, try to reset
// to the factory defaults. Then toggle the extended mode.
// Then put in a carriage return to try and break it out of
// a query mode if it is in one. These additions are cumulative: by the
// end, we're doing them all.
fprintf(stderr,"[DEBUG] Beginning Reset");
resetLen = 0;
num_resets++; // We're trying another reset
if (num_resets > 0) { // Try to get it out of a query loop if its in one
reset[resetLen++] = (char) (13); // Return key -> get ready
reset[resetLen++] = 'F';
reset[resetLen++] = '0';
reset[resetLen++] = (char) (13); // Return key -> get ready
// reset[resetLen++] = (char) (13);
// reset[resetLen++] = (char) (13);
}
/* XXX These commands are probably never needed, and can cause real
headaches for people who are keeping state in their trackers (especially
the InterSense trackers). Taking them out in version 05.01; you can put
them back in if your tracker isn't resetting as well.
if (num_resets > 3) { // Get a little more aggressive
reset[resetLen++] = 'W'; // Reset to factory defaults
reset[resetLen++] = (char) (11); // Ctrl + k --> Burn settings into EPROM
}
*/
if (num_resets > 2) {
reset[resetLen++] = (char) (25); // Ctrl + Y -> reset the tracker
reset[resetLen++] = (char) (13); // Return Key
}
reset[resetLen++] = 'P'; // Put it into polled (not continuous) mode
sprintf(errmsg, "Resetting the tracker (attempt %d)", num_resets);
VRPN_MSG_WARNING(errmsg);
for (i = 0; i < resetLen; i++) {
if (vrpn_write_characters(serial_fd, (unsigned char*)&reset[i], 1) == 1) {
fprintf(stderr,".");
vrpn_SleepMsecs(1000.0*2); // Wait after each character to give it time to respond
} else {
perror("Liberty: Failed writing to tracker");
status = vrpn_TRACKER_FAIL;
return;
}
}
//XXX Take out the sleep and make it keep spinning quickly
// You only need to sleep 10 seconds for an actual Liberty.
// For the Intersense trackers, you need to sleep 20. So,
// sleeping 20 is the more general solution...
if (num_resets > 2) {
vrpn_SleepMsecs(1000.0*20); // Sleep to let the reset happen, if we're doing ^Y
}
fprintf(stderr,"\n");
// Get rid of the characters left over from before the reset
vrpn_flush_input_buffer(serial_fd);
// Make sure that the tracker has stopped sending characters
vrpn_SleepMsecs(1000.0*2);
unsigned char scrap[80];
if ( (ret = vrpn_read_available_characters(serial_fd, scrap, 80)) != 0) {
sprintf(errmsg,"Got >=%d characters after reset",ret);
VRPN_MSG_WARNING(errmsg);
for (i = 0; i < ret; i++) {
if (isprint(scrap[i])) {
fprintf(stderr,"%c",scrap[i]);
} else {
fprintf(stderr,"[0x%02X]",scrap[i]);
}
}
fprintf(stderr, "\n");
vrpn_flush_input_buffer(serial_fd); // Flush what's left
}
// Asking for tracker status. S not implemented in Liberty and hence
// ^V (WhoAmI) is used. It retruns 196 bytes
char statusCommand[2];
statusCommand[0]=(char)(22); // ^V
statusCommand[1]=(char)(13); // Return Key
if (vrpn_write_characters(serial_fd, (const unsigned char *) &statusCommand[0], 2) == 2) {
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
} else {
perror(" Liberty write failed");
status = vrpn_TRACKER_FAIL;
return;
}
// Read Status
unsigned char statusmsg[vrpn_LIBERTY_MAX_WHOAMI_LEN+1];
// Attempt to read whoami_len characters.
ret = vrpn_read_available_characters(serial_fd, statusmsg, whoami_len);
if (ret != whoami_len) {
fprintf(stderr," Got %d of %d characters for status\n",ret, whoami_len);
}
if (ret != -1) {
statusmsg[ret] = '\0'; // Null-terminate the string
}
// It seems like some versions of the tracker report longer
// messages; so we reduced this check so that it does not check for the
// appropriate length of message or for the last character being a 10,
// so that it works more generally. The removed tests are:
// || (ret!=whoami_len) || (statusmsg[ret-1]!=(char)(10))
if ( (statusmsg[0]!='0') ) {
int i;
fprintf(stderr, " Liberty: status is (");
for (i = 0; i < ret; i++) {
if (isprint(statusmsg[i])) {
fprintf(stderr,"%c",statusmsg[i]);
} else {
fprintf(stderr,"[0x%02X]",statusmsg[i]);
}
}
fprintf(stderr,"\n)\n");
VRPN_MSG_ERROR("Bad status report from Liberty, retrying reset");
return;
} else {
VRPN_MSG_WARNING("Liberty/Isense gives status (this is good)");
printf("LIBERTY LATUS STATUS (whoami):\n%s\n\n",statusmsg);
num_resets = 0; // Success, use simple reset next time
}
//--------------------------------------------------------------------
// Now that the tracker has given a valid status report, set all of
// the parameters the way we want them. We rely on power-up setting
// based on the receiver select switches to turn on the receivers that
// the user wants.
//--------------------------------------------------------------------
// Set output format for each of the possible stations.
for (i = 0; i < num_stations; i++) {
if (set_sensor_output_format(i)) {
return;
}
}
// Enable filtering if the constructor parameter said to.
// Set filtering for both position (X command) and orientation (Y command)
// to the values that are recommended as a "jumping off point" in the
// Liberty manual.
if (do_filter) {
if (VRPN_LIBERTY_DEBUG) fprintf(stderr,"[DEBUG]: Enabling filtering\n");
if (vrpn_write_characters(serial_fd,
(const unsigned char *)"X0.2,0.2,0.8,0.8\015", 17) == 17) {
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
} else {
perror(" Liberty write position filter failed");
status = vrpn_TRACKER_FAIL;
return;
}
if (vrpn_write_characters(serial_fd,
(const unsigned char *)"Y0.2,0.2,0.8,0.8\015", 17) == 17) {
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
} else {
perror(" Liberty write orientation filter failed");
status = vrpn_TRACKER_FAIL;
return;
}
} else {
if (VRPN_LIBERTY_DEBUG) fprintf(stderr,"[DEBUG]: Disabling filtering\n");
if (vrpn_write_characters(serial_fd,
(const unsigned char *)"X0,1,0,0\015", 9) == 9) {
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
} else {
perror(" Liberty write position filter failed");
status = vrpn_TRACKER_FAIL;
return;
}
if (vrpn_write_characters(serial_fd,
(const unsigned char *)"Y0,1,0,0\015", 9) == 9) {
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
} else {
perror(" Liberty write orientation filter failed");
status = vrpn_TRACKER_FAIL;
return;
}
}
// Send the additional reset commands, if any, to the tracker.
// These commands come in lines, with character \015 ending each
// line. If a line start with an asterisk (*), treat it as a pause
// command, with the number of seconds to wait coming right after
// the asterisk. Otherwise, the line is sent directly to the tracker.
// Wait a while for them to take effect, then clear the input
// buffer.
if (strlen(add_reset_cmd) > 0) {
char *next_line;
char add_cmd_copy[sizeof(add_reset_cmd)];
char string_to_send[sizeof(add_reset_cmd)];
int seconds_to_wait;
printf(" Liberty writing extended reset commands...\n");
// Make a copy of the additional reset string, since it is consumed
vrpn_strcpy(add_cmd_copy, add_reset_cmd);
// Pass through the string, testing each line to see if it is
// a sleep command or a line to send to the tracker. Continue until
// there are no more line delimiters ('\015'). Be sure to write the
// \015 to the end of the string sent to the tracker.
// Note that strok() puts a NULL character in place of the delimiter.
next_line = strtok(add_cmd_copy, "\015");
while (next_line != NULL) {
if (next_line[0] == '*') { // This is a "sleep" line, see how long
seconds_to_wait = atoi(&next_line[1]);
fprintf(stderr," ...sleeping %d seconds\n",seconds_to_wait);
vrpn_SleepMsecs(1000.0*seconds_to_wait);
} else { // This is a command line, send it
sprintf(string_to_send, "%s\015", next_line);
fprintf(stderr, " ...sending command: %s\n", string_to_send);
vrpn_write_characters(serial_fd,
(const unsigned char *)string_to_send,strlen(string_to_send));
}
next_line = strtok(next_line+strlen(next_line)+1, "\015");
}
// Sleep a little while to let this finish, then clear the input buffer
vrpn_SleepMsecs(1000.0*2);
vrpn_flush_input_buffer(serial_fd);
}
// Set data format to BINARY mode
sprintf(outstring1, "F1\r");
if (vrpn_write_characters(serial_fd, (const unsigned char *)outstring1,
strlen(outstring1)) == (int)strlen(outstring1)) {
fprintf(stderr, " Liberty set to binary mode\n");
}
// Set tracker to continuous mode
sprintf(outstring3, "C\r");
if (vrpn_write_characters(serial_fd, (const unsigned char *)outstring3,
strlen(outstring3)) != (int)strlen(outstring3)) {
perror(" Liberty write failed");
status = vrpn_TRACKER_FAIL;
return;
} else {
fprintf(stderr, " Liberty set to continuous mode\n");
}
// If we are using the Liberty timestamps, clear the timer on the device and
// store the time when we cleared it. First, drain any characters in the output
// buffer to ensure we're sending right away. Then, send the reset command and
// store the time that we sent it, plus the estimated time for the characters to
// get across the serial line to the device at the current baud rate.
// Set time units to milliseconds (MT) and reset the time (MZ).
char clear_timestamp_cmd[] = "Q0\r";
vrpn_drain_output_buffer(serial_fd);
if (vrpn_write_characters(serial_fd, (const unsigned char *)clear_timestamp_cmd,
strlen(clear_timestamp_cmd)) != (int)strlen(clear_timestamp_cmd)) {
VRPN_MSG_ERROR("Cannot send command to clear timestamp");
status = vrpn_TRACKER_FAIL;
return;
}
// Drain the output buffer again, then record the time as the base time from
// the tracker.
vrpn_drain_output_buffer(serial_fd);
vrpn_gettimeofday(&liberty_zerotime, NULL);
// Done with reset.
vrpn_gettimeofday(&watchdog_timestamp, NULL); // Set watchdog now
VRPN_MSG_WARNING("Reset Completed (this is good)");
status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
}
int vrpn_Nikon_Controls::get_report(void)
{
int ret; // Return value from function call to be checked
//--------------------------------------------------------------------
// If we're SYNCing, then the next character we get should be the start
// of a report. If we recognize it, go into READing mode and tell how
// many characters we expect total. If we don't recognize it, then we
// must have misinterpreted a command or something; reset
// and start over
//--------------------------------------------------------------------
if (status == STATUS_SYNCING) {
// Try to get a character. If none, just return.
if (vrpn_read_available_characters(serial_fd, _buffer, 1) != 1) {
return 0;
}
// See if we recognize the character as one of the ones making
// up a report. If not, then we need to continue syncing.
if (strchr(VALID_REPORT_CHARS, _buffer[0]) == NULL) {
VRPN_MSG_WARNING("Syncing");
return 0;
}
// Got the first character of a report -- go into READING 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.
// The time stored here is as close as possible to when the
// report was generated.
_bufcount = 1;
vrpn_gettimeofday(×tamp, NULL);
status = STATUS_READING;
#ifdef VERBOSE2
printf("... Got the 1st char\n");
#endif
}
//--------------------------------------------------------------------
// Read a character at a time from the serial port, storing them
// in the buffer. Do this until we either run out of characters to
// read or else find [CR][LF] along the way, which indicates the end
// of a command.
//--------------------------------------------------------------------
do {
ret = vrpn_read_available_characters(serial_fd, &_buffer[_bufcount], 1);
if (ret == -1) {
VRPN_MSG_ERROR("Error reading");
status = STATUS_RESETTING;
return 0;
}
_bufcount += ret;
#ifdef VERBOSE2
if (ret != 0) printf("... got %d characters (%d total)\n",ret, _bufcount);
#endif
// See if the last characters in the buffer are [CR][LF]. If so, then
// we have a full report so null-terminate and go ahead and parse
if ( (_bufcount > 2) &&
(_buffer[_bufcount-2] == '\r') &&
(_buffer[_bufcount-1] == '\n') ) {
_buffer[_bufcount] = '\0';
break;
}
// If we have a full buffer, then we've gotten into a bad way.
if (_bufcount >= sizeof(_buffer) - 1) {
VRPN_MSG_ERROR("Buffer full when reading");
status = STATUS_RESETTING;
return 0;
}
} while (ret != 0);
if (ret == 0) {
return 0;
}
//--------------------------------------------------------------------
// We now have enough characters to make a full report. Check it by
// trying to parse it. If it is not valid, then we return to
// synch mode and ignore this report. A well-formed report has
// on of VALID_REPORT_CHARS as its command. We expect these responses
// either set the response position (due to a query) or to be valid
// but not set the response position (due to a position move request,
// for which we store the requested position into the value).
//--------------------------------------------------------------------
double response_pos;
ret = parse_focus_position_response((const char *)_buffer, response_pos);
if (ret <= 0) {
fprintf(stderr,"Bad response from nikon [%s], syncing\n", _buffer);
fprintf(stderr," (%s)\n", error_code_to_string((int)response_pos));
status = STATUS_SYNCING;
return 0;
}
// Type 3 returns mean "in progress" for some function. Ignore this
// report.
if (ret == 3) {
status = STATUS_SYNCING;
_bufcount = 0;
return 1;
}
// Type 2 means that the command we issued before has completed -- put
// the value we requested for the focus into the response since that is
// where we are now.
if (ret == 2) { // We must have gotten where we are going.
response_pos = _requested_focus;
}
//--------------------------------------------------------------------
// Done with the decoding, send the reports and go back to syncing
//--------------------------------------------------------------------
channel[0] = response_pos;
report_changes();
status = STATUS_SYNCING;
_bufcount = 0;
return 1;
}
void vrpn_Tracker_InterSense::reset()
{
#ifdef VRPN_INCLUDE_INTERSENSE
char errStr[1024];
int i;
m_Handle = ISD_OpenTracker(NULL,m_CommPort,FALSE,FALSE);
if(m_Handle == -1)
{
sprintf(errStr,"InterSense: Failed to open tracker '%s' on COM%d: ISD_OpenTracker returned -1",d_servicename,m_CommPort);
fprintf(stderr,errStr);
vrpn_gettimeofday(×tamp, NULL);
VRPN_MSG_ERROR(errStr);
status = vrpn_TRACKER_FAIL;
}
else
{
//--------------------------------------------------------------------
// Now that the tracker has given a valid status report, set all of
// the parameters the way we want them. We rely on power-up setting
// based on the receiver select switches to turn on the receivers that
// the user wants.
//--------------------------------------------------------------------
// Set output format for each of the possible stations.
for (i = 0; i < ISD_MAX_STATIONS; i++) {
if (set_sensor_output_format(i)) {
return;
}
}
// Send the additional reset commands, if any, to the tracker.
// These commands come in lines, with character \015 ending each
// line.
if (strlen(add_reset_cmd) > 0) {
printf(" Intersense writing extended reset commands...\n");
if(!ISD_SendScript(m_Handle,add_reset_cmd))
{
sprintf(errStr,"Warning: Your tracker failed executing the additional command string. ");
vrpn_gettimeofday(×tamp, NULL);
VRPN_MSG_WARNING(errStr);
}
}
// If we are using the IS-900 timestamps, clear the timer on the device and
// store the time when we cleared it. First, drain any characters in the output
// buffer to ensure we're sending right away. Then, send the reset command and
// store the time that we sent it, plus the estimated time for the characters to
// get across the serial line to the device at the current baud rate.
// Set time units to milliseconds (MT) and reset the time (MZ).
if (do_is900_timestamps) {
char clear_timestamp_cmd[] = "MT\015MZ\015";
if(!ISD_SendScript(m_Handle,clear_timestamp_cmd))
{
sprintf(errStr,"Warning: Your tracker failed executing the additional command string. ");
vrpn_gettimeofday(×tamp, NULL);
VRPN_MSG_WARNING(errStr);
}
vrpn_gettimeofday(&is900_zerotime, NULL);
}
// Done with reset.
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
VRPN_MSG_WARNING("Reset Completed (this is good)");
status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
}
#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
}
void vrpn_Tracker_Isotrak::reset()
{
static int numResets = 0; // How many resets have we tried?
int i,resetLen,ret;
unsigned char reset[10];
char errmsg[512];
//--------------------------------------------------------------------
// This section deals with resetting the tracker to its default state.
// Multiple attempts are made to reset, getting more aggressive each
// time. This section completes when the tracker reports a valid status
// message after the reset has completed.
//--------------------------------------------------------------------
// Send the tracker a string that should reset it. The first time we
// try this, just do the normal 'c' command to put it into polled mode.
// After a few tries with this, use a [return] character, and then use the ^Y to reset.
resetLen = 0;
numResets++;
// We're trying another reset
if (numResets > 1) { // Try to get it out of a query loop if its in one
reset[resetLen++] = (unsigned char) (13); // Return key -> get ready
}
if (numResets > 2) {
reset[resetLen++] = (unsigned char) (25); // Ctrl + Y -> reset the tracker
}
reset[resetLen++] = 'c'; // Put it into polled (not continuous) mode
sprintf(errmsg, "Resetting the tracker (attempt %d)", numResets);
VRPN_MSG_WARNING(errmsg);
for (i = 0; i < resetLen; i++) {
if (vrpn_write_characters(serial_fd, &reset[i], 1) == 1) {
fprintf(stderr,".");
vrpn_SleepMsecs(1000.0*2); // Wait after each character to give it time to respond
} else {
perror("Isotrack: Failed writing to tracker");
status = vrpn_TRACKER_FAIL;
return;
}
}
//XXX Take out the sleep and make it keep spinning quickly
if (numResets > 2) {
vrpn_SleepMsecs(1000.0*20); // Sleep to let the reset happen, if we're doing ^Y
}
fprintf(stderr,"\n");
// Get rid of the characters left over from before the reset
vrpn_flush_input_buffer(serial_fd);
// Make sure that the tracker has stopped sending characters
vrpn_SleepMsecs(1000.0*2);
unsigned char scrap[80];
if ( (ret = vrpn_read_available_characters(serial_fd, scrap, 80)) != 0) {
sprintf(errmsg,"Got >=%d characters after reset",ret);
VRPN_MSG_WARNING(errmsg);
for (i = 0; i < ret; i++) {
if (isprint(scrap[i])) {
fprintf(stderr,"%c",scrap[i]);
} else {
fprintf(stderr,"[0x%02X]",scrap[i]);
}
}
fprintf(stderr, "\n");
vrpn_flush_input_buffer(serial_fd); // Flush what's left
}
// Asking for tracker status
if (vrpn_write_characters(serial_fd, (const unsigned char *) "S", 1) == 1) {
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
} else {
perror(" Isotrack write failed");
status = vrpn_TRACKER_FAIL;
return;
}
// Read Status
unsigned char statusmsg[22];
// Attempt to read 21 characters.
ret = vrpn_read_available_characters(serial_fd, statusmsg, 21);
if ( (ret != 21) ) {
fprintf(stderr,
" Got %d of 21 characters for status\n",ret);
VRPN_MSG_ERROR("Bad status report from Isotrack, retrying reset");
return;
}
else if ( (statusmsg[0]!='2') ) {
int i;
statusmsg[sizeof(statusmsg) - 1] = '\0'; // Null-terminate the string
fprintf(stderr, " Isotrack: bad status (");
for (i = 0; i < ret; i++) {
if (isprint(statusmsg[i])) {
fprintf(stderr,"%c",statusmsg[i]);
} else {
fprintf(stderr,"[0x%02X]",statusmsg[i]);
}
}
fprintf(stderr,")\n");
VRPN_MSG_ERROR("Bad status report from Isotrack, retrying reset");
return;
} else {
VRPN_MSG_WARNING("Isotrack gives correct status (this is good)");
numResets = 0; // Success, use simple reset next time
}
//--------------------------------------------------------------------
// Now that the tracker has given a valid status report, set all of
// the parameters the way we want them. We rely on power-up setting
// based on the receiver select switches to turn on the receivers that
// the user wants.
//--------------------------------------------------------------------
// Set output format. This is done once for the Isotrak, not per channel.
if (set_sensor_output_format(0)) {
return;
}
// Enable filtering if the constructor parameter said to.
// Set filtering for both position (x command) and orientation (v command)
// to the values that are recommended as a "jumping off point" in the
// Isotrack manual.
if (do_filter) {
if (vrpn_write_characters(serial_fd, (const unsigned char *)"x0.2,0.2,0.8,0.8\015", 17) == 17) {
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
} else {
perror(" Isotrack write position filter failed");
status = vrpn_TRACKER_FAIL;
return;
}
if (vrpn_write_characters(serial_fd, (const unsigned char *)"v0.2,0.2,0.8,0.8\015", 17) == 17) {
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
} else {
perror(" Isotrack write orientation filter failed");
status = vrpn_TRACKER_FAIL;
return;
}
}
// RESET Alignment reference frame
if (vrpn_write_characters(serial_fd, (const unsigned char *) "R1\r", 3) != 3) {
perror(" Isotrack write failed");
status = vrpn_TRACKER_FAIL;
return;
} else {
VRPN_MSG_WARNING("Isotrack reset ALIGNMENT reference frame (this is good)");
}
// reset BORESIGHT
if (vrpn_write_characters(serial_fd, (const unsigned char *) "b1\r", 3) != 3) {
perror(" Isotrack write failed");
status = vrpn_TRACKER_FAIL;
return;
} else {
VRPN_MSG_WARNING("Isotrack reset BORESIGHT (this is good)");
}
// Set data format to METRIC mode
if (vrpn_write_characters(serial_fd, (const unsigned char *) "u", 1) != 1) {
perror(" Isotrack write failed");
status = vrpn_TRACKER_FAIL;
return;
} else {
VRPN_MSG_WARNING("Isotrack set to metric units (this is good)");
}
// Send the additional reset commands, if any, to the tracker.
// These commands come in lines, with character \015 ending each
// line. If a line start with an asterisk (*), treat it as a pause
// command, with the number of seconds to wait coming right after
// the asterisk. Otherwise, the line is sent directly to the tracker.
// Wait a while for them to take effect, then clear the input
// buffer.
if (strlen(add_reset_cmd) > 0) {
char *next_line;
char add_cmd_copy[sizeof(add_reset_cmd)];
char string_to_send[sizeof(add_reset_cmd)];
int seconds_to_wait;
printf(" Isotrack writing extended reset commands...\n");
// Make a copy of the additional reset string, since it is consumed
strncpy(add_cmd_copy, add_reset_cmd, sizeof(add_cmd_copy));
// Pass through the string, testing each line to see if it is
// a sleep command or a line to send to the tracker. Continue until
// there are no more line delimiters ('\015'). Be sure to write the
// \015 to the end of the string sent to the tracker.
// Note that strok() puts a NULL character in place of the delimiter.
next_line = strtok(add_cmd_copy, "\015");
while (next_line != NULL) {
if (next_line[0] == '*') { // This is a "sleep" line, see how long
seconds_to_wait = atoi(&next_line[1]);
fprintf(stderr," ...sleeping %d seconds\n",seconds_to_wait);
vrpn_SleepMsecs(1000.0*seconds_to_wait);
} else { // This is a command line, send it
sprintf(string_to_send, "%s\015", next_line);
fprintf(stderr, " ...sending command: %s\n", string_to_send);
vrpn_write_characters(serial_fd,
(const unsigned char *)string_to_send,strlen(string_to_send));
}
next_line = strtok(next_line+strlen(next_line)+1, "\015");
}
// Sleep a little while to let this finish, then clear the input buffer
vrpn_SleepMsecs(1000.0*2);
vrpn_flush_input_buffer(serial_fd);
}
// Set data format to BINARY mode
// F = ASCII, f = binary
if (vrpn_write_characters(serial_fd, (const unsigned char *) "f", 1) != 1) {
perror(" Isotrack write failed");
status = vrpn_TRACKER_FAIL;
return;
} else {
VRPN_MSG_WARNING("Isotrack set to BINARY mode (this is good)");
}
// Set tracker to continuous mode
if (vrpn_write_characters(serial_fd, (const unsigned char *) "C", 1) != 1) {
perror(" Isotrack write failed");
status = vrpn_TRACKER_FAIL;
return;
} else {
VRPN_MSG_WARNING("Isotrack set to continuous mode (this is good)");
}
VRPN_MSG_WARNING("Reset Completed.");
status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
// Ok, device is ready, we want to calibrate to sensor 1 current position/orientation
while(get_report() != 1);
// Done with reset.
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
}
int vrpn_Tracker_Isotrak::get_report(void)
{
char errmsg[512]; // Error message to send to VRPN
int ret; // Return value from function call to be checked
// The first byte of a binary record has the high order bit set
if (status == vrpn_TRACKER_SYNCING) {
// Try to get a character. If none, just return.
if (vrpn_read_available_characters(serial_fd, buffer, 1) != 1) {
return 0;
}
// The first byte of a record has the high order bit set
if(!(buffer[0] & 0x80)) {
sprintf(errmsg,"While syncing (looking for byte with high order bit set, "
"got '%x')", buffer[0]);
VRPN_MSG_WARNING(errmsg);
vrpn_flush_input_buffer(serial_fd);
return 0;
}
// Got the first byte of a report -- go into TRACKER_PARTIAL mode
// and record that we got one character at this time.
bufcount = 1;
vrpn_gettimeofday(×tamp, NULL);
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],
BINARY_RECORD_SIZE - bufcount);
if (ret == -1) {
VRPN_MSG_ERROR("Error reading report");
status = vrpn_TRACKER_FAIL;
return 0;
}
bufcount += ret;
if (bufcount < BINARY_RECORD_SIZE) { // Not done -- go back for more
return 0;
}
// We now have enough characters for a full report
// Check it to ensure we do not have a high bit set other
// than on the first byte
for(int i=1; i<BINARY_RECORD_SIZE; i++)
{
if (buffer[i] & 0x80) {
status = vrpn_TRACKER_SYNCING;
sprintf(errmsg,"Unexpected sync character in record");
VRPN_MSG_WARNING(errmsg);
//VRPN_MSG_WARNING("Not '0' in record, re-syncing");
vrpn_flush_input_buffer(serial_fd);
return 0;
}
}
// Create a buffer for the decoded message
unsigned char decoded[BINARY_RECORD_SIZE];
int d = 0;
int fullgroups = BINARY_RECORD_SIZE / 8;
// The following decodes the Isotrak binary format. It consists of
// 7 byte values plus an extra byte of the high bit for these
// 7 bytes. First, loop over the 7 byte ranges (8 bytes in binary)
int i;
for(i = 0; i<fullgroups; i++)
{
vrpn_uint8 *group = &buffer[i * 8];
vrpn_uint8 high = buffer[i * 8 + 7];
for(int j=0; j<7; j++)
{
decoded[d] = *group++;
if(high & 1)
decoded[d] |= 0x80;
d++;
high >>= 1;
}
}
// We'll have X bytes left at the end
int left = BINARY_RECORD_SIZE - fullgroups * 8;
vrpn_uint8 *group = &buffer[fullgroups * 8];
vrpn_uint8 high = buffer[fullgroups * 8 + left - 1];
for(int j=0; j<left-1; j++)
{
decoded[d] = *group++;
if(high & 1)
decoded[d] |= 0x80;
d++;
high >>= 1;
}
// ASCII value of 1 == 49 subtracing 49 gives the sensor number
d_sensor = decoded[1] - 49; // 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_WARNING(errmsg);
vrpn_flush_input_buffer(serial_fd);
return 0;
}
// Extract the important information
vrpn_uint8 *item = &decoded[3];
// This is a scale factor from the Isotrak manual
// This will convert the values to meters, the standard vrpn format
double mul = 1.6632 / 32767.;
float div = 1.f / 32767.f; // Fractional amount for angles
pos[0] = ( (vrpn_int8(item[1]) << 8) + item[0]) * mul; item += 2;
pos[1] = ( (vrpn_int8(item[1]) << 8) + item[0]) * mul; item += 2;
pos[2] = ( (vrpn_int8(item[1]) << 8) + item[0]) * mul; item += 2;
d_quat[3] = ( (vrpn_int8(item[1]) << 8) + item[0]) * div; item += 2;
d_quat[0] = ( (vrpn_int8(item[1]) << 8) + item[0]) * div; item += 2;
d_quat[1] = ( (vrpn_int8(item[1]) << 8) + item[0]) * div; item += 2;
d_quat[2] = ( (vrpn_int8(item[1]) << 8) + item[0]) * div;
//--------------------------------------------------------------------
// 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] != NULL)
{
char button = decoded[2];
if(button == '@' || button == '*')
{
stylus_buttons[d_sensor]->set_button(0, button == '@');
}
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 creates a vrpn_3DMicroscribe and sets it to reset mode.
vrpn_3DMicroscribe::vrpn_3DMicroscribe (const char * name, vrpn_Connection * c,
const char * Port, long int BaudRate,
float OffsetX/* = 0.0f*/,
float OffsetY/* = 0.0f*/,
float OffsetZ/* = 0.0f*/,
float Scale/*=1.0f*/):
vrpn_Tracker(name, c),
vrpn_Button_Filter(name, c),
_numbuttons(2)
{
// Set the parameters in the parent classes
vrpn_Button::num_buttons = _numbuttons;
if(!strcmp(Port, "COM1") )
m_PortNumber=1;
else if(!strcmp(Port, "COM2") )
m_PortNumber=2;
else if(!strcmp(Port, "COM3") )
m_PortNumber=3;
else if(!strcmp(Port, "COM4") )
m_PortNumber=4;
m_BaudRate=BaudRate;
m_OffSet[0]=OffsetX; m_OffSet[1]=OffsetY; m_OffSet[2]=OffsetZ;
m_Scale=Scale;
// Set the status of the buttons and analogs to 0 to start
clear_values();
#ifdef VRPN_USE_MICROSCRIBE
int iResult;
iResult=ArmStart(NULL);
if(ARM_SUCCESS != iResult)
{
//error starting the MicroScribe drivers
VRPN_MSG_ERROR( "Unable to start MicroScribe ArmDll32." );
return;
}
//don't use error handlers
iResult = ArmSetErrorHandlerFunction(NO_HCI_HANDLER, NULL);
iResult = ArmSetErrorHandlerFunction(BAD_PORT_HANDLER, NULL);
iResult = ArmSetErrorHandlerFunction(CANT_OPEN_HANDLER, NULL);
iResult = ArmSetErrorHandlerFunction(CANT_BEGIN_HANDLER, NULL);
//connect to the correct port
switch(m_PortNumber)
{
case 1:
iResult = ArmConnect(1, m_BaudRate);
break;
case 2:
iResult = ArmConnect(2, m_BaudRate);
break;
case 3:
iResult = ArmConnect(3, m_BaudRate);
break;
case 4:
iResult = ArmConnect(4, m_BaudRate);
break;
default:
iResult = ArmConnect(0, 0); //try all available ports and baud rates
break;
}
if(ARM_SUCCESS != iResult)
{
//error connecting, end the thread
ArmEnd();
VRPN_MSG_ERROR( "Unable to connect to the MicroScribe." );
return;
}
#endif
// Set the mode to reset
status = STATUS_RESETTING;
}
/******************************************************************************
* 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
}
/******************************************************************************
* 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;
}
}