vrpn  07.33
Virtual Reality Peripheral Network
vrpn_Tracker_Liberty.C
Go to the documentation of this file.
1 // vrpn_Tracker_Liberty.C
2 // This file contains the code to operate a Polhemus Liberty Tracker.
3 // This file is based on the vrpn_Tracker_Fastrak.C file.
4 
5 // Modified to work with the Polhemus Patriot as well.
6 
7 #include <ctype.h> // for isprint
8 #include <stdio.h> // for fprintf, stderr, sprintf, etc
9 #include <stdlib.h> // for atoi
10 #include <string.h> // for strlen, strncpy, strtok
11 
12 #include "quat.h" // for Q_W, Q_X, Q_Y, Q_Z
13 #include "vrpn_BaseClass.h" // for ::vrpn_TEXT_ERROR, etc
14 #include "vrpn_Button.h" // for vrpn_Button_Server
15 #include "vrpn_Connection.h" // for vrpn_Connection
16 #include "vrpn_Serial.h" // for vrpn_write_characters, etc
17 #include "vrpn_Shared.h" // for vrpn_SleepMsecs, timeval, etc
18 #include "vrpn_Tracker.h" // for vrpn_TRACKER_FAIL, etc
19 #include "vrpn_Tracker_Liberty.h"
20 #include "vrpn_MessageMacros.h" // for VRPN_MSG_INFO, VRPN_MSG_WARNING, VRPN_MSG_ERROR
21 
22 #define INCHES_TO_METERS (2.54/100.0)
23 const bool DEBUG = false; // General Debug Messages
24 const bool DEBUGA = false; // Only errors
25 
27  const char *port, long baud, int enable_filtering, int numstations,
28  const char *additional_reset_commands, int whoamilen) :
29  vrpn_Tracker_Serial(name,c,port,baud),
30  do_filter(enable_filtering),
31  num_stations(numstations>vrpn_LIBERTY_MAX_STATIONS ? vrpn_LIBERTY_MAX_STATIONS : numstations),
32  num_resets(0),
33  whoami_len(whoamilen>vrpn_LIBERTY_MAX_WHOAMI_LEN ? vrpn_LIBERTY_MAX_WHOAMI_LEN : whoamilen),
34  got_single_sync_char(0)
35 {
36  int i;
37 
38  reset_time.tv_sec = reset_time.tv_usec = 0;
39  if (additional_reset_commands == NULL) {
40  sprintf(add_reset_cmd, "");
41  } else {
42  strncpy(add_reset_cmd, additional_reset_commands, sizeof(add_reset_cmd)-1);
43  }
44 
45  // Initially, set to no buttons or analogs on the stations. The
46  // methods to add buttons and analogs must be called to add them.
47  for (i = 0; i < num_stations; i++) {
48  stylus_buttons[i] = NULL;
49  }
50 
51  if (DEBUG) fprintf(stderr,"[DEBUG] Constructed Liberty Object\n");
52 }
53 
55 {
56 
57 }
58 
68 {
69  char outstring[64];
70  const char *timestring;
71  const char *buttonstring;
72  const char *analogstring;
73 
74  // Set output format for the station to be position and quaternion,
75  // and any of the extended Liberty (stylus with button) or
76  // IS900 states (timestamp, button, analog).
77  // This command is a capitol 'o' followed by the number of the
78  // station, then comma-separated values (2 for xyz, 7 for quat, 8 for
79  // timestamp, 10 for buttons, 0 for space) that
80  // indicate data sets, followed by character 13 (octal 15).
81  // Note that the sensor number has to be bumped to map to station number.
82 
83  timestring = ",8";
84  buttonstring = stylus_buttons[sensor] ? ",10" : "";
85  analogstring="";
86 
87  sprintf(outstring, "O%d,2,7%s%s%s,0\015", sensor+1, timestring,
88  buttonstring, analogstring);
89 
90  if (DEBUG) fprintf(stderr,"[DEBUG]: %s \n",outstring);
91  if (vrpn_write_characters(serial_fd, (const unsigned char *)outstring,
92  strlen(outstring)) == (int)strlen(outstring)) {
93  vrpn_SleepMsecs(50); // Sleep for a bit to let command run
94  } else {
95  VRPN_MSG_ERROR("Write failed on format command");
97  return -1;
98  }
99 
100  return 0;
101  }
102 
111 {
112  int len;
113 
114  len = 9; // Basic report: Header information (8) + space at the end (1)
115  len += 3*4; // Four bytes/float, 3 floats for position
116  len += 4*4; // Four bytes/float, 4 floats for quaternion
117  len += 4; // Timestamp
118  len += stylus_buttons[sensor] ? 4 : 0; // If applicable, 4 bytes for button info
119 
120  return len;
121 }
122 
123 // This routine will reset the tracker and set it to generate the types
124 // of reports we want.
125 
127 {
128  int i,resetLen,ret;
129  char reset[10];
130  char errmsg[512];
131  char outstring1[64],outstring3[64];
132 
133  //--------------------------------------------------------------------
134  // This section deals with resetting the tracker to its default state.
135  // Multiple attempts are made to reset, getting more aggressive each
136  // time. This section completes when the tracker reports a valid status
137  // message after the reset has completed.
138  //--------------------------------------------------------------------
139 
140  // Send the tracker a string that should reset it. The first time we
141  // try this, just do the normal 'c' command to put it into polled mode.
142  // after a few tries with this, use the ^Y reset. Later, try to reset
143  // to the factory defaults. Then toggle the extended mode.
144  // Then put in a carriage return to try and break it out of
145  // a query mode if it is in one. These additions are cumulative: by the
146  // end, we're doing them all.
147  fprintf(stderr,"[DEBUG] Beginning Reset");
148  resetLen = 0;
149  num_resets++; // We're trying another reset
150  if (num_resets > 0) { // Try to get it out of a query loop if its in one
151  reset[resetLen++] = (char) (13); // Return key -> get ready
152  reset[resetLen++] = 'F';
153  reset[resetLen++] = '0';
154  reset[resetLen++] = (char) (13); // Return key -> get ready
155  // reset[resetLen++] = (char) (13);
156  // reset[resetLen++] = (char) (13);
157  }
158  /* XXX These commands are probably never needed, and can cause real
159  headaches for people who are keeping state in their trackers (especially
160  the InterSense trackers). Taking them out in version 05.01; you can put
161  them back in if your tracker isn't resetting as well.
162  if (num_resets > 3) { // Get a little more aggressive
163  reset[resetLen++] = 'W'; // Reset to factory defaults
164  reset[resetLen++] = (char) (11); // Ctrl + k --> Burn settings into EPROM
165  }
166  */
167  if (num_resets > 2) {
168  reset[resetLen++] = (char) (25); // Ctrl + Y -> reset the tracker
169  reset[resetLen++] = (char) (13); // Return Key
170  }
171  reset[resetLen++] = 'P'; // Put it into polled (not continuous) mode
172 
173  sprintf(errmsg, "Resetting the tracker (attempt %d)", num_resets);
174  VRPN_MSG_WARNING(errmsg);
175  for (i = 0; i < resetLen; i++) {
176  if (vrpn_write_characters(serial_fd, (unsigned char*)&reset[i], 1) == 1) {
177  fprintf(stderr,".");
178  vrpn_SleepMsecs(1000.0*2); // Wait after each character to give it time to respond
179  } else {
180  perror("Liberty: Failed writing to tracker");
182  return;
183  }
184  }
185  //XXX Take out the sleep and make it keep spinning quickly
186  // You only need to sleep 10 seconds for an actual Liberty.
187  // For the Intersense trackers, you need to sleep 20. So,
188  // sleeping 20 is the more general solution...
189  if (num_resets > 2) {
190  vrpn_SleepMsecs(1000.0*20); // Sleep to let the reset happen, if we're doing ^Y
191  }
192 
193  fprintf(stderr,"\n");
194 
195  // Get rid of the characters left over from before the reset
197 
198  // Make sure that the tracker has stopped sending characters
199  vrpn_SleepMsecs(1000.0*2);
200  unsigned char scrap[80];
201  if ( (ret = vrpn_read_available_characters(serial_fd, scrap, 80)) != 0) {
202  sprintf(errmsg,"Got >=%d characters after reset",ret);
203  VRPN_MSG_WARNING(errmsg);
204  for (i = 0; i < ret; i++) {
205  if (isprint(scrap[i])) {
206  fprintf(stderr,"%c",scrap[i]);
207  } else {
208  fprintf(stderr,"[0x%02X]",scrap[i]);
209  }
210  }
211  fprintf(stderr, "\n");
212  vrpn_flush_input_buffer(serial_fd); // Flush what's left
213  }
214 
215  // Asking for tracker status. S not implemented in Liberty and hence
216  // ^V (WhoAmI) is used. It retruns 196 bytes
217 
218  char statusCommand[2];
219  statusCommand[0]=(char)(22); // ^V
220  statusCommand[1]=(char)(13); // Return Key
221 
222  if (vrpn_write_characters(serial_fd, (const unsigned char *) &statusCommand[0], 2) == 2) {
223  vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
224  } else {
225  perror(" Liberty write failed");
227  return;
228  }
229 
230  // Read Status
231  unsigned char statusmsg[vrpn_LIBERTY_MAX_WHOAMI_LEN+1];
232 
233  // Attempt to read whoami_len characters.
235  if (ret != whoami_len) {
236  fprintf(stderr," Got %d of %d characters for status\n",ret, whoami_len);
237  }
238  // It seems like some versions of the tracker report longer
239  // messages; so we reduced this chech so that it does not check for the
240  // appropriate length of message or for the last character being a 10,
241  // so that it works more generally. The removed tests are:
242  // || (ret!=whoami_len) || (statusmsg[ret-1]!=(char)(10))
243  if ( (statusmsg[0]!='0') ) {
244  int i;
245  if (ret != -1) {
246  statusmsg[ret] = '\0'; // Null-terminate the string
247  }
248  fprintf(stderr, " Liberty: status is (");
249  for (i = 0; i < ret; i++) {
250  if (isprint(statusmsg[i])) {
251  fprintf(stderr,"%c",statusmsg[i]);
252  } else {
253  fprintf(stderr,"[0x%02X]",statusmsg[i]);
254  }
255  }
256  fprintf(stderr,"\n)\n");
257  VRPN_MSG_ERROR("Bad status report from Liberty, retrying reset");
258  return;
259  } else {
260  VRPN_MSG_WARNING("Liberty/Isense gives status (this is good)");
261 printf("LIBERTY LATUS STATUS (whoami):\n%s\n\n",statusmsg);
262  num_resets = 0; // Success, use simple reset next time
263  }
264 
265  //--------------------------------------------------------------------
266  // Now that the tracker has given a valid status report, set all of
267  // the parameters the way we want them. We rely on power-up setting
268  // based on the receiver select switches to turn on the receivers that
269  // the user wants.
270  //--------------------------------------------------------------------
271 
272  // Set output format for each of the possible stations.
273 
274  for (i = 0; i < num_stations; i++) {
275  if (set_sensor_output_format(i)) {
276  return;
277  }
278  }
279 
280  // Enable filtering if the constructor parameter said to.
281  // Set filtering for both position (X command) and orientation (Y command)
282  // to the values that are recommended as a "jumping off point" in the
283  // Liberty manual.
284 
285  if (do_filter) {
286  if (DEBUG) fprintf(stderr,"[DEBUG]: Enabling filtering\n");
287 
289  (const unsigned char *)"X0.2,0.2,0.8,0.8\015", 17) == 17) {
290  vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
291  } else {
292  perror(" Liberty write position filter failed");
294  return;
295  }
297  (const unsigned char *)"Y0.2,0.2,0.8,0.8\015", 17) == 17) {
298  vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
299  } else {
300  perror(" Liberty write orientation filter failed");
302  return;
303  }
304  } else {
305  if (DEBUG) fprintf(stderr,"[DEBUG]: Disabling filtering\n");
306 
308  (const unsigned char *)"X0,1,0,0\015", 9) == 9) {
309  vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
310  } else {
311  perror(" Liberty write position filter failed");
313  return;
314  }
316  (const unsigned char *)"Y0,1,0,0\015", 9) == 9) {
317  vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
318  } else {
319  perror(" Liberty write orientation filter failed");
321  return;
322  }
323  }
324 
325 
326  // Send the additional reset commands, if any, to the tracker.
327  // These commands come in lines, with character \015 ending each
328  // line. If a line start with an asterisk (*), treat it as a pause
329  // command, with the number of seconds to wait coming right after
330  // the asterisk. Otherwise, the line is sent directly to the tracker.
331  // Wait a while for them to take effect, then clear the input
332  // buffer.
333  if (strlen(add_reset_cmd) > 0) {
334  char *next_line;
335  char add_cmd_copy[sizeof(add_reset_cmd)];
336  char string_to_send[sizeof(add_reset_cmd)];
337  int seconds_to_wait;
338 
339  printf(" Liberty writing extended reset commands...\n");
340 
341  // Make a copy of the additional reset string, since it is consumed
342  strncpy(add_cmd_copy, add_reset_cmd, sizeof(add_cmd_copy));
343  add_cmd_copy[sizeof(add_cmd_copy)-1] = '\0';
344 
345  // Pass through the string, testing each line to see if it is
346  // a sleep command or a line to send to the tracker. Continue until
347  // there are no more line delimiters ('\015'). Be sure to write the
348  // \015 to the end of the string sent to the tracker.
349  // Note that strok() puts a NULL character in place of the delimiter.
350 
351  next_line = strtok(add_cmd_copy, "\015");
352  while (next_line != NULL) {
353  if (next_line[0] == '*') { // This is a "sleep" line, see how long
354  seconds_to_wait = atoi(&next_line[1]);
355  fprintf(stderr," ...sleeping %d seconds\n",seconds_to_wait);
356  vrpn_SleepMsecs(1000.0*seconds_to_wait);
357  } else { // This is a command line, send it
358  sprintf(string_to_send, "%s\015", next_line);
359  fprintf(stderr, " ...sending command: %s\n", string_to_send);
361  (const unsigned char *)string_to_send,strlen(string_to_send));
362  }
363  next_line = strtok(next_line+strlen(next_line)+1, "\015");
364  }
365 
366  // Sleep a little while to let this finish, then clear the input buffer
367  vrpn_SleepMsecs(1000.0*2);
369  }
370 
371  // Set data format to BINARY mode
372  sprintf(outstring1, "F1\r");
373  if (vrpn_write_characters(serial_fd, (const unsigned char *)outstring1,
374  strlen(outstring1)) == (int)strlen(outstring1)) {
375  fprintf(stderr, " Liberty set to binary mode\n");
376 
377  }
378 
379  // Set tracker to continuous mode
380  sprintf(outstring3, "C\r");
381  if (vrpn_write_characters(serial_fd, (const unsigned char *)outstring3,
382  strlen(outstring3)) != (int)strlen(outstring3)) {
383  perror(" Liberty write failed");
385  return;
386  } else {
387  fprintf(stderr, " Liberty set to continuous mode\n");
388  }
389 
390  // If we are using the Liberty timestamps, clear the timer on the device and
391  // store the time when we cleared it. First, drain any characters in the output
392  // buffer to ensure we're sending right away. Then, send the reset command and
393  // store the time that we sent it, plus the estimated time for the characters to
394  // get across the serial line to the device at the current baud rate.
395  // Set time units to milliseconds (MT) and reset the time (MZ).
396 
397  char clear_timestamp_cmd[] = "Q0\r";
398 
400 
401  if (vrpn_write_characters(serial_fd, (const unsigned char *)clear_timestamp_cmd,
402  strlen(clear_timestamp_cmd)) != (int)strlen(clear_timestamp_cmd)) {
403  VRPN_MSG_ERROR("Cannot send command to clear timestamp");
405  return;
406  }
407 
408  // Drain the output buffer again, then record the time as the base time from
409  // the tracker.
412 
413  // Done with reset.
414  vrpn_gettimeofday(&watchdog_timestamp, NULL); // Set watchdog now
415  VRPN_MSG_WARNING("Reset Completed (this is good)");
416  status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
417 }
418 
419 // This function will read characters until it has a full report, then
420 // put that report into the time, sensor, pos and quat fields so that it can
421 // be sent the next time through the loop. The time stored is that of
422 // the first character received as part of the report. Reports start with
423 // the header "0xy", where x is the station number and y is either the
424 // space character or else one of the characters "A-F". Characters "A-F"
425 // indicate weak signals and so forth, but in practice it is much harder
426 // to deal with them than to ignore them (they don't indicate hard error
427 // conditions). The report follows, 4 bytes per word in little-endian byte
428 // order; each word is an IEEE floating-point binary value. The first three
429 // are position in X,Y and Z. The next four are the unit quaternion in the
430 // order W, X,Y,Z. There are some optional fields for the Intersense 900
431 // tracker, then there is an ASCII space character at the end.
432 // If we get a report that is not valid, we assume that we have lost a
433 // character or something and re-synchronize with the Liberty by waiting
434 // until the start-of-report character ('0') comes around again.
435 // The routine that calls this one makes sure we get a full reading often
436 // enough (ie, it is responsible for doing the watchdog timing to make sure
437 // the tracker hasn't simply stopped sending characters).
438 
440 {
441  char errmsg[512]; // Error message to send to VRPN
442  int ret; // Return value from function call to be checked
443  unsigned char *bufptr; // Points into buffer at the current value to read
444 
445  //--------------------------------------------------------------------
446  // Each report starts with the ASCII 'LY' characters. If we're synching,
447  // read a byte at a time until we find a 'LY' characters.
448  //--------------------------------------------------------------------
449  // For the Patriot this is 'PA'.
450  // For the (high speed) Liberty Latus this is 'LU'.
451 
452  if (status == vrpn_TRACKER_SYNCING) {
453 
454  // Try to get the first sync character if don't already have it.
455  // If none, just return.
456  if (got_single_sync_char != 1) {
458  if (ret != 1) {
459  //if (DEBUG) fprintf(stderr,"[DEBUG]: Missed First Sync Char, ret= %i\n",ret);
460  return 0;
461  }
462  }
463 
464  // Try to get the second sync character. If none, just return
466  if (ret == 1) {
467  //Got second sync Char
469  }
470  else if (ret != -1) {
471  if (DEBUG) fprintf(stderr,"[DEBUG]: Missed Second Sync Char\n");
473  return 0;
474  }
475 
476  // If it is not 'LY' or 'PA' or 'LU' , we don't want it but we
477  // need to look at the next one, so just return and stay
478  // in Syncing mode so that we will try again next time through.
479  // Also, flush the buffer so that it won't take as long to catch up.
480  if (
481  ((( buffer[0] == 'L') && (buffer[1] == 'Y')) != 1)
482  &&
483  ((( buffer[0] == 'P') && (buffer[1] == 'A')) != 1)
484  &&
485  ((( buffer[0] == 'L') && (buffer[1] == 'U')) != 1)
486  )
487  {
488  sprintf(errmsg,"While syncing (looking for 'LY' or 'PA' or 'LU', "
489  "got '%c%c')", buffer[0], buffer[1]);
490  VRPN_MSG_INFO(errmsg);
492  if (DEBUG) fprintf(stderr,"[DEBUGA]: Getting Report - Not LY or PA or LU, Got Character %c %c \n",buffer[0],buffer[1]);
493  return 0;
494  }
495 
496  if (DEBUG) fprintf(stderr,"[DEBUG]: Getting Report - Got LY or PA or LU\n");
497 
498  // Got the first character of a report -- go into AWAITING_STATION mode
499  // and record that we got one character at this time. The next
500  // bit of code will attempt to read the station.
501  // The time stored here is as close as possible to when the
502  // report was generated. For the InterSense 900 in timestamp
503  // mode, this value will be overwritten later.
504  bufcount = 2;
505  // vrpn_gettimeofday(&timestamp, NULL);
507  }
508 
509  //--------------------------------------------------------------------
510  // The third character of each report is the station number. Once
511  // we know this, we can compute how long the report should be for the
512  // given station, based on what values are in its report.
513  // The station number is converted into a VRPN sensor number, where
514  // the first Liberty station is '1' and the first VRPN sensor is 0.
515  //--------------------------------------------------------------------
516 
518 
519  // Try to get a character. If none, just return.
521  return 0;
522  }
523  if (DEBUG) fprintf(stderr,"[DEBUG]: Awaiting Station - Got Station (%i) \n",buffer[2]);
524 
525  d_sensor = buffer[2] - 1; // Convert ASCII 1 to sensor 0 and so on.
526  if ( (d_sensor < 0) || (d_sensor >= num_stations) ) {
528  sprintf(errmsg,"Bad sensor # (%d) in record, re-syncing", d_sensor);
529  VRPN_MSG_INFO(errmsg);
531  return 0;
532  }
533 
534  // Figure out how long the current report should be based on the
535  // settings for this sensor.
537 
538  // Got the station report -- to into PARTIAL mode and record
539  // that we got one character at this time. The next bit of code
540  // will attempt to read the rest of the report.
541  bufcount++;
543  }
544 
545  //--------------------------------------------------------------------
546  // Read as many bytes of this report as we can, storing them
547  // in the buffer. We keep track of how many have been read so far
548  // and only try to read the rest. The routine that calls this one
549  // makes sure we get a full reading often enough (ie, it is responsible
550  // for doing the watchdog timing to make sure the tracker hasn't simply
551  // stopped sending characters).
552  //--------------------------------------------------------------------
553 
556  if (ret == -1) {
557  if (DEBUGA) fprintf(stderr,"[DEBUG]: Error Reading Report\n");
558  VRPN_MSG_ERROR("Error reading report");
560  return 0;
561  }
562  bufcount += ret;
563  if (bufcount < REPORT_LEN) { // Not done -- go back for more
564  if (DEBUG) fprintf(stderr,"[DEBUG]: Don't have full report (%i of %i)\n",bufcount,REPORT_LEN);
565  return 0;
566  }
567 
568  //--------------------------------------------------------------------
569  // We now have enough characters to make a full report. Check to make
570  // sure that its format matches what we expect. If it does, the next
571  // section will parse it. If it does not, we need to go back into
572  // synch mode and ignore this report. A well-formed report has the
573  // first character '0', the next character is the ASCII station
574  // number, and the third character is either a space or a letter.
575  //--------------------------------------------------------------------
576  // fprintf(stderr,"[DEBUG]: Got full report\n");
577 
578  if (
579  ((buffer[0] != 'L') || (buffer[1] != 'Y'))
580  &&
581  ((buffer[0] != 'P') || (buffer[1] != 'A'))
582  &&
583  ((buffer[0] != 'L') || (buffer[1] != 'U'))
584  ) {
585  if (DEBUGA) fprintf(stderr,"[DEBUG]: Don't have LY or PA or 'LU' at beginning");
587  VRPN_MSG_INFO("Not 'LY' or 'PA' or 'LU' in record, re-syncing");
589  return 0;
590  }
591 
592  if (buffer[bufcount-1] != ' ') {
594  VRPN_MSG_INFO("No space character at end of report, re-syncing\n");
596  if (DEBUGA) fprintf(stderr,"[DEBUG]: Don't have space at end of report, got (%c) sensor %i\n",buffer[bufcount-1], d_sensor);
597 
598  return 0;
599  }
600 
601  //Decode the error status and output a debug message
602  if (buffer[4] != ' ') {
603  // An error has been flagged
604  if (DEBUGA) fprintf(stderr,"[DEBUG]:Error Flag %i\n",buffer[4]);
605  }
606 
607  //--------------------------------------------------------------------
608  // Decode the X,Y,Z of the position and the W,X,Y,Z of the quaternion
609  // (keeping in mind that we store quaternions as X,Y,Z, W).
610  //--------------------------------------------------------------------
611  // The reports coming from the Liberty are in little-endian order,
612  // which is the opposite of the network-standard byte order that is
613  // used by VRPN. Here we swap the order to big-endian so that the
614  // routines below can pull out the values in the correct order.
615  // This is slightly inefficient on machines that have little-endian
616  // order to start with, since it means swapping the values twice, but
617  // that is more than outweighed by the cleanliness gained by keeping
618  // all architecture-dependent code in the vrpn_Shared.C file.
619  //--------------------------------------------------------------------
620 
621  // Point at the first value in the buffer (position of the X value)
622  bufptr = &buffer[8];
623 
624  // When copying the positions, convert from inches to meters, since the
625  // Liberty reports in inches and VRPN reports in meters.
626  pos[0] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr) * INCHES_TO_METERS;
627  pos[1] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr) * INCHES_TO_METERS;
628  pos[2] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr) * INCHES_TO_METERS;
629 
630  // Change the order of the quaternion fields to match quatlib order
631  d_quat[Q_W] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
632  d_quat[Q_X] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
633  d_quat[Q_Y] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
634  d_quat[Q_Z] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
635 
636  //--------------------------------------------------------------------
637  // Decode the time from the Liberty system (unsigned 32bit int), add it to the
638  // time we zeroed the tracker, and update the report time. Remember
639  // to convert the MILLIseconds from the report into MICROseconds and
640  // seconds.
641  //--------------------------------------------------------------------
642 
643  struct timeval delta_time; // Time since the clock was reset
644 
645  // Read the integer value of the time from the record.
646  vrpn_uint32 read_time = vrpn_unbuffer_from_little_endian<vrpn_uint32>(bufptr);
647 
648  // Convert from the float in MILLIseconds to the struct timeval
649  delta_time.tv_sec = (long)(read_time / 1000); // Integer trunction to seconds
650  vrpn_uint32 read_time_milliseconds = read_time_milliseconds = read_time - delta_time.tv_sec * 1000; // Subtract out what we just counted
651  delta_time.tv_usec = (long)(read_time_milliseconds * 1000); // Convert remainder to MICROseconds
652 
653  // The time that the report was generated
655  vrpn_gettimeofday(&watchdog_timestamp, NULL); // Set watchdog now
656 
657  //--------------------------------------------------------------------
658  // If this sensor has button on it, decode the button values
659  // into the button device and mainloop the button device so that
660  // it will report any changes.
661  //--------------------------------------------------------------------
662 
663  if (stylus_buttons[d_sensor]) {
664  // Read the integer value of the bytton status from the record.
665  vrpn_uint32 button_status = vrpn_unbuffer_from_little_endian<vrpn_uint32>(bufptr);
666 
667  stylus_buttons[d_sensor]->set_button(0, button_status);
669  }
670 
671  //--------------------------------------------------------------------
672  // Done with the decoding, set the report to ready
673  //--------------------------------------------------------------------
674 
676  bufcount = 0;
677 
678 #ifdef VERBOSE2
680 #endif
681 
682  return 1;
683 }
684 
685 // this routine is called when an "Stylus" button is encountered
686 // by the tracker init string parser it sets up the VRPN button
687 // device
688 int vrpn_Tracker_Liberty::add_stylus_button(const char *button_device_name, int sensor, int numbuttons)
689 {
690  // Make sure this is a valid sensor
691  if ( (sensor < 0) || (sensor >= num_stations) ) {
692  return -1;
693  }
694 
695  // Add a new button device and set the pointer to point at it.
696  stylus_buttons[sensor] = new vrpn_Button_Server(button_device_name, d_connection, numbuttons);
697  if (stylus_buttons[sensor] == NULL) {
698  VRPN_MSG_ERROR("Cannot open button device");
699  return -1;
700  }
701 
702  // Send a new station-format command to the tracker so it will report the button states.
703  return set_sensor_output_format(sensor);
704 }
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
vrpn_Button_Server * stylus_buttons[vrpn_LIBERTY_MAX_STATIONS]
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
virtual int get_report(void)
Gets a report if one is available, returns 0 if not, 1 if complete report.
vrpn_Tracker_Liberty(const char *name, vrpn_Connection *c, const char *port="/dev/ttyS0", long baud=115200, int enable_filtering=1, int numstations=vrpn_LIBERTY_MAX_STATIONS, const char *additional_reset_commands=NULL, int whoamilen=195)
The constructor is given the name of the tracker (the name of the sender it should use),...
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...
unsigned char buffer[VRPN_TRACKER_BUF_SIZE]
Definition: vrpn_Tracker.h:155
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
struct timeval watchdog_timestamp
Definition: vrpn_Tracker.h:111
Generic connection class not specific to the transport mechanism.
#define VRPN_MSG_WARNING(msg)
#define INCHES_TO_METERS
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...
struct timeval liberty_zerotime
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.
const bool DEBUG
const int vrpn_TRACKER_SYNCING
Definition: vrpn_Tracker.h:35
vrpn_uint32 bufcount
Definition: vrpn_Tracker.h:157
#define VRPN_MSG_INFO(msg)
const int vrpn_LIBERTY_MAX_STATIONS
int add_stylus_button(const char *button_device_name, int sensor, int numbuttons=1)
Add a stylus (with button) to one of the sensors.
const int vrpn_LIBERTY_MAX_WHOAMI_LEN
const bool DEBUGA
const int vrpn_TRACKER_AWAITING_STATION
Definition: vrpn_Tracker.h:36
int report_length(int sensor)
Augments the basic Liberty report length.
#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 mainloop()
Called once each time through the server program's mainloop to handle various functions (like setting...
Definition: vrpn_Button.C:470
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
timeval vrpn_TimevalSum(const timeval &tv1, const timeval &tv2)
Definition: vrpn_Shared.C:45
virtual void reset()
Reset the tracker.
#define VRPN_MSG_ERROR(msg)
int set_sensor_output_format(int sensor)
Augments the basic Liberty format.