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