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