vrpn  07.33
Virtual Reality Peripheral Network
vrpn_Tracker_LibertyHS.C
Go to the documentation of this file.
1 // vrpn_Tracker_LibertyHS.C
2 // This file contains the class header for a High Speed Polhemus Liberty
3 // Latus Tracker.
4 // This file is based on the vrpn_Tracker_Liberty.C file, with modifications made
5 // to allow it to operate a Liberty Latus instead. It has been tested on Linux.
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_WARNING, etc
14 #include "vrpn_Connection.h" // for vrpn_Connection
15 #include "vrpn_Shared.h" // for vrpn_SleepMsecs, timeval, etc
16 #include "vrpn_Tracker.h" // for vrpn_TRACKER_FAIL, etc
17 #include "vrpn_Tracker_LibertyHS.h"
18 #include "vrpn_MessageMacros.h" // for VRPN_MSG_INFO, VRPN_MSG_WARNING, VRPN_MSG_ERROR
19 
21 
22 #if defined(VRPN_USE_LIBUSB_1_0)
23 
24 #include <libusb.h> // for libusb_bulk_transfer, etc
25 
26 #define INCHES_TO_METERS (2.54f/100.0f)
27 const bool METRIC_UNITS = true;
28 const bool DEBUG = false; // General Debug Messages
29 const bool DEBUGA = false; // Only errors
30 
32  long baud, int enable_filtering, int numstations,
33  int receptoridx, const char *additional_reset_commands, int whoamilen) :
34  vrpn_Tracker_USB(name,c,LIBERTYHS_VENDOR_ID,LIBERTYHS_PRODUCT_ID,baud),
35  do_filter(enable_filtering),
36  num_stations(numstations>vrpn_LIBERTYHS_MAX_STATIONS ? vrpn_LIBERTYHS_MAX_STATIONS : numstations),
37  receptor_index(receptoridx),
38  num_resets(0),
39  whoami_len(whoamilen>vrpn_LIBERTYHS_MAX_WHOAMI_LEN ? vrpn_LIBERTYHS_MAX_WHOAMI_LEN : whoamilen),
40  sync_index(-1), read_len(0)
41 {
42  if (additional_reset_commands == NULL) {
43  sprintf(add_reset_cmd, "");
44  } else {
45  strncpy(add_reset_cmd, additional_reset_commands, sizeof(add_reset_cmd)-1);
46  }
47 
48  if (DEBUG) fprintf(stderr,"[DEBUG] Constructed LibertyHS Object\n");
49 }
50 
52 {
53 
54  fprintf(stderr," interrupting continuous print output mode\n");
55  char pollCommand = 'P';
56  write_usb_data(&pollCommand,1);
57  vrpn_SleepMsecs(1000.0); // Sleep for a second to let it respond
59 
60 }
61 
71 {
72  // Set output format for the station to be position and quaternion.
73  // This command is a capitol 'o' followed by '*' to format all sensors
74  // if sensor equals -1 or by the number of one specific station,
75  // then comma-separated values (2 for xyz, 7 for quat, 8 for timestamp,
76  // 0 for space) that indicate data sets, followed by character 13 (octal 15).
77  // Note that the sensor number has to be bumped to map to station number.
78 
79  char outstring[64];
80  if (sensor == -1)
81  sprintf(outstring, "O*,2,7,8,9,0\015");
82  else
83  sprintf(outstring, "O%d,2,7,8,9,0\015", sensor+1);
84  int len = strlen(outstring);
85  int ret;
86 
87  if (DEBUG) fprintf(stderr,"[DEBUG]: %s \n",outstring);
88  if ( (ret = write_usb_data(outstring,len)) != len) {
89 #ifdef libusb_strerror
90  fprintf(stderr,"vrpn_Tracker_LibertyHS::libusb_bulk_transfer(): Could not send: %s\n",
91  libusb_strerror(static_cast<libusb_error>(ret)));
92 #else
93  fprintf(stderr,"vrpn_Tracker_LibertyHS::libusb_bulk_transfer(): Could not send: code %d\n",
94  ret);
95 #endif
97  return -1;
98  }
99  vrpn_SleepMsecs(50); // Sleep for a bit to let command run
100 
101  return 0;
102  }
103 
112 {
113  int len;
114 
115  len = 9; // Basic report: Header information (8) + space at the end (1)
116  len += 3*4; // Four bytes/float, 3 floats for position
117  len += 4*4; // Four bytes/float, 4 floats for quaternion
118  len += 4; // Timestamp
119  len += 4; // Framecount
120 
121  return len;
122 }
123 
124 
126 {
127  int sent_len = 0;
128  int ret = libusb_bulk_transfer(_device_handle, LIBERTYHS_WRITE_EP | LIBUSB_ENDPOINT_OUT,
129  (vrpn_uint8*)data, len, &sent_len, 50);
130 
131  if (ret != 0)
132  fprintf(stderr,"vrpn_Tracker_LibertyHS::write_usb_data(): LIBUSB ERROR '%i'\n",ret);
133 
134  return sent_len;
135 }
136 
137 
138 int vrpn_Tracker_LibertyHS::read_usb_data(void* data, int maxlen, unsigned int timeout)
139 {
140  int read_len = 0;
141 
142  int ret = libusb_bulk_transfer(_device_handle, LIBERTYHS_READ_EP | LIBUSB_ENDPOINT_IN,
143  (vrpn_uint8*)data, maxlen, &read_len, timeout);
144 
145  /*
146  fprintf(stderr,"vrpn_Tracker_LibertyHS::read_usb_data() READ %i chars: ___",read_len);
147  for (int i = 0; i < read_len; i++) {
148  if (isprint(((unsigned char*)data)[i])) {
149  fprintf(stderr,"%c",((unsigned char*)data)[i]);
150  } else {
151  fprintf(stderr,"[0x%02X]",((unsigned char*)data)[i]);
152  }
153  }
154  fprintf(stderr,"___ ");
155  if (ret != 0) fprintf(stderr," LIBUSB ERROR: code #'%i'\n",ret); else fprintf(stderr,"\n");
156  */
157 
158  // Success: return number of read bytes
159  return read_len;
160 }
161 
163 {
164 
165  int len;
166  vrpn_uint8 buf[VRPN_TRACKER_USB_BUF_SIZE];
167 
168  // Flush usb data as long as they are available on usb port
169  do {
171  } while(len);
172 
173 }
174 
175 
176 // This routine will reset the tracker and set it to generate the types
177 // of reports we want.
178 
180 {
181  int i,resetLen,ret;
182  char reset[10];
183  char errmsg[512];
184  char outstring1[64],outstring2[64],outstring3[64],outstring4[64];
185 
186  //--------------------------------------------------------------------
187  // This section deals with resetting the tracker to its default state.
188  // Multiple attempts are made to reset, getting more aggressive each
189  // time. This section completes when the tracker reports a valid status
190  // message after the reset has completed.
191  //--------------------------------------------------------------------
192 
193  // Send the tracker a string that should reset it. The first time we
194  // try this, just do the normal 'c' command to put it into polled mode.
195  // after a few tries with this, use the '^Y' reset. Later, try to reset
196  // to the factory defaults. Then toggle the extended mode.
197  // Then put in a carriage return to try and break it out of
198  // a query mode if it is in one. These additions are cumulative: by the
199  // end, we're doing them all.
200  if (DEBUG) fprintf(stderr,"[DEBUG] Beginning Reset");
201  resetLen = 0;
202  num_resets++; // We're trying another reset
203 
204  if (num_resets > 0) { // Try to get it out of a query loop if its in one
205  reset[resetLen++] = 'F';
206  reset[resetLen++] = '0';
207  reset[resetLen++] = (char) (13); // Return key -> get ready
208  }
209 
210  if (num_resets > 2) {
211  reset[resetLen++] = (char) (25); // Ctrl + Y -> reset the tracker
212  reset[resetLen++] = (char) (13); // Return Key
213  }
214 
215  reset[resetLen++] = 'P'; // Put it into polled (not continuous) mode
216 
217  sprintf(errmsg, "Resetting the tracker (attempt %d)", num_resets);
218  VRPN_MSG_WARNING(errmsg);
219  for (i = 0; i < resetLen; i++) {
220  if (write_usb_data(&reset[i],1) == 1) {
221  fprintf(stderr,".");
222  vrpn_SleepMsecs(1000.0*2); // Wait after each character to give it time to respond
223  } else {
224  perror("Liberty: Failed writing to tracker");
226  return;
227  }
228  }
229 
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
237  flush_usb_data();
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 = read_usb_data((void*)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  flush_usb_data(); // Flush what's left
254  }
255 
256  // Asking for tracker status. ^V (WhoAmI) is used. It retruns 288 bytes
257  char statusCommand[2];
258  statusCommand[0]=(char)(22); // ^V
259  statusCommand[1]=(char)(13); // Return Key
260 
261  if (write_usb_data(&statusCommand[0],2) == 2) {
262  vrpn_SleepMsecs(1000.0); // Sleep for a second to let it respond
263  } else {
264  perror(" LibertyHS write failed (WhoAmI command)");
266  return;
267  }
268 
269  // Read Status
270  unsigned char statusmsg[vrpn_LIBERTYHS_MAX_WHOAMI_LEN+1];
271 
272  // Attempt to read whoami_len characters.
273  if ( (ret = read_usb_data((void*)statusmsg, whoami_len)) != whoami_len) {
274  fprintf(stderr," Got %d of %d characters for status\n",ret, whoami_len);
275  }
276 
277  if ( (statusmsg[0]!='0') ) {
278  int i;
279  if (ret != -1) {
280  statusmsg[ret] = '\0'; // Null-terminate the string
281  }
282  fprintf(stderr, " LibertyHS: status is (");
283  for (i = 0; i < ret; i++) {
284  if (isprint(statusmsg[i])) {
285  fprintf(stderr,"%c",statusmsg[i]);
286  } else {
287  fprintf(stderr,"[0x%02X]",statusmsg[i]);
288  }
289  }
290  fprintf(stderr,"\n)\n");
291  VRPN_MSG_ERROR("Bad status report from LibertyHS, retrying reset");
292  return;
293  } else {
294  VRPN_MSG_WARNING("LibertyHS gives status (this is good)");
295  num_resets = 0; // Success, use simple reset next time
296  }
297 
298  //--------------------------------------------------------------------
299  // Now that the tracker has given a valid status report, set all of
300  // the parameters the way we want them.
301  //--------------------------------------------------------------------
302 
303  // Set output format for each of the possible stations.
304 
305  if (set_sensor_output_format()) {
306  return;
307  }
308 
309  // Enable filtering if the constructor parameter said to.
310  // Set filtering for both position (X command) and orientation (Y command)
311  // to the values that are recommended as a "jumping off point" in the
312  // LibertyHS manual.
313 
314  if (do_filter) {
315  if (DEBUG) fprintf(stderr,"[DEBUG]: Enabling filtering\n");
316 
317  if (write_usb_data((void*)"X0.2,0.2,0.8,0.8\015", 17) == 17) {
318  vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
319  } else {
320  perror(" LibertyHS write position filter failed");
322  return;
323  }
324  if (write_usb_data((void*)"Y0.2,0.2,0.8,0.8\015", 17) == 17) {
325  vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
326  } else {
327  perror(" LibertyHS write orientation filter failed");
329  return;
330  }
331  } else {
332  if (DEBUG) fprintf(stderr,"[DEBUG]: Disabling filtering\n");
333 
334  if (write_usb_data((void*)"X0,1,0,0\015", 9) == 9) {
335  vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
336  } else {
337  perror(" LibertyHS write position filter failed");
339  return;
340  }
341  if (write_usb_data((void*)"Y0,1,0,0\015", 9) == 9) {
342  vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
343  } else {
344  perror(" LibertyHS write orientation filter failed");
346  return;
347  }
348  }
349 
350  // Send the additional reset commands, if any, to the tracker.
351  // These commands come in lines, with character \015 ending each
352  // line. If a line start with an asterisk (*), treat it as a pause
353  // command, with the number of seconds to wait coming right after
354  // the asterisk. Otherwise, the line is sent directly to the tracker.
355  // Wait a while for them to take effect, then clear the input
356  // buffer.
357  if (strlen(add_reset_cmd) > 0) {
358  char *next_line;
359  char add_cmd_copy[sizeof(add_reset_cmd)];
360  char string_to_send[sizeof(add_reset_cmd)];
361  int seconds_to_wait, count;
362 
363  printf(" LibertyHS writing extended reset commands...\n");
364 
365  // Make a copy of the additional reset string, since it is consumed
366  strncpy(add_cmd_copy, add_reset_cmd, sizeof(add_cmd_copy));
367 
368  // Pass through the string, testing each line to see if it is
369  // a sleep command or a line to send to the tracker. Continue until
370  // there are no more line delimiters ('\015'). Be sure to write the
371  // \015 to the end of the string sent to the tracker.
372  // Note that strok() puts a NULL character in place of the delimiter.
373 
374  next_line = strtok(add_cmd_copy, "\015");
375  count = 0;
376  while (next_line != NULL) {
377  if (next_line[0] == '*') { // This is a "sleep" line, see how long
378  seconds_to_wait = atoi(&next_line[1]);
379  fprintf(stderr," ...sleeping %d seconds\n",seconds_to_wait);
380  vrpn_SleepMsecs(1000.0*seconds_to_wait);
381  } else { // This is a command line, send it
382  sprintf(string_to_send, "%s\015", next_line);
383  fprintf(stderr," ...sending command: %s\n", string_to_send);
384  write_usb_data(string_to_send,strlen(string_to_send));
385  vrpn_SleepMsecs(1000.0*2);
386  }
387  next_line = strtok(next_line+strlen(next_line)+1, "\015");
388  }
389 
390  // Sleep a little while to let this finish, then clear the input buffer
391  vrpn_SleepMsecs(1000.0);
392  flush_usb_data();
393  }
394 
395  // Disable USB BUFFERING mode
396  // (disbale output buffering before USB transmission to the host)
397  sprintf(outstring1, "@B0\r");
398  if (write_usb_data(outstring1, strlen(outstring1)) == (int)strlen(outstring1)) {
399  fprintf(stderr, "\n Tracker USB buffering mode disabled\n");
400  }
401  vrpn_SleepMsecs(1000.0);
402 
403  // Set METRIC units
404  if (METRIC_UNITS) {
405  sprintf(outstring2, "U1\r");
406  if (write_usb_data(outstring2, strlen(outstring2)) == (int)strlen(outstring2)) {
407  fprintf(stderr, " LibertyHS set to metric units\n");
408  }
409  }
410  else
411  fprintf(stderr, " LibertyHS set to English units\n");
412  vrpn_SleepMsecs(1000.0);
413 
414  // Set data format to BINARY mode
415  sprintf(outstring3, "F1\r");
416  if (write_usb_data(outstring3, strlen(outstring3)) == (int)strlen(outstring3)) {
417  fprintf(stderr, " LibertyHS set to binary mode\n\n");
418  }
419  vrpn_SleepMsecs(1000.0);
420 
421  // Launch detected markers
422  if (launch_markers() != num_stations) {
423  fprintf(stderr, "\nCould not launch the %i requested markers\n", num_stations);
425  return;
426  } else {
427  fprintf(stderr, "\nAll %i markers are ready!\n", num_stations);
428  }
429 
430  // Set tracker to CONTINUOUS mode
431  sprintf(outstring4, "C\r");
432  if (write_usb_data(outstring4, strlen(outstring4)) != (int)strlen(outstring4)) {
433  perror(" LibertyHS write failed");
435  return;
436  } else {
437  fprintf(stderr, "\n LibertyHS set to continuous mode\n\n");
438  }
439 
440  // If we are using the LibertyHS timestamps, clear the timer on the device and
441  // store the time when we cleared it. Send the reset command and
442  // store the time that we sent it.
443 
444  char clear_timestamp_cmd[] = "Q0\r";
445 
446  if (write_usb_data(clear_timestamp_cmd,
447  strlen(clear_timestamp_cmd)) != (int)strlen(clear_timestamp_cmd)) {
448  VRPN_MSG_ERROR("Cannot send command to clear timestamp");
450  return;
451  }
452 
453  // Record the time as the base time from the tracker.
455 
456  // Done with reset.
457  vrpn_gettimeofday(&watchdog_timestamp, NULL); // Set watchdog now
458 
459  VRPN_MSG_WARNING("Reset Completed (this is good)");
460  status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
461 }
462 
463 
464 // This function will try to read and send as many reports as there are
465 // sensors (i.e. num_stations).
466 // It reads characters until it has a full report, then put that report
467 // into the time, sensor, pos and quat fields so that it can
468 // be sent the next time through the loop. The time stored is that of
469 // the first character received as part of the report. Reports start with
470 // the header "0xy", where x is the station number and y is either the
471 // space character or else one of the characters "A-F". Characters "A-F"
472 // indicate weak signals and so forth, but in practice it is much harder
473 // to deal with them than to ignore them (they don't indicate hard error
474 // conditions). The report follows, 4 bytes per word in little-endian byte
475 // order; each word is an IEEE floating-point binary value. The first three
476 // are position in X,Y and Z. The next four are the unit quaternion in the
477 // order W, X,Y,Z. Then there is an ASCII space character at the end.
478 // If we get a report that is not valid, we assume that we have lost a
479 // character or something and re-synchronize with the LibertyHS by waiting
480 // until the start-of-report character ('0') comes around again.
481 // The routine that calls this one makes sure we get a full reading often
482 // enough (ie, it is responsible for doing the watchdog timing to make sure
483 // the tracker hasn't simply stopped sending characters).
484 
486 {
487  char errmsg[512]; // Error message to send to VRPN
488  unsigned char *bufptr; // Points into buffer at the current value to read
489 
490  //--------------------------------------------------------------------
491  // Each report starts with the ASCII 'LU' characters. If we're synching,
492  // read available bytes and check if we find the 'LU' characters.
493  //--------------------------------------------------------------------
494 
495  for (int i = 0; i < num_stations; i++)
496  {
497  // Read new data from USB buffer if we are in Syncing mode
498  if (status == vrpn_TRACKER_SYNCING) {
500 
501  if (read_len < 1) {
502  if (DEBUG) fprintf(stderr,"[DEBUG]: Missed First Sync Char, read_len = %i\n",read_len);
503  return 0;
504  }
505 
507  sync_index = 0;
508  }
509  else { // Partial mode (status = vrpn_TRACKER_PARTIAL)
510  bufcount = 0;
511  }
512 
513  // Try to get the two first sync characters. If none, just return.
514  while (sync_index < read_len - 1) {
515 
516  // If it is not 'LU', we don't want it but we
517  // need to look at the next one, so just keep reading the buffer
518  // and stay in Syncing mode until we find them.
519  if ((buffer[sync_index] == 'L') && (buffer[sync_index + 1] == 'U')) {
520  if (DEBUG) fprintf(stderr,"[DEBUG]: Getting Report - Got LU\n");
521  bufcount = 2;
522  break;
523  }
524 
525  if (DEBUG) fprintf(stderr,"[DEBUGA] While syncing: Getting Report - Not LU, Got Character %c \n",buffer[sync_index]);
526  sync_index++;
527 
528  }
529 
530  // Try to get next character. If none, just return and go back to Syncing mode.
531  if ( static_cast<vrpn_int32>(sync_index + bufcount) >= (read_len - 1) ) {
532  return 0;
533  }
534 
535  // Get sensor number
536  if (DEBUG) fprintf(stderr,"[DEBUG]: Awaiting Station - Got Station (%i) \n",buffer[sync_index + bufcount]);
537  d_sensor = buffer[sync_index + bufcount] - 1; // Convert ASCII 1 to sensor 0 and so on.
538  if ( (d_sensor < 0) || (d_sensor >= vrpn_LIBERTYHS_MAX_STATIONS) ) {
539  sprintf(errmsg,"Bad sensor # (%d) in record, re-syncing", d_sensor + 1);
540  VRPN_MSG_INFO(errmsg);
542  sync_index += bufcount;
543  continue;
544  }
545 
546  bufcount++;
547 
548  // Figure out how long the current report should be based on the
549  // settings for this sensor.
551 
552  //--------------------------------------------------------------------
553  // Read as many bytes of this report as we can, storing them
554  // in the buffer. We keep track of how many have been read so far
555  // and only try to read the rest. The routine that calls this one
556  // makes sure we get a full reading often enough (ie, it is responsible
557  // for doing the watchdog timing to make sure the tracker hasn't simply
558  // stopped sending characters).
559  //--------------------------------------------------------------------
560 
561  if (read_len - sync_index < static_cast<vrpn_int32>(REPORT_LEN)) {
562  if (DEBUG) fprintf(stderr,"[DEBUG]: Don't have full report (%i of %i)\n",
564  return 0;
565  }
566 
567  // Full report available
569 
570  //--------------------------------------------------------------------
571  // We now have enough characters to make a full report. Check to make
572  // sure that its format matches what we expect. If it does, the next
573  // section will parse it. If it does not, we need to go to Partial mode
574  // and ignore this report. A well-formed report has the
575  // first characters 'LU', the next character is the ASCII station
576  // number, and the third character is either a space or a letter.
577  //--------------------------------------------------------------------
578  if (DEBUG) fprintf(stderr,"[DEBUG]: Got full report\n");
579 
580  if ((buffer[sync_index] != 'L') || (buffer[sync_index + 1] != 'U')) {
581  if (DEBUGA) fprintf(stderr,"[DEBUG]: Don't have 'LU' at beginning");
582  VRPN_MSG_INFO("Not 'LU' in record, re-syncing");
584  sync_index++;
585  continue;
586  }
587 
588  if (buffer[sync_index + bufcount - 1] != ' ') {
589  VRPN_MSG_INFO("No space character at end of report, re-syncing\n");
590  if (DEBUGA) fprintf(stderr,"[DEBUG]: Don't have space at end of report, got (%c) sensor %i\n",
593  sync_index++;
594  continue;
595  }
596 
597  // Decode the error status and output a debug message
598 
599  if (buffer[sync_index + 4] != ' ') {
600  // An error has been flagged
601  if (DEBUGA) fprintf(stderr,"[DEBUG]:Error Flag %i\n",buffer[sync_index + 4]);
602  }
603 
604  //--------------------------------------------------------------------
605  // Decode the X,Y,Z of the position and the W,X,Y,Z of the quaternion
606  // (keeping in mind that we store quaternions as X,Y,Z, W).
607  //--------------------------------------------------------------------
608  // The reports coming from the LibertyHS are in little-endian order,
609  // which is the opposite of the network-standard byte order that is
610  // used by VRPN. Here we swap the order to big-endian so that the
611  // routines below can pull out the values in the correct order.
612  // This is slightly inefficient on machines that have little-endian
613  // order to start with, since it means swapping the values twice, but
614  // that is more than outweighed by the cleanliness gained by keeping
615  // all architecture-dependent code in the vrpn_Shared.C file.
616  //--------------------------------------------------------------------
617 
618  // Point at the first value in the report (position of the X value)
619  bufptr = &buffer[sync_index + 8];
620 
621  // When copying the positions, convert from inches to meters, since the
622  // LibertyHS reports in inches and VRPN reports in meters.
623  float convFactor = METRIC_UNITS ? 1.0f : INCHES_TO_METERS;
624  pos[0] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr) * convFactor;
625  pos[1] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr) * convFactor;
626  pos[2] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr) * convFactor;
627 
628 
629  // Change the order of the quaternion fields to match quatlib order
630  d_quat[Q_W] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
631  d_quat[Q_X] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
632  d_quat[Q_Y] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
633  d_quat[Q_Z] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
634 
635  //--------------------------------------------------------------------
636  // Decode the time from the LibertyHS system (unsigned 32bit int), add it to the
637  // time we zeroed the tracker, and update the report time. Remember
638  // to convert the MILLIseconds from the report into MICROseconds and
639  // seconds.
640  //--------------------------------------------------------------------
641 
642  struct timeval delta_time; // Time since the clock was reset
643 
644  // Read the integer value of the time from the record.
645  vrpn_uint32 read_time = vrpn_unbuffer_from_little_endian<vrpn_uint32>(bufptr);
646 
647  // Convert from the float in MILLIseconds to the struct timeval
648  delta_time.tv_sec = (long)(read_time / 1000); // Integer trunction to seconds
649  vrpn_uint32 read_time_milliseconds = read_time - delta_time.tv_sec * 1000; // Subtract out what we just counted
650  delta_time.tv_usec = (long)(read_time_milliseconds * 1000); // Convert remainder to MICROseconds
651 
652  // The time that the report was generated
654  vrpn_gettimeofday(&watchdog_timestamp, NULL); // Set watchdog now
655 
656  // The frame number corresponding to the report
657  frame_count = vrpn_unbuffer_from_little_endian<vrpn_uint32>(bufptr);
658 
659  /*
660  fprintf(stderr,"READ_TIME = %d,%d + liberty_zerotime %d,%d = TIMESTAMP %d,%d ; WATCHDOG = %d,%d\n",
661  (int)delta_time.tv_sec,(int)delta_time.tv_usec,
662  (int)liberty_zerotime.tv_sec, (int)liberty_zerotime.tv_usec,
663  (int)timestamp.tv_sec,(int)timestamp.tv_usec,
664  (int)watchdog_timestamp.tv_sec,(int)watchdog_timestamp.tv_usec);
665  */
666  /*
667  print_latest_report();
668  double delta = vrpn_TimevalMsecs(vrpn_TimevalDiff(watchdog_timestamp,liberty_zerotime)) - (double) read_time;
669  fprintf(stderr,"--> LATENCY = %6.3f msec\n",delta);
670  */
671 
672 
673  //--------------------------------------------------------------------
674  // Done with the decoding, send the report
675  //--------------------------------------------------------------------
676 
677  send_report();
678 
679  // Check if there's still data to read in the buffer
680  sync_index += bufcount;
681  if (sync_index < read_len) {
682  // There might be remaining reports from other trackers in buffer.
683  // Go to Partial mode.
685  }
686  else {
687  // There is no more report in buffer
688  break;
689  }
690 
691 #ifdef VERBOSE2
693 #endif
694  }
695 
696  return 1;
697 }
698 
699 
701 {
702  // Define a marker map request
703  char mapmsg[4];
704  mapmsg[0] = (char) (21); // Ctrl + U -> active marker map
705  mapmsg[1] = '0';
706  mapmsg[2] = (char) (13); // Return Key
707 
708  // Send a marker map command to the tracker
709  if (write_usb_data(&mapmsg[0], 3) == 3) {
710  vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
711  } else {
712  perror(" LibertyHS write failed");
713  return 0;
714  }
715 
716  // Read binary output record
717  unsigned char markermap[vrpn_LIBERTYHS_MAX_MARKERMAP_LEN+1];
718  int ret = read_usb_data((void*)markermap, vrpn_LIBERTYHS_MAX_MARKERMAP_LEN);
720  fprintf(stderr," Got %d of %d bytes for marker map\n",ret,vrpn_LIBERTYHS_MAX_MARKERMAP_LEN);
721  return 0;
722  }
723 
724  // Flush what's left
725  flush_usb_data();
726 
727  // Check marker map to identify launched markers
728  int marker_found = 0;
729  unsigned char mask = (char) (1);
730  for (int bit = 1; bit <= vrpn_LIBERTYHS_MAX_STATIONS; bit++) {
731  if (mask & markermap[10]) {
732  marker_found++;
733  fprintf(stderr,"Tracker #%i launched\n",bit);
734  }
735  mask *= 2;
736  }
737 
738  return marker_found;
739 }
740 
741 
743 {
744  fprintf(stderr,"\nDetect and launch %i markers:\n\n",num_stations);
745 
746  // Clear the input buffer
747  flush_usb_data();
748 
749  // Check if the markers are already launched
750  int marker_found = test_markers ();
751  if (marker_found > 0) {
752  fprintf(stderr,"\nWARNING: %i markers are already launched! If you want to change markers,\n",marker_found);
753  fprintf(stderr, " turn off the SEU (System Electronics Unit) and run this application again.\n");
754  return marker_found;
755  }
756 
757  // Define a launch marker request
758  char launchmsg[4];
759  launchmsg[0] = 'L'; // 'Li' command line (i.e. launch new tracker near receptor i)
760  launchmsg[1] = (char) ((int) '0' + receptor_index);
761  launchmsg[2] = (char) (13); // Return Key
762 
763  // Detect and launch num_stations markers
764  while (marker_found < num_stations) {
765 
766  // Wait for the user to place a tracker for activation
767  fprintf(stderr,"\n--> PLACE A NEW POWERED UP MARKER UNDER RECEPTOR #%i\n ",receptor_index);
768  for (int i = 10; i > 0 ; i--) {
769  fprintf(stderr,"%i...",i);
770  vrpn_SleepMsecs(1000.0);
771  }
772 
773  // Send a launch marker command to receptor 'receptor_index'
774  fprintf(stderr," sending LAUNCH MARKER command\n DON'T MOVE THE TRACKER!\n\n");
775  if (write_usb_data(&launchmsg[0], 3) == 3) {
776  vrpn_SleepMsecs(1000.0*2); // Wait after each character to give it time to respond
777  } else {
778  perror("LibertyHS: Failed writing launch marker command to tracker");
779  break;
780  }
781 
782  // Check if the markers are already launched
783  if (test_markers () == num_stations)
784  marker_found = num_stations;
785  }
786 
787  return marker_found;
788 }
789 
790 // End of LIBUSB
791 #endif
vrpn_Tracker.h
vrpn_Tracker_USB::send_report
virtual void send_report(void)
Definition: vrpn_Tracker.C:1042
vrpn_Tracker_LibertyHS::receptor_index
int receptor_index
Definition: vrpn_Tracker_LibertyHS.h:66
vrpn_BaseClass.h
vrpn_Tracker_LibertyHS::liberty_zerotime
struct timeval liberty_zerotime
Definition: vrpn_Tracker_LibertyHS.h:73
vrpn_Tracker_LibertyHS::read_len
int read_len
Definition: vrpn_Tracker_LibertyHS.h:70
vrpn_Tracker::d_sensor
vrpn_int32 d_sensor
Definition: vrpn_Tracker.h:94
vrpn_Tracker_LibertyHS::flush_usb_data
void flush_usb_data()
Empties the USB read buffer.
Definition: vrpn_Tracker_LibertyHS.C:162
vrpn_LIBERTYHS_MAX_MARKERMAP_LEN
const int vrpn_LIBERTYHS_MAX_MARKERMAP_LEN
Definition: vrpn_Tracker_LibertyHS.h:31
VRPN_TRACKER_USB_BUF_SIZE
#define VRPN_TRACKER_USB_BUF_SIZE
Definition: vrpn_Tracker.h:179
vrpn_LIBERTYHS_MAX_STATIONS
const int vrpn_LIBERTYHS_MAX_STATIONS
Definition: vrpn_Tracker_LibertyHS.h:29
DEBUGA
const bool DEBUGA
Definition: vrpn_Tracker_LibertyHS.C:29
vrpn_Tracker_USB::bufcount
vrpn_uint32 bufcount
Definition: vrpn_Tracker.h:196
vrpn_Tracker_LibertyHS::num_resets
int num_resets
Definition: vrpn_Tracker_LibertyHS.h:67
INCHES_TO_METERS
#define INCHES_TO_METERS
Definition: vrpn_Tracker_LibertyHS.C:26
vrpn_Tracker::d_quat
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
vrpn_Tracker_LibertyHS::REPORT_LEN
vrpn_uint32 REPORT_LEN
Definition: vrpn_Tracker_LibertyHS.h:75
vrpn_Tracker::timestamp
struct timeval timestamp
Definition: vrpn_Tracker.h:100
vrpn_Tracker_LibertyHS::sync_index
int sync_index
Definition: vrpn_Tracker_LibertyHS.h:71
vrpn_Tracker_LibertyHS::vrpn_Tracker_LibertyHS
vrpn_Tracker_LibertyHS(const char *name, vrpn_Connection *c, long baud=115200, int enable_filtering=1, int numstations=vrpn_LIBERTYHS_MAX_STATIONS, int receptoridx=1, const char *additional_reset_commands=NULL, int whoamilen=288)
The constructor is given the name of the tracker (the name of the sender it should use),...
Definition: vrpn_Tracker_LibertyHS.C:31
vrpn_Tracker_LibertyHS::whoami_len
int whoami_len
Definition: vrpn_Tracker_LibertyHS.h:69
vrpn_Tracker_LibertyHS::read_usb_data
int read_usb_data(void *data, int maxlen, unsigned int timeout=50)
Reads at most maxlen bytes from USB device and copy them into data buffer. Returns the number of read...
Definition: vrpn_Tracker_LibertyHS.C:138
vrpn_Tracker_LibertyHS::~vrpn_Tracker_LibertyHS
~vrpn_Tracker_LibertyHS()
Definition: vrpn_Tracker_LibertyHS.C:51
vrpn_SleepMsecs
void vrpn_SleepMsecs(double dMsecs)
Definition: vrpn_Shared.C:157
VRPN_MSG_WARNING
#define VRPN_MSG_WARNING(msg)
Definition: vrpn_MessageMacros.h:39
vrpn_Shared.h
vrpn_TRACKER_PARTIAL
const int vrpn_TRACKER_PARTIAL
Definition: vrpn_Tracker.h:38
vrpn_Tracker_LibertyHS::set_sensor_output_format
int set_sensor_output_format(int sensor=-1)
Augments the basic LibertyHS format.
Definition: vrpn_Tracker_LibertyHS.C:70
vrpn_Tracker_LibertyHS::add_reset_cmd
char add_reset_cmd[2048]
Definition: vrpn_Tracker_LibertyHS.h:68
vrpn_Tracker_LibertyHS::reset
virtual void reset()
Reset the tracker.
Definition: vrpn_Tracker_LibertyHS.C:179
vrpn_Tracker_LibertyHS::launch_markers
int launch_markers()
Returns the number of detected and lauched trackers.
Definition: vrpn_Tracker_LibertyHS.C:742
vrpn_Tracker_LibertyHS.h
vrpn_Connection
Generic connection class not specific to the transport mechanism.
Definition: vrpn_Connection.h:510
vrpn_Connection.h
vrpn_TRACKER_FAIL
const int vrpn_TRACKER_FAIL
Definition: vrpn_Tracker.h:40
vrpn_Tracker_LibertyHS::report_length
int report_length(int sensor)
Augments the basic LibertyHS report length.
Definition: vrpn_Tracker_LibertyHS.C:111
vrpn_gettimeofday
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
vrpn_Tracker::watchdog_timestamp
struct timeval watchdog_timestamp
Definition: vrpn_Tracker.h:111
vrpn_Tracker::status
int status
Definition: vrpn_Tracker.h:129
METRIC_UNITS
const bool METRIC_UNITS
Definition: vrpn_Tracker_LibertyHS.C:27
vrpn_Tracker_USB::_device_handle
struct libusb_device_handle * _device_handle
Definition: vrpn_Tracker.h:188
vrpn_TRACKER_SYNCING
const int vrpn_TRACKER_SYNCING
Definition: vrpn_Tracker.h:35
vrpn_Tracker_USB
Definition: vrpn_Tracker.h:181
vrpn_Tracker::frame_count
vrpn_int32 frame_count
Definition: vrpn_Tracker.h:101
vrpn_Tracker_LibertyHS::test_markers
int test_markers()
Launches num_stations markers using receptor receptor_index to detect them.
Definition: vrpn_Tracker_LibertyHS.C:700
vrpn_Tracker_LibertyHS::get_report
virtual int get_report(void)
Gets reports if some are available, returns 0 if not, 1 if complete report(s).
Definition: vrpn_Tracker_LibertyHS.C:485
VRPN_MSG_INFO
#define VRPN_MSG_INFO(msg)
Definition: vrpn_MessageMacros.h:32
vrpn_MessageMacros.h
Header containing macros formerly duplicated in a lot of implementation files.
vrpn_Tracker_LibertyHS::write_usb_data
int write_usb_data(void *data, int len)
Writes len bytes from data buffer to USB device.
Definition: vrpn_Tracker_LibertyHS.C:125
vrpn_TimevalSum
timeval vrpn_TimevalSum(const timeval &tv1, const timeval &tv2)
Definition: vrpn_Shared.C:45
vrpn_Tracker::pos
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
vrpn_Tracker_USB::buffer
vrpn_uint8 buffer[VRPN_TRACKER_USB_BUF_SIZE]
Definition: vrpn_Tracker.h:194
VRPN_SUPPRESS_EMPTY_OBJECT_WARNING
#define VRPN_SUPPRESS_EMPTY_OBJECT_WARNING()
Definition: vrpn_Configure.h:495
vrpn_Tracker::print_latest_report
void print_latest_report(void)
Definition: vrpn_Tracker.C:306
VRPN_MSG_ERROR
#define VRPN_MSG_ERROR(msg)
Definition: vrpn_MessageMacros.h:46
vrpn_LIBERTYHS_MAX_WHOAMI_LEN
const int vrpn_LIBERTYHS_MAX_WHOAMI_LEN
Definition: vrpn_Tracker_LibertyHS.h:30
vrpn_Tracker_LibertyHS::do_filter
int do_filter
Definition: vrpn_Tracker_LibertyHS.h:64
vrpn_Tracker_LibertyHS::num_stations
int num_stations
Definition: vrpn_Tracker_LibertyHS.h:65
DEBUG
const bool DEBUG
Definition: vrpn_Tracker_LibertyHS.C:28