// vrpn_Tracker_Isotrak.C

// This file contains the code to operate a Polhemus Isotrack Tracker.

// This file is based on the vrpn_Tracker_Fastrack.C file, with modifications made

// to allow it to operate a Isotrack instead. The modifications are based

// on the old version of the Isotrack driver.

// This version was written in the Spring 2006 by Bruno Herbelin.

//

// Revised by Charles B. Owen (cbowen@cse.msu.edu) 1-20-2012

// Revisions:

//

// The version before was reporting in centimeters. Revised to report in Meters per VRPN requirements.

// The version before negated the coordinates. I have no idea why. That has been fixed.

// Now reads the binary format instead of ASCII. It's faster and supports the stylus button.

// Now supports a stylus button for either channel.

#include <ctype.h> // for isprint

#include <stdio.h> // for fprintf, perror, sprintf, etc

#include <stdlib.h> // for atoi

#include <string.h> // for strlen, strncpy, strtok

#include "vrpn_BaseClass.h" // for ::vrpn_TEXT_WARNING, etc

#include "vrpn_Button.h" // for vrpn_Button_Server

#include "vrpn_Connection.h" // for vrpn_Connection

#include "vrpn_Serial.h" // for vrpn_write_characters, etc

#include "vrpn_Shared.h" // for vrpn_SleepMsecs, timeval, etc

#include "vrpn_Tracker.h" // for vrpn_TRACKER_FAIL, etc

#include "vrpn_Tracker_Isotrak.h"

#include "vrpn_Types.h" // for vrpn_uint8, vrpn_float64, etc

const int BINARY_RECORD_SIZE = 20;

#define FT_INFO(msg) { send_text_message(msg, timestamp, vrpn_TEXT_NORMAL) ; if (d_connection && d_connection->connected()) d_connection->send_pending_reports(); }

#define FT_WARNING(msg) { send_text_message(msg, timestamp, vrpn_TEXT_WARNING) ; if (d_connection && d_connection->connected()) d_connection->send_pending_reports(); }

#define FT_ERROR(msg) { send_text_message(msg, timestamp, vrpn_TEXT_ERROR) ; if (d_connection && d_connection->connected()) d_connection->send_pending_reports(); }

vrpn_Tracker_Isotrak::vrpn_Tracker_Isotrak(const char *name, vrpn_Connection *c,

const char *port, long baud, int enable_filtering, int numstations,

const char *additional_reset_commands) :

vrpn_Tracker_Serial(name,c,port,baud),

do_filter(enable_filtering),

num_stations(numstations>vrpn_ISOTRAK_MAX_STATIONS ? vrpn_ISOTRAK_MAX_STATIONS : numstations)

{

reset_time.tv_sec = reset_time.tv_usec = 0;

if (additional_reset_commands == NULL) {

sprintf(add_reset_cmd, "");

} else {

strncpy(add_reset_cmd, additional_reset_commands, sizeof(add_reset_cmd)-1);

}

// Initially, set to no buttons

for(int i=0; i<vrpn_ISOTRAK_MAX_STATIONS; i++) {

stylus_buttons[i] = NULL;

}

}

vrpn_Tracker_Isotrak::~vrpn_Tracker_Isotrak()

{

}

/** This routine sets the device for position + quaternion

It puts a space at the end so that we can check to make

sure we have complete good records for each report.

Returns 0 on success and -1 on failure.

*/

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 {

FT_ERROR("Write failed on format command");

status = vrpn_TRACKER_FAIL;

return -1;

}

return 0;

}

// This routine will reset the tracker and set it to generate the types

// of reports we want.

// This was based on the Isotrak User Manual from Polhemus (2001 Edition, Rev A)

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);

FT_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);

FT_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);

FT_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");

FT_ERROR("Bad status report from Isotrack, retrying reset");

return;

} else {

FT_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 {

FT_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 {

FT_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 {

FT_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 {

FT_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 {

FT_WARNING("Isotrack set to continuous mode (this is good)");

}

FT_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(&timestamp, NULL); // Set watchdog now

status = vrpn_TRACKER_SYNCING; // We're trying for a new reading

}

// 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. The time stored is that of

// the first character received as part of the report. Reports start with

// the header "0xy", where x is the station number and y is either the

// space character or else one of the characters "A-F". Characters "A-F"

// indicate weak signals and so forth, but in practice it is much harder

// to deal with them than to ignore them (they don't indicate hard error

// conditions). The report follows, 7 values in 7 characters each. The first three

// are position in X,Y and Z. The next four are the quaternion in the

// order W, X,Y,Z.

// If we get a report that is not valid, we assume that we have lost a

// character or something and re-synchronize with the Isotrack by waiting

// until the start-of-report character ('0') comes around again.

// 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).

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]);

FT_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(&timestamp, 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) {

FT_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");

FT_WARNING(errmsg);

//FT_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);

FT_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 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.

stylus_buttons[sensor] = new vrpn_Button_Server(button_device_name, d_connection, 1);

if (stylus_buttons[sensor] == NULL)

{

FT_ERROR("Cannot open button device");

return -1;

}

return 0;

}