vrpn  07.33
Virtual Reality Peripheral Network
vrpn_Tracker_Isotrak.C
Go to the documentation of this file.
1 // vrpn_Tracker_Isotrak.C
2 // This file contains the code to operate a Polhemus Isotrack Tracker.
3 // This file is based on the vrpn_Tracker_Fastrack.C file, with modifications made
4 // to allow it to operate a Isotrack instead. The modifications are based
5 // on the old version of the Isotrack driver.
6 // This version was written in the Spring 2006 by Bruno Herbelin.
7 //
8 // Revised by Charles B. Owen (cbowen@cse.msu.edu) 1-20-2012
9 // Revisions:
10 //
11 // The version before was reporting in centimeters. Revised to report in Meters per VRPN requirements.
12 // The version before negated the coordinates. I have no idea why. That has been fixed.
13 // Now reads the binary format instead of ASCII. It's faster and supports the stylus button.
14 // Now supports a stylus button for either channel.
15 
16 
17 #include <ctype.h> // for isprint
18 #include <stdio.h> // for fprintf, perror, sprintf, etc
19 #include <stdlib.h> // for atoi
20 #include <string.h> // for strlen, strncpy, strtok
21 
22 #include "vrpn_BaseClass.h" // for ::vrpn_TEXT_WARNING, etc
23 #include "vrpn_Button.h" // for vrpn_Button_Server
24 #include "vrpn_Connection.h" // for vrpn_Connection
25 #include "vrpn_Serial.h" // for vrpn_write_characters, etc
26 #include "vrpn_Shared.h" // for vrpn_SleepMsecs, timeval, etc
27 #include "vrpn_Tracker.h" // for vrpn_TRACKER_FAIL, etc
28 #include "vrpn_Tracker_Isotrak.h"
29 #include "vrpn_Types.h" // for vrpn_uint8, vrpn_float64, etc
30 #include "vrpn_MessageMacros.h" // for VRPN_MSG_INFO, VRPN_MSG_WARNING, VRPN_MSG_ERROR
31 
32 const int BINARY_RECORD_SIZE = 20;
33 
35  const char *port, long baud, int enable_filtering, int numstations,
36  const char *additional_reset_commands) :
37  vrpn_Tracker_Serial(name,c,port,baud),
38  do_filter(enable_filtering),
39  num_stations(numstations>vrpn_ISOTRAK_MAX_STATIONS ? vrpn_ISOTRAK_MAX_STATIONS : numstations),
40  num_resets(0)
41 {
42  reset_time.tv_sec = reset_time.tv_usec = 0;
43  if (additional_reset_commands == NULL) {
44  sprintf(add_reset_cmd, "");
45  } else {
46  strncpy(add_reset_cmd, additional_reset_commands, sizeof(add_reset_cmd)-1);
47  }
48 
49  // Initially, set to no buttons
50  for(int i=0; i<vrpn_ISOTRAK_MAX_STATIONS; i++) {
51  stylus_buttons[i] = NULL;
52  }
53 }
54 
56 {
57 
58 }
59 
60 
69 {
70  char outstring[16];
71 
72  // Set output format for the station to be position, quaternion
73  // Don't need the space anymore, though
74  sprintf(outstring, "O2,11\r");
75 
76  if (vrpn_write_characters(serial_fd, (const unsigned char *)outstring,
77  strlen(outstring)) == (int)strlen(outstring)) {
78  vrpn_SleepMsecs(50); // Sleep for a bit to let command run
79  } else {
80  VRPN_MSG_ERROR("Write failed on format command");
82  return -1;
83  }
84  return 0;
85 }
86 
87 // This routine will reset the tracker and set it to generate the types
88 // of reports we want.
89 // This was based on the Isotrak User Manual from Polhemus (2001 Edition, Rev A)
90 
92 {
93  int i,resetLen,ret;
94  unsigned char reset[10];
95  char errmsg[512];
96 
97  //--------------------------------------------------------------------
98  // This section deals with resetting the tracker to its default state.
99  // Multiple attempts are made to reset, getting more aggressive each
100  // time. This section completes when the tracker reports a valid status
101  // message after the reset has completed.
102  //--------------------------------------------------------------------
103 
104  // Send the tracker a string that should reset it. The first time we
105  // try this, just do the normal 'c' command to put it into polled mode.
106  // After a few tries with this, use a [return] character, and then use the ^Y to reset.
107 
108  resetLen = 0;
109  num_resets++;
110 
111  // We're trying another reset
112  if (num_resets > 1) { // Try to get it out of a query loop if its in one
113  reset[resetLen++] = (unsigned char) (13); // Return key -> get ready
114  }
115 
116  if (num_resets > 2) {
117  reset[resetLen++] = (unsigned char) (25); // Ctrl + Y -> reset the tracker
118  }
119 
120  reset[resetLen++] = 'c'; // Put it into polled (not continuous) mode
121 
122 
123  sprintf(errmsg, "Resetting the tracker (attempt %d)", num_resets);
124  VRPN_MSG_WARNING(errmsg);
125 
126  for (i = 0; i < resetLen; i++) {
127  if (vrpn_write_characters(serial_fd, &reset[i], 1) == 1) {
128  fprintf(stderr,".");
129  vrpn_SleepMsecs(1000.0*2); // Wait after each character to give it time to respond
130  } else {
131  perror("Isotrack: Failed writing to tracker");
133  return;
134  }
135  }
136  //XXX Take out the sleep and make it keep spinning quickly
137  if (num_resets > 2) {
138  vrpn_SleepMsecs(1000.0*20); // Sleep to let the reset happen, if we're doing ^Y
139  }
140 
141  fprintf(stderr,"\n");
142 
143  // Get rid of the characters left over from before the reset
145 
146  // Make sure that the tracker has stopped sending characters
147  vrpn_SleepMsecs(1000.0*2);
148  unsigned char scrap[80];
149  if ( (ret = vrpn_read_available_characters(serial_fd, scrap, 80)) != 0) {
150  sprintf(errmsg,"Got >=%d characters after reset",ret);
151  VRPN_MSG_WARNING(errmsg);
152  for (i = 0; i < ret; i++) {
153  if (isprint(scrap[i])) {
154  fprintf(stderr,"%c",scrap[i]);
155  } else {
156  fprintf(stderr,"[0x%02X]",scrap[i]);
157  }
158  }
159  fprintf(stderr, "\n");
160  vrpn_flush_input_buffer(serial_fd); // Flush what's left
161  }
162 
163  // Asking for tracker status
164  if (vrpn_write_characters(serial_fd, (const unsigned char *) "S", 1) == 1) {
165  vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
166  } else {
167  perror(" Isotrack write failed");
169  return;
170  }
171 
172  // Read Status
173  unsigned char statusmsg[22];
174 
175  // Attempt to read 21 characters.
176  ret = vrpn_read_available_characters(serial_fd, statusmsg, 21);
177 
178  if ( (ret != 21) ) {
179  fprintf(stderr,
180  " Got %d of 21 characters for status\n",ret);
181  VRPN_MSG_ERROR("Bad status report from Isotrack, retrying reset");
182  return;
183  }
184  else if ( (statusmsg[0]!='2') ) {
185  int i;
186  statusmsg[sizeof(statusmsg) - 1] = '\0'; // Null-terminate the string
187  fprintf(stderr, " Isotrack: bad status (");
188  for (i = 0; i < ret; i++) {
189  if (isprint(statusmsg[i])) {
190  fprintf(stderr,"%c",statusmsg[i]);
191  } else {
192  fprintf(stderr,"[0x%02X]",statusmsg[i]);
193  }
194  }
195  fprintf(stderr,")\n");
196  VRPN_MSG_ERROR("Bad status report from Isotrack, retrying reset");
197  return;
198  } else {
199  VRPN_MSG_WARNING("Isotrack gives correct status (this is good)");
200  num_resets = 0; // Success, use simple reset next time
201  }
202 
203  //--------------------------------------------------------------------
204  // Now that the tracker has given a valid status report, set all of
205  // the parameters the way we want them. We rely on power-up setting
206  // based on the receiver select switches to turn on the receivers that
207  // the user wants.
208  //--------------------------------------------------------------------
209 
210  // Set output format. This is done once for the Isotrak, not per channel.
211  if (set_sensor_output_format(0)) {
212  return;
213  }
214 
215  // Enable filtering if the constructor parameter said to.
216  // Set filtering for both position (x command) and orientation (v command)
217  // to the values that are recommended as a "jumping off point" in the
218  // Isotrack manual.
219 
220  if (do_filter) {
221  if (vrpn_write_characters(serial_fd, (const unsigned char *)"x0.2,0.2,0.8,0.8\015", 17) == 17) {
222  vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
223  } else {
224  perror(" Isotrack write position filter failed");
226  return;
227  }
228  if (vrpn_write_characters(serial_fd, (const unsigned char *)"v0.2,0.2,0.8,0.8\015", 17) == 17) {
229  vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
230  } else {
231  perror(" Isotrack write orientation filter failed");
233  return;
234  }
235  }
236 
237  // RESET Alignment reference frame
238  if (vrpn_write_characters(serial_fd, (const unsigned char *) "R1\r", 3) != 3) {
239  perror(" Isotrack write failed");
241  return;
242  } else {
243  VRPN_MSG_WARNING("Isotrack reset ALIGNMENT reference frame (this is good)");
244  }
245 
246  // reset BORESIGHT
247  if (vrpn_write_characters(serial_fd, (const unsigned char *) "b1\r", 3) != 3) {
248  perror(" Isotrack write failed");
250  return;
251  } else {
252  VRPN_MSG_WARNING("Isotrack reset BORESIGHT (this is good)");
253  }
254 
255  // Set data format to METRIC mode
256  if (vrpn_write_characters(serial_fd, (const unsigned char *) "u", 1) != 1) {
257  perror(" Isotrack write failed");
259  return;
260  } else {
261  VRPN_MSG_WARNING("Isotrack set to metric units (this is good)");
262  }
263 
264 
265 
266  // Send the additional reset commands, if any, to the tracker.
267  // These commands come in lines, with character \015 ending each
268  // line. If a line start with an asterisk (*), treat it as a pause
269  // command, with the number of seconds to wait coming right after
270  // the asterisk. Otherwise, the line is sent directly to the tracker.
271  // Wait a while for them to take effect, then clear the input
272  // buffer.
273  if (strlen(add_reset_cmd) > 0) {
274  char *next_line;
275  char add_cmd_copy[sizeof(add_reset_cmd)];
276  char string_to_send[sizeof(add_reset_cmd)];
277  int seconds_to_wait;
278 
279  printf(" Isotrack writing extended reset commands...\n");
280 
281  // Make a copy of the additional reset string, since it is consumed
282  strncpy(add_cmd_copy, add_reset_cmd, sizeof(add_cmd_copy));
283  add_cmd_copy[sizeof(add_cmd_copy)-1] = '\0';
284 
285  // Pass through the string, testing each line to see if it is
286  // a sleep command or a line to send to the tracker. Continue until
287  // there are no more line delimiters ('\015'). Be sure to write the
288  // \015 to the end of the string sent to the tracker.
289  // Note that strok() puts a NULL character in place of the delimiter.
290 
291  next_line = strtok(add_cmd_copy, "\015");
292  while (next_line != NULL) {
293  if (next_line[0] == '*') { // This is a "sleep" line, see how long
294  seconds_to_wait = atoi(&next_line[1]);
295  fprintf(stderr," ...sleeping %d seconds\n",seconds_to_wait);
296  vrpn_SleepMsecs(1000.0*seconds_to_wait);
297  } else { // This is a command line, send it
298  sprintf(string_to_send, "%s\015", next_line);
299  fprintf(stderr, " ...sending command: %s\n", string_to_send);
301  (const unsigned char *)string_to_send,strlen(string_to_send));
302  }
303  next_line = strtok(next_line+strlen(next_line)+1, "\015");
304  }
305 
306  // Sleep a little while to let this finish, then clear the input buffer
307  vrpn_SleepMsecs(1000.0*2);
309  }
310 
311  // Set data format to BINARY mode
312  // F = ASCII, f = binary
313  if (vrpn_write_characters(serial_fd, (const unsigned char *) "f", 1) != 1) {
314  perror(" Isotrack write failed");
316  return;
317  } else {
318  VRPN_MSG_WARNING("Isotrack set to BINARY mode (this is good)");
319  }
320 
321 
322  // Set tracker to continuous mode
323  if (vrpn_write_characters(serial_fd, (const unsigned char *) "C", 1) != 1) {
324  perror(" Isotrack write failed");
326  return;
327  } else {
328  VRPN_MSG_WARNING("Isotrack set to continuous mode (this is good)");
329  }
330 
331  VRPN_MSG_WARNING("Reset Completed.");
332 
333  status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
334 
335  // Ok, device is ready, we want to calibrate to sensor 1 current position/orientation
336  while(get_report() != 1);
337 
338  // Done with reset.
339  vrpn_gettimeofday(&timestamp, NULL); // Set watchdog now
340 
341  status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
342 }
343 
344 
345 
346 // This function will read characters until it has a full report, then
347 // put that report into the time, sensor, pos and quat fields so that it can
348 // be sent the next time through the loop. The time stored is that of
349 // the first character received as part of the report. Reports start with
350 // the header "0xy", where x is the station number and y is either the
351 // space character or else one of the characters "A-F". Characters "A-F"
352 // indicate weak signals and so forth, but in practice it is much harder
353 // to deal with them than to ignore them (they don't indicate hard error
354 // conditions). The report follows, 7 values in 7 characters each. The first three
355 // are position in X,Y and Z. The next four are the quaternion in the
356 // order W, X,Y,Z.
357 // If we get a report that is not valid, we assume that we have lost a
358 // character or something and re-synchronize with the Isotrack by waiting
359 // until the start-of-report character ('0') comes around again.
360 // The routine that calls this one makes sure we get a full reading often
361 // enough (ie, it is responsible for doing the watchdog timing to make sure
362 // the tracker hasn't simply stopped sending characters).
363 
365 {
366  char errmsg[512]; // Error message to send to VRPN
367  int ret; // Return value from function call to be checked
368 
369  // The first byte of a binary record has the high order bit set
370 
371  if (status == vrpn_TRACKER_SYNCING) {
372  // Try to get a character. If none, just return.
374  return 0;
375  }
376 
377  // The first byte of a record has the high order bit set
378  if(!(buffer[0] & 0x80)) {
379  sprintf(errmsg,"While syncing (looking for byte with high order bit set, "
380  "got '%x')", buffer[0]);
381  VRPN_MSG_WARNING(errmsg);
383 
384  return 0;
385  }
386 
387  // Got the first byte of a report -- go into TRACKER_PARTIAL mode
388  // and record that we got one character at this time.
389  bufcount = 1;
392  }
393 
394  //--------------------------------------------------------------------
395  // Read as many bytes of this report as we can, storing them
396  // in the buffer. We keep track of how many have been read so far
397  // and only try to read the rest. The routine that calls this one
398  // makes sure we get a full reading often enough (ie, it is responsible
399  // for doing the watchdog timing to make sure the tracker hasn't simply
400  // stopped sending characters).
401  //--------------------------------------------------------------------
402 
405  if (ret == -1) {
406  VRPN_MSG_ERROR("Error reading report");
408  return 0;
409  }
410 
411  bufcount += ret;
412 
413  if (bufcount < BINARY_RECORD_SIZE) { // Not done -- go back for more
414  return 0;
415  }
416 
417  // We now have enough characters for a full report
418  // Check it to ensure we do not have a high bit set other
419  // than on the first byte
420  for(int i=1; i<BINARY_RECORD_SIZE; i++)
421  {
422  if (buffer[i] & 0x80) {
424 
425  sprintf(errmsg,"Unexpected sync character in record");
426  VRPN_MSG_WARNING(errmsg);
427 
428  //VRPN_MSG_WARNING("Not '0' in record, re-syncing");
430  return 0;
431  }
432  }
433 
434  // Create a buffer for the decoded message
435  unsigned char decoded[BINARY_RECORD_SIZE];
436  int d = 0;
437 
438  int fullgroups = BINARY_RECORD_SIZE / 8;
439 
440  // The following decodes the Isotrak binary format. It consists of
441  // 7 byte values plus an extra byte of the high bit for these
442  // 7 bytes. First, loop over the 7 byte ranges (8 bytes in binary)
443  int i;
444  for(i = 0; i<fullgroups; i++)
445  {
446  vrpn_uint8 *group = &buffer[i * 8];
447  vrpn_uint8 high = buffer[i * 8 + 7];
448 
449  for(int j=0; j<7; j++)
450  {
451  decoded[d] = *group++;
452  if(high & 1)
453  decoded[d] |= 0x80;
454 
455  d++;
456  high >>= 1;
457  }
458  }
459 
460  // We'll have X bytes left at the end
461  int left = BINARY_RECORD_SIZE - fullgroups * 8;
462  vrpn_uint8 *group = &buffer[fullgroups * 8];
463  vrpn_uint8 high = buffer[fullgroups * 8 + left - 1];
464 
465  for(int j=0; j<left-1; j++)
466  {
467  decoded[d] = *group++;
468  if(high & 1)
469  decoded[d] |= 0x80;
470 
471  d++;
472  high >>= 1;
473  }
474 
475  // ASCII value of 1 == 49 subtracing 49 gives the sensor number
476  d_sensor = decoded[1] - 49; // Convert ASCII 1 to sensor 0 and so on.
477  if ( (d_sensor < 0) || (d_sensor >= num_stations) ) {
479  sprintf(errmsg,"Bad sensor # (%d) in record, re-syncing", d_sensor);
480  VRPN_MSG_WARNING(errmsg);
482  return 0;
483  }
484 
485 
486 
487  // Extract the important information
488  vrpn_uint8 *item = &decoded[3];
489 
490  // This is a scale factor from the Isotrak manual
491  // This will convert the values to meters, the standard vrpn format
492  double mul = 1.6632 / 32767.;
493  float div = 1.f / 32767.f; // Fractional amount for angles
494 
495  pos[0] = ( (vrpn_int8(item[1]) << 8) + item[0]) * mul; item += 2;
496  pos[1] = ( (vrpn_int8(item[1]) << 8) + item[0]) * mul; item += 2;
497  pos[2] = ( (vrpn_int8(item[1]) << 8) + item[0]) * mul; item += 2;
498  d_quat[3] = ( (vrpn_int8(item[1]) << 8) + item[0]) * div; item += 2;
499  d_quat[0] = ( (vrpn_int8(item[1]) << 8) + item[0]) * div; item += 2;
500  d_quat[1] = ( (vrpn_int8(item[1]) << 8) + item[0]) * div; item += 2;
501  d_quat[2] = ( (vrpn_int8(item[1]) << 8) + item[0]) * div;
502 
503  //--------------------------------------------------------------------
504  // If this sensor has button on it, decode the button values
505  // into the button device and mainloop the button device so that
506  // it will report any changes.
507  //--------------------------------------------------------------------
508 
509  if(stylus_buttons[d_sensor] != NULL)
510  {
511  char button = decoded[2];
512  if(button == '@' || button == '*')
513  {
514  stylus_buttons[d_sensor]->set_button(0, button == '@');
515 
516  }
517 
519  }
520 
521  //--------------------------------------------------------------------
522  // Done with the decoding,
523  // set the report to ready
524  //--------------------------------------------------------------------
526  bufcount = 0;
527 
528  #ifdef VERBOSE2
530  #endif
531 
532  return 1;
533 }
534 
535 
536 // this routine is called when an "Stylus" button is encountered
537 // by the tracker init string parser it sets up the VRPN button
538 // device
539 int vrpn_Tracker_Isotrak::add_stylus_button(const char *button_device_name, int sensor)
540 {
541  // Make sure this is a valid sensor
542  if ( (sensor < 0) || (sensor >= num_stations) ) {
543  return -1;
544  }
545 
546  // Add a new button device and set the pointer to point at it.
547  stylus_buttons[sensor] = new vrpn_Button_Server(button_device_name, d_connection, 1);
548  if (stylus_buttons[sensor] == NULL)
549  {
550  VRPN_MSG_ERROR("Cannot open button device");
551  return -1;
552  }
553 
554  return 0;
555 }
556 
int add_stylus_button(const char *button_device_name, int sensor)
Add a stylus (with button) to one of the sensors.
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
Definition: vrpn_Serial.C:643
void vrpn_SleepMsecs(double dMsecs)
Definition: vrpn_Shared.C:157
int set_button(int button, int new_value)
Allows the server program to set current button states (to 0 or 1)
Definition: vrpn_Button.C:476
class VRPN_API vrpn_Button_Server
const int BINARY_RECORD_SIZE
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
Definition: vrpn_Serial.C:435
virtual int get_report(void)
Gets a report if one is available, returns 0 if not, 1 if complete report.
Header containing macros formerly duplicated in a lot of implementation files.
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
vrpn_Tracker_Isotrak(const char *name, vrpn_Connection *c, const char *port="/dev/ttyS1", long baud=19200, int enable_filtering=1, int numstations=vrpn_ISOTRAK_MAX_STATIONS, const char *additional_reset_commands=NULL)
The constructor is given the name of the tracker (the name of the sender it should use),...
int set_sensor_output_format(int sensor)
This routine sets the device for position + quaternion It puts a space at the end so that we can chec...
unsigned char buffer[VRPN_TRACKER_BUF_SIZE]
Definition: vrpn_Tracker.h:155
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
Generic connection class not specific to the transport mechanism.
#define VRPN_MSG_WARNING(msg)
const int vrpn_TRACKER_FAIL
Definition: vrpn_Tracker.h:40
vrpn_int32 d_sensor
Definition: vrpn_Tracker.h:94
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
Definition: vrpn_Serial.C:512
const int vrpn_TRACKER_PARTIAL
Definition: vrpn_Tracker.h:38
vrpn_Connection * d_connection
Connection that this object talks to.
vrpn_Button_Server * stylus_buttons[vrpn_ISOTRAK_MAX_STATIONS]
const int vrpn_TRACKER_SYNCING
Definition: vrpn_Tracker.h:35
vrpn_uint32 bufcount
Definition: vrpn_Tracker.h:157
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
struct timeval timestamp
Definition: vrpn_Tracker.h:100
void print_latest_report(void)
Definition: vrpn_Tracker.C:306
virtual void reset()
Reset the tracker.
virtual void mainloop()
Called once each time through the server program's mainloop to handle various functions (like setting...
Definition: vrpn_Button.C:470
const int vrpn_ISOTRAK_MAX_STATIONS
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
#define VRPN_MSG_ERROR(msg)