int vrpn_Tracker_NovintFalcon::get_report(void)
{
if (!m_dev)
return 0;
if (status == vrpn_TRACKER_SYNCING) {
if (m_dev->get_status(pos, vel, d_quat, vel_quat, &vel_quat_dt, buttons)) {
// if all buttons are pressed. we force a reset.
int i,j;
j=0;
for (i=0; i < num_buttons; i++)
j += buttons[i];
// all buttons pressed
if (j == num_buttons) {
status = vrpn_TRACKER_FAIL;
return 0;
}
} else {
status = vrpn_TRACKER_FAIL;
return 0;
}
}
status = vrpn_TRACKER_SYNCING;
#ifdef VERBOSE2
print_latest_report();
#endif
return 1;
}
int vrpn_Tracker_3Space::get_report(void)
{
int ret;
// The reports are each 20 characters long, and each start with a
// byte that has the high bit set and no other bytes have the high
// bit set. If we're synching, read a byte at a time until we find
// one with the high 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;
}
// If the high bit isn't set, we don't want it we
// need to look at the next one, so just return
if ( (buffer[0] & 0x80) == 0) {
send_text_message("Syncing (high bit not set)", timestamp, vrpn_TEXT_WARNING);
return 0;
}
// Got the first character of a report -- go into PARTIAL mode
// and say that we got one character at this time.
bufcount = 1;
vrpn_gettimeofday(×tamp, NULL);
status = vrpn_TRACKER_PARTIAL;
}
// Read as many bytes of this 20 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],
20-bufcount);
if (ret == -1) {
send_text_message("Error reading, resetting", timestamp, vrpn_TEXT_ERROR);
status = vrpn_TRACKER_FAIL;
return 0;
}
bufcount += ret;
if (bufcount < 20) { // Not done -- go back for more
return 0;
}
{ // Decode the report
unsigned char decode[17];
int i;
static unsigned char mask[8] = {0x01, 0x02, 0x04, 0x08,
0x10, 0x20, 0x40, 0x80 };
// Clear the MSB in the first byte
buffer[0] &= 0x7F;
// Decode the 3Space binary representation into standard
// 8-bit bytes. This is done according to page 4-4 of the
// 3Space user's manual, which says that the high-order bits
// of each group of 7 bytes is packed into the 8th byte of the
// group. Decoding involves setting those bits in the bytes
// iff their encoded counterpart is set and then skipping the
// byte that holds the encoded bits.
// We decode from buffer[] into decode[] (which is 3 bytes
// shorter due to the removal of the bit-encoding bytes).
// decoding from buffer[0-6] into decode[0-6]
for (i=0; i<7; i++) {
decode[i] = buffer[i];
if ( (buffer[7] & mask[i]) != 0) {
decode[i] |= (unsigned char)(0x80);
}
}
// decoding from buffer[8-14] into decode[7-13]
for (i=7; i<14; i++) {
decode[i] = buffer[i+1];
if ( (buffer[15] & mask[i-7]) != 0) {
decode[i] |= (unsigned char)(0x80);
}
}
// decoding from buffer[16-18] into decode[14-16]
for (i=14; i<17; i++) {
decode[i] = buffer[i+2];
if ( (buffer[19] & mask[i-14]) != 0) {
decode[i] |= (unsigned char)(0x80);
}
}
// Parse out sensor number, which is the second byte and is
// stored as the ASCII number of the sensor, with numbers
// starting from '1'. We turn it into a zero-based unit number.
d_sensor = decode[1] - '1';
// Position
for (i=0; i<3; i++) {
pos[i] = (* (short*)(&decode[3+2*i])) * T_3_BINARY_TO_METERS;
}
// Quarternion orientation. The 3Space gives quaternions
// as w,x,y,z while the VR code handles them as x,y,z,w,
// so we need to switch the order when decoding. Also the
// tracker does not normalize the quaternions.
d_quat[3] = (* (short*)(&decode[9]));
for (i=0; i<3; i++) {
d_quat[i] = (* (short*)(&decode[11+2*i]));
}
//Normalize quaternion
double norm = sqrt ( d_quat[0]*d_quat[0] + d_quat[1]*d_quat[1]
+ d_quat[2]*d_quat[2] + d_quat[3]*d_quat[3]);
for (i=0; i<4; i++) {
d_quat[i] /= norm;
}
// Done with the decoding, set the report to ready
// Ready for another report
status = vrpn_TRACKER_SYNCING;
bufcount = 0;
}
return 1; // Got a report.
#ifdef VERBOSE
print_latest_report();
#endif
}
int vrpn_Tracker_Liberty::get_report(void)
{
char errmsg[512]; // Error message to send to VRPN
int ret; // Return value from function call to be checked
unsigned char *bufptr; // Points into buffer at the current value to read
//--------------------------------------------------------------------
// 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;
}
int vrpn_Tracker_3DMouse::get_report(void)
{
int ret; // Return value from function call to be checked
static int count = 0;
timeval waittime;
waittime.tv_sec = 2;
if (status == vrpn_TRACKER_SYNCING) {
unsigned char tmpc;
if (vrpn_write_characters(serial_fd, (const unsigned char*)"*d", 2) !=2)
{
perror(" 3DMouse write command failed");
status = vrpn_TRACKER_RESETTING;
return 0;
}
ret = vrpn_read_available_characters(serial_fd, _buffer+count, 16-count, &waittime);
if (ret < 0)
{
perror(" 3DMouse read failed (disconnected)");
status = vrpn_TRACKER_RESETTING;
return 0;
}
count += ret;
if (count < 16) return 0;
if (count > 16)
{
perror(" 3DMouse read failed (wrong message)");
status = vrpn_TRACKER_RESETTING;
return 0;
}
count = 0;
tmpc = _buffer[0];
if (tmpc & 32)
{
//printf("port%d: Out of Range\n", i);
//ret |= 1 << (3-i);
} else
{
long ax, ay, az; // integer form of absolute translational data
short arx, ary, arz; // integer form of absolute rotational data
float p, y, r;
ax = (_buffer[1] & 0x40) ? 0xFFE00000 : 0;
ax |= (long)(_buffer[1] & 0x7f) << 14;
ax |= (long)(_buffer[2] & 0x7f) << 7;
ax |= (_buffer[3] & 0x7f);
ay = (_buffer[4] & 0x40) ? 0xFFE00000 : 0;
ay |= (long)(_buffer[4] & 0x7f) << 14;
ay |= (long)(_buffer[5] & 0x7f) << 7;
ay |= (_buffer[6] & 0x7f);
az = (_buffer[7] & 0x40) ? 0xFFE00000 : 0;
az |= (long)(_buffer[7] & 0x7f) << 14;
az |= (long)(_buffer[8] & 0x7f) << 7;
az |= (_buffer[9] & 0x7f);
pos[0] = static_cast<float>(ax / 100000.0 * 2.54);
pos[2] = static_cast<float>(ay / 100000.0 * 2.54);
pos[1] = -static_cast<float>(az / 100000.0f * 2.54);
arx = (_buffer[10] & 0x7f) << 7;
arx += (_buffer[11] & 0x7f);
ary = (_buffer[12] & 0x7f) << 7;
ary += (_buffer[13] & 0x7f);
arz = (_buffer[14] & 0x7f) << 7;
arz += (_buffer[15] & 0x7f);
p = static_cast<float>(arx / 40.0); // pitch
y = static_cast<float>(ary / 40.0); // yaw
r = static_cast<float>(arz / 40.0); // roll
p = static_cast<float>(p * M_PI / 180);
y = static_cast<float>(y * M_PI / 180);
r = static_cast<float>((360-r) * M_PI / 180);
float cosp2 = static_cast<float>(cos(p/2));
float cosy2 = static_cast<float>(cos(y/2));
float cosr2 = static_cast<float>(cos(r/2));
float sinp2 = static_cast<float>(sin(p/2));
float siny2 = static_cast<float>(sin(y/2));
float sinr2 = static_cast<float>(sin(r/2));
d_quat[0] = cosr2*sinp2*cosy2 + sinr2*cosp2*siny2;
d_quat[1] = sinr2*cosp2*cosy2 + cosr2*sinp2*siny2;
d_quat[2] = cosr2*cosp2*siny2 + sinr2*sinp2*cosy2;
d_quat[3] = cosr2*cosp2*cosy2 + sinr2*sinp2*siny2;
}
buttons[0] = tmpc & 16; // Mouse stand button
buttons[1] = tmpc & 8; // Suspend button
buttons[2] = tmpc & 4; // Left button
buttons[3] = tmpc & 2; // Middle button
buttons[4] = tmpc & 1; // Right button
}
vrpn_Button::report_changes();
status = vrpn_TRACKER_SYNCING;
bufcount = 0;
#ifdef VERBOSE2
print_latest_report();
#endif
return 1;
}
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;
}