vrpn  07.33
Virtual Reality Peripheral Network
vrpn_Tracker_Fastrak.C
Go to the documentation of this file.
1 // vrpn_Tracker_Fastrak.C
2 // This file contains the code to operate a Polhemus Fastrak Tracker.
3 // This file is based on the vrpn_3Space.C file, with modifications made
4 // to allow it to operate a Fastrak instead. The modifications are based
5 // on the old version of the Fastrak driver, which had been mainly copied
6 // from the Trackerlib driver and was very difficult to understand.
7 // This version was written in the Summer of 1999 by Russ Taylor.
8 // Modifications were made to this version to allow it to run the
9 // Intersense IS600 tracker; basically, this involved allowing the user
10 // to set extra things in the reset routine, and to pause to let the tracker
11 // handle the parameter-setting commands.
12 // Modifications were later made to support the IS-900 trackers,
13 // including wands and styli.
14 
15 #include <ctype.h> // for isprint, isalpha
16 #include <stdio.h> // for fprintf, sprintf, stderr, etc
17 #include <stdlib.h> // for atoi
18 #include <string.h> // for strlen, strncpy, strtok
19 
20 #include "quat.h" // for Q_W, Q_X, Q_Y, Q_Z
21 #include "vrpn_Analog.h" // for vrpn_Clipping_Analog_Server
22 #include "vrpn_BaseClass.h" // for ::vrpn_TEXT_ERROR, 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_MessageMacros.h" // for VRPN_MSG_INFO, VRPN_MSG_WARNING, VRPN_MSG_ERROR
29 #include "vrpn_Tracker_Fastrak.h"
30 
31 #define INCHES_TO_METERS (2.54/100.0)
32 
34  const char *port, long baud, int enable_filtering, int numstations,
35  const char *additional_reset_commands, int is900_timestamps) :
36  vrpn_Tracker_Serial(name,c,port,baud),
37  do_filter(enable_filtering),
38  num_stations(numstations>vrpn_FASTRAK_MAX_STATIONS ? vrpn_FASTRAK_MAX_STATIONS : numstations),
39  do_is900_timestamps(is900_timestamps),
40  num_resets(0)
41 {
42  int i;
43 
44  reset_time.tv_sec = reset_time.tv_usec = 0;
45  if (additional_reset_commands == NULL) {
46  sprintf(add_reset_cmd, "");
47  } else {
48  strncpy(add_reset_cmd, additional_reset_commands, sizeof(add_reset_cmd)-1);
49  }
50 
51  // Initially, set to no buttons or analogs on the stations. The
52  // methods to add buttons and analogs must be called to add them.
53  for (i = 0; i < num_stations; i++) {
54  is900_buttons[i] = NULL;
55  is900_analogs[i] = NULL;
56  }
57 
58  // Let me assume I am an Intersense till proven false,
59  // by an instance of a fastrak-only command like "FTStylus"
60  // in the initialization strings.
61  really_fastrak = false;
62 }
63 
65 {
66  int i;
67 
68  // Delete any button and analog devices that were created
69  for (i = 0; i < num_stations; i++) {
70  if (is900_buttons[i]) {
71  delete is900_buttons[i];
72  }
73  if (is900_analogs[i]) {
74  delete is900_analogs[i];
75  }
76  }
77 }
78 
89 {
90  char outstring[64];
91  const char *timestring;
92  const char *buttonstring;
93  const char *analogstring;
94 
95  // Set output format for the station to be position and quaternion,
96  // and any of the extended Fastrak (stylus with button) or
97  // IS900 states (timestamp, button, analog).
98  // This command is a capitol 'o' followed by the number of the
99  // station, then comma-separated values (2 for xyz, 11 for quat, 21 for
100  // timestamp, 22 for buttons, 16 for Fastrak stylus button,
101  // 23 for joystick, 0 for space) that
102  // indicate data sets, followed by character 13 (octal 15).
103  // Note that the sensor number has to be bumped to map to station number.
104 
105  timestring = do_is900_timestamps ? ",21" : "";
106  if (really_fastrak) {
107  buttonstring = is900_buttons[sensor] ? ",16" : "";
108  } else {
109  buttonstring = is900_buttons[sensor] ? ",22" : "";
110  }
111  analogstring = is900_analogs[sensor] ? ",23" : "";
112  sprintf(outstring, "O%d,2,11%s%s%s,0\015", sensor+1, timestring,
113  buttonstring, analogstring);
114  if (vrpn_write_characters(serial_fd, (const unsigned char *)outstring,
115  strlen(outstring)) == (int)strlen(outstring)) {
116  vrpn_SleepMsecs(50); // Sleep for a bit to let command run
117  } else {
118  VRPN_MSG_ERROR("Write failed on format command");
120  return -1;
121  }
122 
123  return 0;
124 }
125 
135 {
136  int len;
137 
138  len = 4; // Basic report, "0" plus Fastrak station char + status char + space at the end
139  len += 3*4; // Four bytes/float, 3 floats for position
140  len += 4*4; // Four bytes/float, 4 floats for quaternion
141 
142  // Add in the timestamp (4 bytes of float) if it is present
143  if (do_is900_timestamps) {
144  len += 4;
145  }
146 
147  // Add in the buttons (1 byte for IS900, 2 for Fastrak) if present
148  if (is900_buttons[sensor]) {
149  if (really_fastrak) {
150  len += 2;
151  } else {
152  len += 1;
153  }
154  }
155 
156  // Add in the joystick (one byte each for two values) if present
157  if (is900_analogs[sensor]) {
158  len += 2*1;
159  }
160 
161  return len;
162 }
163 
164 // This routine will reset the tracker and set it to generate the types
165 // of reports we want. It relies on the power-on configuration to set the
166 // active sensors based on the 'Rcvr Select Switch', as described on page
167 // 128 of the Fastrak manual printed November 1993.
168 
170 {
171  int i,resetLen,ret;
172  unsigned char reset[10];
173  char errmsg[512];
174 
175  //--------------------------------------------------------------------
176  // This section deals with resetting the tracker to its default state.
177  // Multiple attempts are made to reset, getting more aggressive each
178  // time. This section completes when the tracker reports a valid status
179  // message after the reset has completed.
180  //--------------------------------------------------------------------
181 
182  // Send the tracker a string that should reset it. The first time we
183  // try this, just do the normal 'c' command to put it into polled mode.
184  // after a few tries with this, use the ^Y reset. Later, try to reset
185  // to the factory defaults. Then toggle the extended mode.
186  // Then put in a carriage return to try and break it out of
187  // a query mode if it is in one. These additions are cumulative: by the
188  // end, we're doing them all.
189  resetLen = 0;
190  num_resets++; // We're trying another reset
191  if (num_resets > 1) { // Try to get it out of a query loop if its in one
192  reset[resetLen++] = (unsigned char) (13); // Return key -> get ready
193  }
194  if (num_resets > 5) {
195  reset[resetLen++] = 'Y'; // Put tracker into tracking (not point) mode
196  }
197  if (num_resets > 4) { // Even more aggressive
198  reset[resetLen++] = 't'; // Toggle extended mode (in case it is on)
199  }
200  /* XXX These commands are probably never needed, and can cause real
201  headaches for people who are keeping state in their trackers (especially
202  the InterSense trackers). Taking them out in version 05.01; you can put
203  them back in if your tracker isn't resetting as well.
204  if (num_resets > 3) { // Get a little more aggressive
205  reset[resetLen++] = 'W'; // Reset to factory defaults
206  reset[resetLen++] = (unsigned char) (11); // Ctrl + k --> Burn settings into EPROM
207  }
208  */
209  if (num_resets > 2) {
210  reset[resetLen++] = (unsigned char) (25); // Ctrl + Y -> reset the tracker
211  }
212  reset[resetLen++] = 'c'; // Put it into polled (not continuous) mode
213 
214  sprintf(errmsg, "Resetting the tracker (attempt %d)", num_resets);
215  VRPN_MSG_WARNING(errmsg);
216  for (i = 0; i < resetLen; i++) {
217  if (vrpn_write_characters(serial_fd, &reset[i], 1) == 1) {
218  fprintf(stderr,".");
219  vrpn_SleepMsecs(1000.0*2); // Wait after each character to give it time to respond
220  } else {
221  perror("Fastrak: Failed writing to tracker");
223  return;
224  }
225  }
226  //XXX Take out the sleep and make it keep spinning quickly
227  // You only need to sleep 10 seconds for an actual Fastrak.
228  // For the Intersense trackers, you need to sleep 20. So,
229  // sleeping 20 is the more general solution...
230  if (num_resets > 2) {
231  vrpn_SleepMsecs(1000.0*20); // Sleep to let the reset happen, if we're doing ^Y
232  }
233 
234  fprintf(stderr,"\n");
235 
236  // Get rid of the characters left over from before the reset
238 
239  // Make sure that the tracker has stopped sending characters
240  vrpn_SleepMsecs(1000.0*2);
241  unsigned char scrap[80];
242  if ( (ret = vrpn_read_available_characters(serial_fd, scrap, 80)) != 0) {
243  sprintf(errmsg,"Got >=%d characters after reset",ret);
244  VRPN_MSG_WARNING(errmsg);
245  for (i = 0; i < ret; i++) {
246  if (isprint(scrap[i])) {
247  fprintf(stderr,"%c",scrap[i]);
248  } else {
249  fprintf(stderr,"[0x%02X]",scrap[i]);
250  }
251  }
252  fprintf(stderr, "\n");
253  vrpn_flush_input_buffer(serial_fd); // Flush what's left
254  }
255 
256  // Asking for tracker status
257  if (vrpn_write_characters(serial_fd, (const unsigned char *) "S", 1) == 1) {
258  vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
259  } else {
260  perror(" Fastrak write failed");
262  return;
263  }
264 
265  // Read Status
266  unsigned char statusmsg[56];
267 
268  // Attempt to read 55 characters. For some reason, later versions of the
269  // InterSense IS900 only report a 54-character status message. If this
270  // happens, handle it.
271  ret = vrpn_read_available_characters(serial_fd, statusmsg, 55);
272  if ( (ret != 55) && (ret != 54) ) {
273  fprintf(stderr,
274  " Got %d of 55 characters for status (54 expected for IS900)\n",ret);
275  }
276  if ( (statusmsg[0]!='2') || (statusmsg[ret-1]!=(char)(10)) ) {
277  int i;
278  statusmsg[55] = '\0'; // Null-terminate the string
279  fprintf(stderr, " Fastrak: status is (");
280  for (i = 0; i < ret; i++) {
281  if (isprint(statusmsg[i])) {
282  fprintf(stderr,"%c",statusmsg[i]);
283  } else {
284  fprintf(stderr,"[0x%02X]",statusmsg[i]);
285  }
286  }
287  fprintf(stderr,"\n)\n");
288  VRPN_MSG_ERROR("Bad status report from Fastrak, retrying reset");
289  return;
290  } else {
291  VRPN_MSG_WARNING("Fastrak/Isense gives status (this is good)");
292  num_resets = 0; // Success, use simple reset next time
293  }
294 
295  //--------------------------------------------------------------------
296  // Now that the tracker has given a valid status report, set all of
297  // the parameters the way we want them. We rely on power-up setting
298  // based on the receiver select switches to turn on the receivers that
299  // the user wants.
300  //--------------------------------------------------------------------
301 
302  // Set output format for each of the possible stations.
303 
304  for (i = 0; i < num_stations; i++) {
305  if (set_sensor_output_format(i)) {
306  return;
307  }
308  }
309 
310  if (really_fastrak) {
311  char outstring[64];
312  sprintf(outstring, "e1,0\r");
313  if (vrpn_write_characters(serial_fd, (const unsigned char *)outstring,
314  strlen(outstring)) == (int)strlen(outstring)) {
315  vrpn_SleepMsecs(50); // Sleep for a bit to let command run
316  } else {
317  VRPN_MSG_ERROR("Write failed on mouse format command");
319  }
320  }
321 
322  // Enable filtering if the constructor parameter said to.
323  // Set filtering for both position (x command) and orientation (v command)
324  // to the values that are recommended as a "jumping off point" in the
325  // Fastrak manual.
326 
327  if (do_filter) {
329  (const unsigned char *)"x0.2,0.2,0.8,0.8\015", 17) == 17) {
330  vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
331  } else {
332  perror(" Fastrak write position filter failed");
334  return;
335  }
337  (const unsigned char *)"v0.2,0.2,0.8,0.8\015", 17) == 17) {
338  vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
339  } else {
340  perror(" Fastrak write orientation filter failed");
342  return;
343  }
344  }
345 
346  // Send the additional reset commands, if any, to the tracker.
347  // These commands come in lines, with character \015 ending each
348  // line. If a line start with an asterisk (*), treat it as a pause
349  // command, with the number of seconds to wait coming right after
350  // the asterisk. Otherwise, the line is sent directly to the tracker.
351  // Wait a while for them to take effect, then clear the input
352  // buffer.
353  if (strlen(add_reset_cmd) > 0) {
354  char *next_line;
355  char add_cmd_copy[sizeof(add_reset_cmd)+1];
356  char string_to_send[sizeof(add_reset_cmd)+1];
357  int seconds_to_wait;
358 
359  printf(" Fastrak writing extended reset commands...\n");
360 
361  // Make a copy of the additional reset string, since it is consumed
362  strncpy(add_cmd_copy, add_reset_cmd, sizeof(add_cmd_copy));
363  add_cmd_copy[sizeof(add_cmd_copy)-1] = '\0';
364 
365  // Pass through the string, testing each line to see if it is
366  // a sleep command or a line to send to the tracker. Continue until
367  // there are no more line delimiters ('\015'). Be sure to write the
368  // \015 to the end of the string sent to the tracker.
369  // Note that strok() puts a NULL character in place of the delimiter.
370 
371  next_line = strtok(add_cmd_copy, "\015");
372  while (next_line != NULL) {
373  if (next_line[0] == '*') { // This is a "sleep" line, see how long
374  seconds_to_wait = atoi(&next_line[1]);
375  fprintf(stderr," ...sleeping %d seconds\n",seconds_to_wait);
376  vrpn_SleepMsecs(1000.0*seconds_to_wait);
377  } else { // This is a command line, send it
378  sprintf(string_to_send, "%s\015", next_line);
379  fprintf(stderr, " ...sending command: %s\n", string_to_send);
381  (const unsigned char *)string_to_send,strlen(string_to_send));
382  }
383  next_line = strtok(next_line+strlen(next_line)+1, "\015");
384  }
385 
386  // Sleep a little while to let this finish, then clear the input buffer
387  vrpn_SleepMsecs(1000.0*2);
389  }
390 
391  // Set data format to BINARY mode
392  vrpn_write_characters(serial_fd, (const unsigned char *)"f", 1);
393 
394  // Set tracker to continuous mode
395  if (vrpn_write_characters(serial_fd,(const unsigned char *) "C", 1) != 1) {
396  perror(" Fastrak write failed");
398  return;
399  } else {
400  fprintf(stderr, " Fastrak set to continuous mode\n");
401  }
402 
403  // If we are using the IS-900 timestamps, clear the timer on the device and
404  // store the time when we cleared it. First, drain any characters in the output
405  // buffer to ensure we're sending right away. Then, send the reset command and
406  // store the time that we sent it, plus the estimated time for the characters to
407  // get across the serial line to the device at the current baud rate.
408  // Set time units to milliseconds (MT) and reset the time (MZ).
409  if (do_is900_timestamps) {
410  char clear_timestamp_cmd[] = "MT\015MZ\015";
411 
413 
414  if (vrpn_write_characters(serial_fd, (const unsigned char *)clear_timestamp_cmd,
415  strlen(clear_timestamp_cmd)) != (int)strlen(clear_timestamp_cmd)) {
416  VRPN_MSG_ERROR("Cannot send command to clear timestamp");
418  return;
419  }
420 
421  // Drain the output buffer again, then record the time as the base time from
422  // the tracker.
425  }
426 
427  // Done with reset.
428  vrpn_gettimeofday(&timestamp, NULL); // Set watchdog now
429  VRPN_MSG_WARNING("Reset Completed (this is good)");
430  status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
431 }
432 
433 // This function will read characters until it has a full report, then
434 // put that report into the time, sensor, pos and quat fields so that it can
435 // be sent the next time through the loop. The time stored is that of
436 // the first character received as part of the report. Reports start with
437 // the header "0xy", where x is the station number and y is either the
438 // space character or else one of the characters "A-F". Characters "A-F"
439 // indicate weak signals and so forth, but in practice it is much harder
440 // to deal with them than to ignore them (they don't indicate hard error
441 // conditions). The report follows, 4 bytes per word in little-endian byte
442 // order; each word is an IEEE floating-point binary value. The first three
443 // are position in X,Y and Z. The next four are the unit quaternion in the
444 // order W, X,Y,Z. There are some optional fields for the Intersense 900
445 // tracker, then there is an ASCII space character at the end.
446 // If we get a report that is not valid, we assume that we have lost a
447 // character or something and re-synchronize with the Fastrak by waiting
448 // until the start-of-report character ('0') comes around again.
449 // The routine that calls this one makes sure we get a full reading often
450 // enough (ie, it is responsible for doing the watchdog timing to make sure
451 // the tracker hasn't simply stopped sending characters).
452 
454 {
455  char errmsg[512]; // Error message to send to VRPN
456  int ret; // Return value from function call to be checked
457  int i; // Loop counter
458  unsigned char *bufptr; // Points into buffer at the current value to read
459 
460  //--------------------------------------------------------------------
461  // Each report starts with an ASCII '0' character. If we're synching,
462  // read a byte at a time until we find a '0' character.
463  //--------------------------------------------------------------------
464 
465  if (status == vrpn_TRACKER_SYNCING) {
466  // Try to get a character. If none, just return.
468  return 0;
469  }
470 
471  // If it is not an '0', we don't want it but we
472  // need to look at the next one, so just return and stay
473  // in Syncing mode so that we will try again next time through.
474  // Also, flush the buffer so that it won't take as long to catch up.
475  if ( buffer[0] != '0') {
476  sprintf(errmsg,"While syncing (looking for '0', "
477  "got '%c')", buffer[0]);
478  VRPN_MSG_INFO(errmsg);
480  return 0;
481  }
482 
483  // Got the first character of a report -- go into AWAITING_STATION mode
484  // and record that we got one character at this time. The next
485  // bit of code will attempt to read the station.
486  // The time stored here is as close as possible to when the
487  // report was generated. For the InterSense 900 in timestamp
488  // mode, this value will be overwritten later.
489  bufcount = 1;
492  }
493 
494  //--------------------------------------------------------------------
495  // The second character of each report is the station number. Once
496  // we know this, we can compute how long the report should be for the
497  // given station, based on what values are in its report.
498  // The station number is converted into a VRPN sensor number, where
499  // the first Fastrak station is '1' and the first VRPN sensor is 0.
500  //--------------------------------------------------------------------
501 
503  // Try to get a character. If none, just return.
505  return 0;
506  }
507 
508  d_sensor = buffer[1] - '1'; // Convert ASCII 1 to sensor 0 and so on.
509  if ( (d_sensor < 0) || (d_sensor >= num_stations) ) {
511  sprintf(errmsg,"Bad sensor # (%d) in record, re-syncing", d_sensor);
512  VRPN_MSG_INFO(errmsg);
514  return 0;
515  }
516 
517  // Figure out how long the current report should be based on the
518  // settings for this sensor.
520 
521  // Got the station report -- to into PARTIAL mode and record
522  // that we got one character at this time. The next bit of code
523  // will attempt to read the rest of the report.
524  bufcount++;
526  }
527 
528  //--------------------------------------------------------------------
529  // Read as many bytes of this report as we can, storing them
530  // in the buffer. We keep track of how many have been read so far
531  // and only try to read the rest. The routine that calls this one
532  // makes sure we get a full reading often enough (ie, it is responsible
533  // for doing the watchdog timing to make sure the tracker hasn't simply
534  // stopped sending characters).
535  //--------------------------------------------------------------------
536 
539  if (ret == -1) {
540  VRPN_MSG_ERROR("Error reading report");
542  return 0;
543  }
544  bufcount += ret;
545  if (bufcount < REPORT_LEN) { // Not done -- go back for more
546  return 0;
547  }
548 
549  //--------------------------------------------------------------------
550  // We now have enough characters to make a full report. Check to make
551  // sure that its format matches what we expect. If it does, the next
552  // section will parse it. If it does not, we need to go back into
553  // synch mode and ignore this report. A well-formed report has the
554  // first character '0', the next character is the ASCII station
555  // number, and the third character is either a space or a letter.
556  //--------------------------------------------------------------------
557 
558  if (buffer[0] != '0') {
560  VRPN_MSG_INFO("Not '0' in record, re-syncing");
562  return 0;
563  }
564  // Sensor checking was handled when we received the character for it
565  if ( (buffer[2] != ' ') && !isalpha(buffer[2]) ) {
567  VRPN_MSG_INFO("Bad 3rd char in record, re-syncing");
569  return 0;
570  }
571  if (buffer[bufcount-1] != ' ') {
573  VRPN_MSG_INFO("No space character at end of report, re-syncing");
575  return 0;
576  }
577 
578  //--------------------------------------------------------------------
579  // Decode the X,Y,Z of the position and the W,X,Y,Z of the quaternion
580  // (keeping in mind that we store quaternions as X,Y,Z, W).
581  //--------------------------------------------------------------------
582  // The reports coming from the Fastrak are in little-endian order,
583  // which is the opposite of the network-standard byte order that is
584  // used by VRPN. Here we swap the order to big-endian so that the
585  // routines below can pull out the values in the correct order.
586  // This is slightly inefficient on machines that have little-endian
587  // order to start with, since it means swapping the values twice, but
588  // that is more than outweighed by the cleanliness gained by keeping
589  // all architecture-dependent code in the vrpn_Shared.C file.
590  //--------------------------------------------------------------------
591 
592  // Point at the first value in the buffer (position of the X value)
593  bufptr = &buffer[3];
594 
595  // When copying the positions, convert from inches to meters, since the
596  // Fastrak reports in inches and VRPN reports in meters.
597  pos[0] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr) * INCHES_TO_METERS;
598  pos[1] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr) * INCHES_TO_METERS;
599  pos[2] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr) * INCHES_TO_METERS;
600 
601  // Change the order of the quaternion fields to match quatlib order
602  d_quat[Q_W] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
603  d_quat[Q_X] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
604  d_quat[Q_Y] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
605  d_quat[Q_Z] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
606 
607  //--------------------------------------------------------------------
608  // If we are doing IS900 timestamps, decode the time, add it to the
609  // time we zeroed the tracker, and update the report time. Remember
610  // to convert the MILLIseconds from the report into MICROseconds and
611  // seconds.
612  //--------------------------------------------------------------------
613 
614  if (do_is900_timestamps) {
615  struct timeval delta_time; // Time since the clock was reset
616 
617  // Read the floating-point value of the time from the record.
618  vrpn_float32 read_time = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
619 
620  // Convert from the float in MILLIseconds to the struct timeval
621  delta_time.tv_sec = (long)(read_time / 1000); // Integer trunction to seconds
622  read_time -= delta_time.tv_sec * 1000; // Subtract out what we just counted
623  delta_time.tv_usec = (long)(read_time * 1000); // Convert remainder to MICROseconds
624 
625  // Store the current time
627  }
628 
629  //--------------------------------------------------------------------
630  // If this sensor has an IS900 button or fastrak button on it, decode
631  // the button values into the button device and mainloop the button
632  // device so that it will report any changes. Each button is stored
633  // in one bit of the byte, with the lowest-numbered button in the
634  // lowest bit.
635  //--------------------------------------------------------------------
636 
637  if (is900_buttons[d_sensor]) {
638  // the following section was modified by Debug to add support for Fastrak stylus button
639  int value;
640  if (really_fastrak) {
641  value=*(++bufptr)-'0'; // only one button, status in ASCII, skip over space
642  is900_buttons[d_sensor]->set_button(0, value);
643  } else {// potentially multiple buttons, values encoded in binary as a bit field
644  for (i = 0; i < is900_buttons[d_sensor]->number_of_buttons(); i++) {
645  value = ( (*bufptr) >> i) & 1;
646  is900_buttons[d_sensor]->set_button(i, value);
647  }
648  }
650 
651  bufptr++;
652  }
653 
654  //--------------------------------------------------------------------
655  // If this sensor has an IS900 analog on it, decode the analog values
656  // into the analog device and mainloop the analog device so that it
657  // will report any changes. The first byte holds the unsigned char
658  // representation of left/right. The second holds up/down. For each,
659  // 0 means min (left or rear), 127 means center and 255 means max.
660  //--------------------------------------------------------------------
661 
662  if (is900_analogs[d_sensor]) {
663 
664  // Read the raw values for the left/right and top/bottom channels
665  unsigned char raw_lr = *bufptr;
666  bufptr++;
667  unsigned char raw_tb = *bufptr;
668  bufptr++;
669 
670  // Normalize the values to the range -1 to 1
671  is900_analogs[d_sensor]->setChannelValue(0, (raw_lr - 127) / 128.0);
672  is900_analogs[d_sensor]->setChannelValue(1, (raw_tb - 127) / 128.0);
673 
674  // Report the new values
677  }
678 
679  //--------------------------------------------------------------------
680  // Done with the decoding, set the report to ready
681  //--------------------------------------------------------------------
682 
684  bufcount = 0;
685 
686 #ifdef VERBOSE2
688 #endif
689 
690  return 1;
691 }
692 
693 
702 int vrpn_Tracker_Fastrak::add_is900_button(const char *button_device_name, int sensor, int numbuttons)
703 {
704  // Make sure this is a valid sensor
705  if ( (sensor < 0) || (sensor >= num_stations) ) {
706  return -1;
707  }
708 
709  // Add a new button device and set the pointer to point at it.
710  is900_buttons[sensor] = new vrpn_Button_Server(button_device_name, d_connection, numbuttons);
711  if (is900_buttons[sensor] == NULL) {
712  VRPN_MSG_ERROR("Cannot open button device");
713  return -1;
714  }
715 
716  // Send a new station-format command to the tracker so it will report the button states.
717  return set_sensor_output_format(sensor);
718 }
719 
720 // this routine is called when an "FTStylus" button is encountered by the tracker init string parser
721 // it sets up the VRPN button device & enables the switch "really_fastrak" that affects subsequent interpretation
722 // of button readings - Debug
723 int vrpn_Tracker_Fastrak::add_fastrak_stylus_button(const char *button_device_name, int sensor, int numbuttons)
724 {
725  // Make sure this is a valid sensor
726  if ( (sensor < 0) || (sensor >= num_stations) ) {
727  return -1;
728  }
729 
730  // Add a new button device and set the pointer to point at it.
731  is900_buttons[sensor] = new vrpn_Button_Server(button_device_name, d_connection, numbuttons);
732  if (is900_buttons[sensor] == NULL) {
733  VRPN_MSG_ERROR("Cannot open button device");
734  return -1;
735  }
736 
737  // Debug's HACK here. Make sure it knows that it is a plain vanilla fastrak, so it will
738  // get the button output parameters right.
739  really_fastrak=true;
740 
741  // Send a new station-format command to the tracker so it will report the button states.
742  return set_sensor_output_format(sensor);
743 }
744 
745 
760 int vrpn_Tracker_Fastrak::add_is900_analog(const char *analog_device_name, int sensor,
761  double c0Min, double c0Low, double c0Hi, double c0Max,
762  double c1Min, double c1Low, double c1Hi, double c1Max)
763 {
764  // Make sure this is a valid sensor
765  if ( (sensor < 0) || (sensor >= num_stations) ) {
766  return -1;
767  }
768 
769  // Add a new analog device and set the pointer to point at it.
770  is900_analogs[sensor] = new vrpn_Clipping_Analog_Server(analog_device_name, d_connection);
771  if (is900_analogs[sensor] == NULL) {
772  VRPN_MSG_ERROR("Cannot open analog device");
773  return -1;
774  }
775 
776  // Set the analog to have two channels, and set its channels to 0 to start with
777  is900_analogs[sensor]->setNumChannels(2);
778  is900_analogs[sensor]->setChannelValue(0, 0.0);
779  is900_analogs[sensor]->setChannelValue(1, 0.0);
780 
781  // Set the scaling on the two channels.
782  is900_analogs[sensor]->setClipValues(0, c0Min, c0Low, c0Hi, c0Max);
783  is900_analogs[sensor]->setClipValues(1, c1Min, c1Low, c1Hi, c1Max);
784 
785  // Send a new station-format command to the tracker so it will report the analog status
786  return set_sensor_output_format(sensor);
787 }
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
class VRPN_API vrpn_Clipping_Analog_Server
int add_is900_button(const char *button_device_name, int sensor, int numbuttons=5)
Add an IS900 button device to one of the sensors This allows configuration of an InterSense IS-900...
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
Definition: vrpn_Serial.C:435
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...
virtual void report_changes(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY, const struct timeval time=vrpn_ANALOG_NOW)
Makes public the protected base class function.
Definition: vrpn_Analog.C:182
unsigned char buffer[VRPN_TRACKER_BUF_SIZE]
Definition: vrpn_Tracker.h:155
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
virtual void reset()
Reset the tracker.
vrpn_Tracker_Fastrak(const char *name, vrpn_Connection *c, const char *port="/dev/ttyS1", long baud=19200, int enable_filtering=1, int numstations=vrpn_FASTRAK_MAX_STATIONS, const char *additional_reset_commands=NULL, int is900_timestamps=0)
The constructor is given the name of the tracker (the name of the sender it should use)...
Generic connection class not specific to the transport mechanism.
virtual void mainloop()
For this server, the user must normally call report() or report_changes() directly. This mainloop() only takes care of the things any server object should do.
Definition: vrpn_Analog.h:114
#define VRPN_MSG_WARNING(msg)
int report_length(int sensor)
Augments the basic Fastrak report length to include IS900 features if needed.
int vrpn_drain_output_buffer(int comm)
Wait until all of the characters in the output buffer are sent, then return.
Definition: vrpn_Serial.C:485
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 set_sensor_output_format(int sensor)
Augments the basic Fastrak format to include IS900 features if needed.
#define INCHES_TO_METERS
virtual int get_report(void)
Gets a report if one is available, returns 0 if not, 1 if complete report.
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
Definition: vrpn_Serial.C:512
int add_is900_analog(const char *analog_device_name, int sensor, double c0Min=-1, double c0Low=0, double c0Hi=0, double c0Max=1, double c1Min=-1, double c1Low=0, double c1Hi=0, double c1Max=1)
Add the analog part of an IS900 joystick device to one of the sensors This allows configuration of an...
const int vrpn_TRACKER_PARTIAL
Definition: vrpn_Tracker.h:38
vrpn_Connection * d_connection
Connection that this object talks to.
const int vrpn_TRACKER_SYNCING
Definition: vrpn_Tracker.h:35
vrpn_Button_Server * is900_buttons[vrpn_FASTRAK_MAX_STATIONS]
vrpn_uint32 bufcount
Definition: vrpn_Tracker.h:157
#define VRPN_MSG_INFO(msg)
struct timeval is900_zerotime
int add_fastrak_stylus_button(const char *button_device_name, int sensor, int numbuttons=1)
const int vrpn_TRACKER_AWAITING_STATION
Definition: vrpn_Tracker.h:36
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
int setClipValues(int channel, double min, double lowzero, double highzero, double max)
Set the clipping values for the specified channel. min maps to -1, values between lowzero and highzer...
Definition: vrpn_Analog.C:225
vrpn_Clipping_Analog_Server * is900_analogs[vrpn_FASTRAK_MAX_STATIONS]
struct timeval timestamp
Definition: vrpn_Tracker.h:100
void print_latest_report(void)
Definition: vrpn_Tracker.C:306
virtual void mainloop()
Called once each time through the server program&#39;s mainloop to handle various functions (like setting...
Definition: vrpn_Button.C:470
int setChannelValue(int channel, double value)
This method should be used to set the value of a channel. It will be scaled and clipped as described ...
Definition: vrpn_Analog.C:253
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
timeval vrpn_TimevalSum(const timeval &tv1, const timeval &tv2)
Definition: vrpn_Shared.C:45
const int vrpn_FASTRAK_MAX_STATIONS
int number_of_buttons(void)
Tells how many buttons there are (may be clipped to MAX_BUTTONS)
Definition: vrpn_Button.C:468
vrpn_int32 setNumChannels(vrpn_int32 sizeRequested)
Sets the size of the array; returns the size actually set. (May be clamped to vrpn_CHANNEL_MAX) This ...
Definition: vrpn_Analog.C:195
#define VRPN_MSG_ERROR(msg)