vrpn 07.35
Virtual Reality Peripheral Network
Loading...
Searching...
No Matches
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, 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
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)
23static const bool VRPN_LIBERTY_DEBUG = false; // General Debug Messages
24static const bool VRPN_LIBERTY_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 add_reset_cmd[0] = '\0';
41 } else {
42 vrpn_strcpy(add_reset_cmd, additional_reset_commands);
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 (VRPN_LIBERTY_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 (VRPN_LIBERTY_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 if (ret != -1) {
239 statusmsg[ret] = '\0'; // Null-terminate the string
240 }
241 // It seems like some versions of the tracker report longer
242 // messages; so we reduced this check so that it does not check for the
243 // appropriate length of message or for the last character being a 10,
244 // so that it works more generally. The removed tests are:
245 // || (ret!=whoami_len) || (statusmsg[ret-1]!=(char)(10))
246 if ( (statusmsg[0]!='0') ) {
247 int i;
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)");
261printf("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++) {
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 (VRPN_LIBERTY_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 (VRPN_LIBERTY_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 vrpn_strcpy(add_cmd_copy, add_reset_cmd);
343
344 // Pass through the string, testing each line to see if it is
345 // a sleep command or a line to send to the tracker. Continue until
346 // there are no more line delimiters ('\015'). Be sure to write the
347 // \015 to the end of the string sent to the tracker.
348 // Note that strok() puts a NULL character in place of the delimiter.
349
350 next_line = strtok(add_cmd_copy, "\015");
351 while (next_line != NULL) {
352 if (next_line[0] == '*') { // This is a "sleep" line, see how long
353 seconds_to_wait = atoi(&next_line[1]);
354 fprintf(stderr," ...sleeping %d seconds\n",seconds_to_wait);
355 vrpn_SleepMsecs(1000.0*seconds_to_wait);
356 } else { // This is a command line, send it
357 sprintf(string_to_send, "%s\015", next_line);
358 fprintf(stderr, " ...sending command: %s\n", string_to_send);
360 (const unsigned char *)string_to_send,strlen(string_to_send));
361 }
362 next_line = strtok(next_line+strlen(next_line)+1, "\015");
363 }
364
365 // Sleep a little while to let this finish, then clear the input buffer
366 vrpn_SleepMsecs(1000.0*2);
368 }
369
370 // Set data format to BINARY mode
371 sprintf(outstring1, "F1\r");
372 if (vrpn_write_characters(serial_fd, (const unsigned char *)outstring1,
373 strlen(outstring1)) == (int)strlen(outstring1)) {
374 fprintf(stderr, " Liberty set to binary mode\n");
375
376 }
377
378 // Set tracker to continuous mode
379 sprintf(outstring3, "C\r");
380 if (vrpn_write_characters(serial_fd, (const unsigned char *)outstring3,
381 strlen(outstring3)) != (int)strlen(outstring3)) {
382 perror(" Liberty write failed");
384 return;
385 } else {
386 fprintf(stderr, " Liberty set to continuous mode\n");
387 }
388
389 // If we are using the Liberty timestamps, clear the timer on the device and
390 // store the time when we cleared it. First, drain any characters in the output
391 // buffer to ensure we're sending right away. Then, send the reset command and
392 // store the time that we sent it, plus the estimated time for the characters to
393 // get across the serial line to the device at the current baud rate.
394 // Set time units to milliseconds (MT) and reset the time (MZ).
395
396 char clear_timestamp_cmd[] = "Q0\r";
397
399
400 if (vrpn_write_characters(serial_fd, (const unsigned char *)clear_timestamp_cmd,
401 strlen(clear_timestamp_cmd)) != (int)strlen(clear_timestamp_cmd)) {
402 VRPN_MSG_ERROR("Cannot send command to clear timestamp");
404 return;
405 }
406
407 // Drain the output buffer again, then record the time as the base time from
408 // the tracker.
411
412 // Done with reset.
413 vrpn_gettimeofday(&watchdog_timestamp, NULL); // Set watchdog now
414 VRPN_MSG_WARNING("Reset Completed (this is good)");
415 status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
416}
417
418// This function will read characters until it has a full report, then
419// put that report into the time, sensor, pos and quat fields so that it can
420// be sent the next time through the loop. The time stored is that of
421// the first character received as part of the report. Reports start with
422// the header "0xy", where x is the station number and y is either the
423// space character or else one of the characters "A-F". Characters "A-F"
424// indicate weak signals and so forth, but in practice it is much harder
425// to deal with them than to ignore them (they don't indicate hard error
426// conditions). The report follows, 4 bytes per word in little-endian byte
427// order; each word is an IEEE floating-point binary value. The first three
428// are position in X,Y and Z. The next four are the unit quaternion in the
429// order W, X,Y,Z. There are some optional fields for the Intersense 900
430// tracker, then there is an ASCII space character at the end.
431// If we get a report that is not valid, we assume that we have lost a
432// character or something and re-synchronize with the Liberty by waiting
433// until the start-of-report character ('0') comes around again.
434// The routine that calls this one makes sure we get a full reading often
435// enough (ie, it is responsible for doing the watchdog timing to make sure
436// the tracker hasn't simply stopped sending characters).
437
439{
440 char errmsg[512]; // Error message to send to VRPN
441 int ret; // Return value from function call to be checked
442 unsigned char *bufptr; // Points into buffer at the current value to read
443
444 //--------------------------------------------------------------------
445 // Each report starts with the ASCII 'LY' characters. If we're synching,
446 // read a byte at a time until we find a 'LY' characters.
447 //--------------------------------------------------------------------
448 // For the Patriot this is 'PA'.
449 // For the (high speed) Liberty Latus this is 'LU'.
450
452
453 // Try to get the first sync character if don't already have it.
454 // If none, just return.
455 if (got_single_sync_char != 1) {
457 if (ret != 1) {
458 //if (VRPN_LIBERTY_DEBUG) fprintf(stderr,"[DEBUG]: Missed First Sync Char, ret= %i\n",ret);
459 return 0;
460 }
461 }
462
463 // Try to get the second sync character. If none, just return
465 if (ret == 1) {
466 //Got second sync Char
468 }
469 else if (ret != -1) {
470 if (VRPN_LIBERTY_DEBUG) fprintf(stderr,"[DEBUG]: Missed Second Sync Char\n");
472 return 0;
473 }
474
475 // If it is not 'LY' or 'PA' or 'LU' , we don't want it but we
476 // need to look at the next one, so just return and stay
477 // in Syncing mode so that we will try again next time through.
478 // Also, flush the buffer so that it won't take as long to catch up.
479 if (
480 ((( buffer[0] == 'L') && (buffer[1] == 'Y')) != 1)
481 &&
482 ((( buffer[0] == 'P') && (buffer[1] == 'A')) != 1)
483 &&
484 ((( buffer[0] == 'L') && (buffer[1] == 'U')) != 1)
485 )
486 {
487 sprintf(errmsg,"While syncing (looking for 'LY' or 'PA' or 'LU', "
488 "got '%c%c')", buffer[0], buffer[1]);
489 VRPN_MSG_INFO(errmsg);
491 if (VRPN_LIBERTY_DEBUG) fprintf(stderr,"[DEBUGA]: Getting Report - Not LY or PA or LU, Got Character %c %c \n",buffer[0],buffer[1]);
492 return 0;
493 }
494
495 if (VRPN_LIBERTY_DEBUG) fprintf(stderr,"[DEBUG]: Getting Report - Got LY or PA or LU\n");
496
497 // Got the first character of a report -- go into AWAITING_STATION mode
498 // and record that we got one character at this time. The next
499 // bit of code will attempt to read the station.
500 // The time stored here is as close as possible to when the
501 // report was generated. For the InterSense 900 in timestamp
502 // mode, this value will be overwritten later.
503 bufcount = 2;
504 // vrpn_gettimeofday(&timestamp, NULL);
506 }
507
508 //--------------------------------------------------------------------
509 // The third character of each report is the station number. Once
510 // we know this, we can compute how long the report should be for the
511 // given station, based on what values are in its report.
512 // The station number is converted into a VRPN sensor number, where
513 // the first Liberty station is '1' and the first VRPN sensor is 0.
514 //--------------------------------------------------------------------
515
517
518 // Try to get a character. If none, just return.
520 return 0;
521 }
522 if (VRPN_LIBERTY_DEBUG) fprintf(stderr,"[DEBUG]: Awaiting Station - Got Station (%i) \n",buffer[2]);
523
524 d_sensor = buffer[2] - 1; // Convert ASCII 1 to sensor 0 and so on.
525 if ( (d_sensor < 0) || (d_sensor >= num_stations) ) {
527 sprintf(errmsg,"Bad sensor # (%d) in record, re-syncing", d_sensor);
528 VRPN_MSG_INFO(errmsg);
530 return 0;
531 }
532
533 // Figure out how long the current report should be based on the
534 // settings for this sensor.
536
537 // Got the station report -- to into PARTIAL mode and record
538 // that we got one character at this time. The next bit of code
539 // will attempt to read the rest of the report.
540 bufcount++;
542 }
543
544 //--------------------------------------------------------------------
545 // Read as many bytes of this report as we can, storing them
546 // in the buffer. We keep track of how many have been read so far
547 // and only try to read the rest. The routine that calls this one
548 // makes sure we get a full reading often enough (ie, it is responsible
549 // for doing the watchdog timing to make sure the tracker hasn't simply
550 // stopped sending characters).
551 //--------------------------------------------------------------------
552
555 if (ret == -1) {
556 if (VRPN_LIBERTY_DEBUGA) fprintf(stderr,"[DEBUG]: Error Reading Report\n");
557 VRPN_MSG_ERROR("Error reading report");
559 return 0;
560 }
561 bufcount += ret;
562 if (bufcount < REPORT_LEN) { // Not done -- go back for more
563 if (VRPN_LIBERTY_DEBUG) fprintf(stderr,"[DEBUG]: Don't have full report (%i of %i)\n",bufcount,REPORT_LEN);
564 return 0;
565 }
566
567 //--------------------------------------------------------------------
568 // We now have enough characters to make a full report. Check to make
569 // sure that its format matches what we expect. If it does, the next
570 // section will parse it. If it does not, we need to go back into
571 // synch mode and ignore this report. A well-formed report has the
572 // first character '0', the next character is the ASCII station
573 // number, and the third character is either a space or a letter.
574 //--------------------------------------------------------------------
575 // fprintf(stderr,"[DEBUG]: Got full report\n");
576
577 if (
578 ((buffer[0] != 'L') || (buffer[1] != 'Y'))
579 &&
580 ((buffer[0] != 'P') || (buffer[1] != 'A'))
581 &&
582 ((buffer[0] != 'L') || (buffer[1] != 'U'))
583 ) {
584 if (VRPN_LIBERTY_DEBUGA) fprintf(stderr,"[DEBUG]: Don't have LY or PA or 'LU' at beginning");
586 VRPN_MSG_INFO("Not 'LY' or 'PA' or 'LU' in record, re-syncing");
588 return 0;
589 }
590
591 if (buffer[bufcount-1] != ' ') {
593 VRPN_MSG_INFO("No space character at end of report, re-syncing\n");
595 if (VRPN_LIBERTY_DEBUGA) fprintf(stderr,"[DEBUG]: Don't have space at end of report, got (%c) sensor %i\n",buffer[bufcount-1], d_sensor);
596
597 return 0;
598 }
599
600 //Decode the error status and output a debug message
601 if (buffer[4] != ' ') {
602 // An error has been flagged
603 if (VRPN_LIBERTY_DEBUGA) fprintf(stderr,"[DEBUG]:Error Flag %i\n",buffer[4]);
604 }
605
606 //--------------------------------------------------------------------
607 // Decode the X,Y,Z of the position and the W,X,Y,Z of the quaternion
608 // (keeping in mind that we store quaternions as X,Y,Z, W).
609 //--------------------------------------------------------------------
610 // The reports coming from the Liberty are in little-endian order,
611 // which is the opposite of the network-standard byte order that is
612 // used by VRPN. Here we swap the order to big-endian so that the
613 // routines below can pull out the values in the correct order.
614 // This is slightly inefficient on machines that have little-endian
615 // order to start with, since it means swapping the values twice, but
616 // that is more than outweighed by the cleanliness gained by keeping
617 // all architecture-dependent code in the vrpn_Shared.C file.
618 //--------------------------------------------------------------------
619
620 // Point at the first value in the buffer (position of the X value)
621 bufptr = &buffer[8];
622
623 // When copying the positions, convert from inches to meters, since the
624 // Liberty reports in inches and VRPN reports in meters.
625 pos[0] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr) * INCHES_TO_METERS;
626 pos[1] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr) * INCHES_TO_METERS;
627 pos[2] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr) * INCHES_TO_METERS;
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 Liberty 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 //--------------------------------------------------------------------
657 // If this sensor has button on it, decode the button values
658 // into the button device and mainloop the button device so that
659 // it will report any changes.
660 //--------------------------------------------------------------------
661
663 // Read the integer value of the bytton status from the record.
664 vrpn_uint32 button_status = vrpn_unbuffer_from_little_endian<vrpn_uint32>(bufptr);
665
666 stylus_buttons[d_sensor]->set_button(0, button_status);
668 }
669
670 //--------------------------------------------------------------------
671 // Done with the decoding, set the report to ready
672 //--------------------------------------------------------------------
673
675 bufcount = 0;
676
677#ifdef VERBOSE2
679#endif
680
681 return 1;
682}
683
684// this routine is called when an "Stylus" button is encountered
685// by the tracker init string parser it sets up the VRPN button
686// device
687int vrpn_Tracker_Liberty::add_stylus_button(const char *button_device_name, int sensor, int numbuttons)
688{
689 // Make sure this is a valid sensor
690 if ( (sensor < 0) || (sensor >= num_stations) ) {
691 return -1;
692 }
693
694 // Add a new button device and set the pointer to point at it.
695 try { stylus_buttons[sensor] = new vrpn_Button_Server(button_device_name, d_connection, numbuttons); }
696 catch (...) {
697 VRPN_MSG_ERROR("Cannot open button device");
698 return -1;
699 }
700
701 // Send a new station-format command to the tracker so it will report the button states.
702 return set_sensor_output_format(sensor);
703}
vrpn_Connection * d_connection
Connection that this object talks to.
virtual void mainloop()
Called once each time through the server program's mainloop to handle various functions (like setting...
Definition: vrpn_Button.C:471
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:477
Generic connection class not specific to the transport mechanism.
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),...
vrpn_Button_Server * stylus_buttons[vrpn_LIBERTY_MAX_STATIONS]
struct timeval liberty_zerotime
int report_length(int sensor)
Augments the basic Liberty report length.
int add_stylus_button(const char *button_device_name, int sensor, int numbuttons=1)
Add a stylus (with button) to one of the sensors.
int set_sensor_output_format(int sensor)
Augments the basic Liberty format.
virtual void reset()
Reset the tracker.
unsigned char buffer[VRPN_TRACKER_BUF_SIZE]
Definition: vrpn_Tracker.h:155
vrpn_uint32 bufcount
Definition: vrpn_Tracker.h:157
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
struct timeval watchdog_timestamp
Definition: vrpn_Tracker.h:111
vrpn_int32 d_sensor
Definition: vrpn_Tracker.h:94
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
void print_latest_report(void)
Definition: vrpn_Tracker.C:325
struct timeval timestamp
Definition: vrpn_Tracker.h:100
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
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)
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
Definition: vrpn_Serial.C:651
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
Definition: vrpn_Serial.C:438
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:488
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
Definition: vrpn_Serial.C:515
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
void vrpn_SleepMsecs(double dMilliSecs)
Definition: vrpn_Shared.C:166
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...
Definition: vrpn_Shared.h:510
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:99
const int vrpn_TRACKER_FAIL
Definition: vrpn_Tracker.h:40
const int vrpn_TRACKER_SYNCING
Definition: vrpn_Tracker.h:35
const int vrpn_TRACKER_PARTIAL
Definition: vrpn_Tracker.h:38
const int vrpn_TRACKER_AWAITING_STATION
Definition: vrpn_Tracker.h:36
#define INCHES_TO_METERS
const int vrpn_LIBERTY_MAX_STATIONS
const int vrpn_LIBERTY_MAX_WHOAMI_LEN
class VRPN_API vrpn_Button_Server