vrpn 07.35
Virtual Reality Peripheral Network
Loading...
Searching...
No Matches
vrpn_Tracker_Isotrak.C
Go to the documentation of this file.
1// vrpn_Tracker_Isotrak.C
2// This file contains the code to operate a Polhemus Isotrack Tracker.
3// This file is based on the vrpn_Tracker_Fastrack.C file, with modifications made
4// to allow it to operate a Isotrack instead. The modifications are based
5// on the old version of the Isotrack driver.
6// This version was written in the Spring 2006 by Bruno Herbelin.
7//
8// Revised by Charles B. Owen (cbowen@cse.msu.edu) 1-20-2012
9// Revisions:
10//
11// The version before was reporting in centimeters. Revised to report in Meters per VRPN requirements.
12// The version before negated the coordinates. I have no idea why. That has been fixed.
13// Now reads the binary format instead of ASCII. It's faster and supports the stylus button.
14// Now supports a stylus button for either channel.
15
16
17#include <ctype.h> // for isprint
18#include <stdio.h> // for fprintf, perror, sprintf, etc
19#include <stdlib.h> // for atoi
20#include <string.h> // for strlen, strtok
21
22#include "vrpn_BaseClass.h" // for ::vrpn_TEXT_WARNING, 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
29#include "vrpn_Types.h" // for vrpn_uint8, vrpn_float64, etc
30#include "vrpn_MessageMacros.h" // for VRPN_MSG_INFO, VRPN_MSG_WARNING, VRPN_MSG_ERROR
31
32const int BINARY_RECORD_SIZE = 20;
33
35 const char *port, long baud, int enable_filtering, int numstations,
36 const char *additional_reset_commands) :
37 vrpn_Tracker_Serial(name,c,port,baud),
38 do_filter(enable_filtering),
39 num_stations(numstations>vrpn_ISOTRAK_MAX_STATIONS ? vrpn_ISOTRAK_MAX_STATIONS : numstations),
40 num_resets(0)
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
50 for(int i=0; i<vrpn_ISOTRAK_MAX_STATIONS; i++) {
51 stylus_buttons[i] = NULL;
52 }
53}
54
56{
57
58}
59
60
69{
70 char outstring[16];
71
72 // Set output format for the station to be position, quaternion
73 // Don't need the space anymore, though
74 sprintf(outstring, "O2,11\r");
75
76 if (vrpn_write_characters(serial_fd, (const unsigned char *)outstring,
77 strlen(outstring)) == (int)strlen(outstring)) {
78 vrpn_SleepMsecs(50); // Sleep for a bit to let command run
79 } else {
80 VRPN_MSG_ERROR("Write failed on format command");
82 return -1;
83 }
84 return 0;
85}
86
87// This routine will reset the tracker and set it to generate the types
88// of reports we want.
89// This was based on the Isotrak User Manual from Polhemus (2001 Edition, Rev A)
90
92{
93 int i,resetLen,ret;
94 unsigned char reset[10];
95 char errmsg[512];
96
97 //--------------------------------------------------------------------
98 // This section deals with resetting the tracker to its default state.
99 // Multiple attempts are made to reset, getting more aggressive each
100 // time. This section completes when the tracker reports a valid status
101 // message after the reset has completed.
102 //--------------------------------------------------------------------
103
104 // Send the tracker a string that should reset it. The first time we
105 // try this, just do the normal 'c' command to put it into polled mode.
106 // After a few tries with this, use a [return] character, and then use the ^Y to reset.
107
108 resetLen = 0;
109 num_resets++;
110
111 // We're trying another reset
112 if (num_resets > 1) { // Try to get it out of a query loop if its in one
113 reset[resetLen++] = (unsigned char) (13); // Return key -> get ready
114 }
115
116 if (num_resets > 2) {
117 reset[resetLen++] = (unsigned char) (25); // Ctrl + Y -> reset the tracker
118 }
119
120 reset[resetLen++] = 'c'; // Put it into polled (not continuous) mode
121
122
123 sprintf(errmsg, "Resetting the tracker (attempt %d)", num_resets);
124 VRPN_MSG_WARNING(errmsg);
125
126 for (i = 0; i < resetLen; i++) {
127 if (vrpn_write_characters(serial_fd, &reset[i], 1) == 1) {
128 fprintf(stderr,".");
129 vrpn_SleepMsecs(1000.0*2); // Wait after each character to give it time to respond
130 } else {
131 perror("Isotrack: Failed writing to tracker");
133 return;
134 }
135 }
136 //XXX Take out the sleep and make it keep spinning quickly
137 if (num_resets > 2) {
138 vrpn_SleepMsecs(1000.0*20); // Sleep to let the reset happen, if we're doing ^Y
139 }
140
141 fprintf(stderr,"\n");
142
143 // Get rid of the characters left over from before the reset
145
146 // Make sure that the tracker has stopped sending characters
147 vrpn_SleepMsecs(1000.0*2);
148 unsigned char scrap[80];
149 if ( (ret = vrpn_read_available_characters(serial_fd, scrap, 80)) != 0) {
150 sprintf(errmsg,"Got >=%d characters after reset",ret);
151 VRPN_MSG_WARNING(errmsg);
152 for (i = 0; i < ret; i++) {
153 if (isprint(scrap[i])) {
154 fprintf(stderr,"%c",scrap[i]);
155 } else {
156 fprintf(stderr,"[0x%02X]",scrap[i]);
157 }
158 }
159 fprintf(stderr, "\n");
160 vrpn_flush_input_buffer(serial_fd); // Flush what's left
161 }
162
163 // Asking for tracker status
164 if (vrpn_write_characters(serial_fd, (const unsigned char *) "S", 1) == 1) {
165 vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
166 } else {
167 perror(" Isotrack write failed");
169 return;
170 }
171
172 // Read Status
173 unsigned char statusmsg[22];
174
175 // Attempt to read 21 characters.
176 ret = vrpn_read_available_characters(serial_fd, statusmsg, 21);
177
178 if ( (ret != 21) ) {
179 fprintf(stderr,
180 " Got %d of 21 characters for status\n",ret);
181 VRPN_MSG_ERROR("Bad status report from Isotrack, retrying reset");
182 return;
183 }
184 else if ( (statusmsg[0]!='2') ) {
185 int i;
186 statusmsg[sizeof(statusmsg) - 1] = '\0'; // Null-terminate the string
187 fprintf(stderr, " Isotrack: bad status (");
188 for (i = 0; i < ret; i++) {
189 if (isprint(statusmsg[i])) {
190 fprintf(stderr,"%c",statusmsg[i]);
191 } else {
192 fprintf(stderr,"[0x%02X]",statusmsg[i]);
193 }
194 }
195 fprintf(stderr,")\n");
196 VRPN_MSG_ERROR("Bad status report from Isotrack, retrying reset");
197 return;
198 } else {
199 VRPN_MSG_WARNING("Isotrack gives correct status (this is good)");
200 num_resets = 0; // Success, use simple reset next time
201 }
202
203 //--------------------------------------------------------------------
204 // Now that the tracker has given a valid status report, set all of
205 // the parameters the way we want them. We rely on power-up setting
206 // based on the receiver select switches to turn on the receivers that
207 // the user wants.
208 //--------------------------------------------------------------------
209
210 // Set output format. This is done once for the Isotrak, not per channel.
212 return;
213 }
214
215 // Enable filtering if the constructor parameter said to.
216 // Set filtering for both position (x command) and orientation (v command)
217 // to the values that are recommended as a "jumping off point" in the
218 // Isotrack manual.
219
220 if (do_filter) {
221 if (vrpn_write_characters(serial_fd, (const unsigned char *)"x0.2,0.2,0.8,0.8\015", 17) == 17) {
222 vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
223 } else {
224 perror(" Isotrack write position filter failed");
226 return;
227 }
228 if (vrpn_write_characters(serial_fd, (const unsigned char *)"v0.2,0.2,0.8,0.8\015", 17) == 17) {
229 vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
230 } else {
231 perror(" Isotrack write orientation filter failed");
233 return;
234 }
235 }
236
237 // RESET Alignment reference frame
238 if (vrpn_write_characters(serial_fd, (const unsigned char *) "R1\r", 3) != 3) {
239 perror(" Isotrack write failed");
241 return;
242 } else {
243 VRPN_MSG_WARNING("Isotrack reset ALIGNMENT reference frame (this is good)");
244 }
245
246 // reset BORESIGHT
247 if (vrpn_write_characters(serial_fd, (const unsigned char *) "b1\r", 3) != 3) {
248 perror(" Isotrack write failed");
250 return;
251 } else {
252 VRPN_MSG_WARNING("Isotrack reset BORESIGHT (this is good)");
253 }
254
255 // Set data format to METRIC mode
256 if (vrpn_write_characters(serial_fd, (const unsigned char *) "u", 1) != 1) {
257 perror(" Isotrack write failed");
259 return;
260 } else {
261 VRPN_MSG_WARNING("Isotrack set to metric units (this is good)");
262 }
263
264
265
266 // Send the additional reset commands, if any, to the tracker.
267 // These commands come in lines, with character \015 ending each
268 // line. If a line start with an asterisk (*), treat it as a pause
269 // command, with the number of seconds to wait coming right after
270 // the asterisk. Otherwise, the line is sent directly to the tracker.
271 // Wait a while for them to take effect, then clear the input
272 // buffer.
273 if (strlen(add_reset_cmd) > 0) {
274 char *next_line;
275 char add_cmd_copy[sizeof(add_reset_cmd)];
276 char string_to_send[sizeof(add_reset_cmd)];
277 int seconds_to_wait;
278
279 printf(" Isotrack writing extended reset commands...\n");
280
281 // Make a copy of the additional reset string, since it is consumed
282 vrpn_strcpy(add_cmd_copy, add_reset_cmd);
283
284 // Pass through the string, testing each line to see if it is
285 // a sleep command or a line to send to the tracker. Continue until
286 // there are no more line delimiters ('\015'). Be sure to write the
287 // \015 to the end of the string sent to the tracker.
288 // Note that strok() puts a NULL character in place of the delimiter.
289
290 next_line = strtok(add_cmd_copy, "\015");
291 while (next_line != NULL) {
292 if (next_line[0] == '*') { // This is a "sleep" line, see how long
293 seconds_to_wait = atoi(&next_line[1]);
294 fprintf(stderr," ...sleeping %d seconds\n",seconds_to_wait);
295 vrpn_SleepMsecs(1000.0*seconds_to_wait);
296 } else { // This is a command line, send it
297 sprintf(string_to_send, "%s\015", next_line);
298 fprintf(stderr, " ...sending command: %s\n", string_to_send);
300 (const unsigned char *)string_to_send,strlen(string_to_send));
301 }
302 next_line = strtok(next_line+strlen(next_line)+1, "\015");
303 }
304
305 // Sleep a little while to let this finish, then clear the input buffer
306 vrpn_SleepMsecs(1000.0*2);
308 }
309
310 // Set data format to BINARY mode
311 // F = ASCII, f = binary
312 if (vrpn_write_characters(serial_fd, (const unsigned char *) "f", 1) != 1) {
313 perror(" Isotrack write failed");
315 return;
316 } else {
317 VRPN_MSG_WARNING("Isotrack set to BINARY mode (this is good)");
318 }
319
320
321 // Set tracker to continuous mode
322 if (vrpn_write_characters(serial_fd, (const unsigned char *) "C", 1) != 1) {
323 perror(" Isotrack write failed");
325 return;
326 } else {
327 VRPN_MSG_WARNING("Isotrack set to continuous mode (this is good)");
328 }
329
330 VRPN_MSG_WARNING("Reset Completed.");
331
332 status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
333
334 // Ok, device is ready, we want to calibrate to sensor 1 current position/orientation
335 while(get_report() != 1);
336
337 // Done with reset.
338 vrpn_gettimeofday(&timestamp, NULL); // Set watchdog now
339
340 status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
341}
342
343
344
345// This function will read characters until it has a full report, then
346// put that report into the time, sensor, pos and quat fields so that it can
347// be sent the next time through the loop. The time stored is that of
348// the first character received as part of the report. Reports start with
349// the header "0xy", where x is the station number and y is either the
350// space character or else one of the characters "A-F". Characters "A-F"
351// indicate weak signals and so forth, but in practice it is much harder
352// to deal with them than to ignore them (they don't indicate hard error
353// conditions). The report follows, 7 values in 7 characters each. The first three
354// are position in X,Y and Z. The next four are the quaternion in the
355// order W, X,Y,Z.
356// If we get a report that is not valid, we assume that we have lost a
357// character or something and re-synchronize with the Isotrack by waiting
358// until the start-of-report character ('0') comes around again.
359// The routine that calls this one makes sure we get a full reading often
360// enough (ie, it is responsible for doing the watchdog timing to make sure
361// the tracker hasn't simply stopped sending characters).
362
364{
365 char errmsg[512]; // Error message to send to VRPN
366 int ret; // Return value from function call to be checked
367
368 // The first byte of a binary record has the high order bit set
369
371 // Try to get a character. If none, just return.
373 return 0;
374 }
375
376 // The first byte of a record has the high order bit set
377 if(!(buffer[0] & 0x80)) {
378 sprintf(errmsg,"While syncing (looking for byte with high order bit set, "
379 "got '%x')", buffer[0]);
380 VRPN_MSG_WARNING(errmsg);
382
383 return 0;
384 }
385
386 // Got the first byte of a report -- go into TRACKER_PARTIAL mode
387 // and record that we got one character at this time.
388 bufcount = 1;
391 }
392
393 //--------------------------------------------------------------------
394 // Read as many bytes of this report as we can, storing them
395 // in the buffer. We keep track of how many have been read so far
396 // and only try to read the rest. The routine that calls this one
397 // makes sure we get a full reading often enough (ie, it is responsible
398 // for doing the watchdog timing to make sure the tracker hasn't simply
399 // stopped sending characters).
400 //--------------------------------------------------------------------
401
404 if (ret == -1) {
405 VRPN_MSG_ERROR("Error reading report");
407 return 0;
408 }
409
410 bufcount += ret;
411
412 if (bufcount < BINARY_RECORD_SIZE) { // Not done -- go back for more
413 return 0;
414 }
415
416 // We now have enough characters for a full report
417 // Check it to ensure we do not have a high bit set other
418 // than on the first byte
419 for(int i=1; i<BINARY_RECORD_SIZE; i++)
420 {
421 if (buffer[i] & 0x80) {
423
424 sprintf(errmsg,"Unexpected sync character in record");
425 VRPN_MSG_WARNING(errmsg);
426
427 //VRPN_MSG_WARNING("Not '0' in record, re-syncing");
429 return 0;
430 }
431 }
432
433 // Create a buffer for the decoded message
434 unsigned char decoded[BINARY_RECORD_SIZE];
435 int d = 0;
436
437 int fullgroups = BINARY_RECORD_SIZE / 8;
438
439 // The following decodes the Isotrak binary format. It consists of
440 // 7 byte values plus an extra byte of the high bit for these
441 // 7 bytes. First, loop over the 7 byte ranges (8 bytes in binary)
442 int i;
443 for(i = 0; i<fullgroups; i++)
444 {
445 vrpn_uint8 *group = &buffer[i * 8];
446 vrpn_uint8 high = buffer[i * 8 + 7];
447
448 for(int j=0; j<7; j++)
449 {
450 decoded[d] = *group++;
451 if(high & 1)
452 decoded[d] |= 0x80;
453
454 d++;
455 high >>= 1;
456 }
457 }
458
459 // We'll have X bytes left at the end
460 int left = BINARY_RECORD_SIZE - fullgroups * 8;
461 vrpn_uint8 *group = &buffer[fullgroups * 8];
462 vrpn_uint8 high = buffer[fullgroups * 8 + left - 1];
463
464 for(int j=0; j<left-1; j++)
465 {
466 decoded[d] = *group++;
467 if(high & 1)
468 decoded[d] |= 0x80;
469
470 d++;
471 high >>= 1;
472 }
473
474 // ASCII value of 1 == 49 subtracing 49 gives the sensor number
475 d_sensor = decoded[1] - 49; // Convert ASCII 1 to sensor 0 and so on.
476 if ( (d_sensor < 0) || (d_sensor >= num_stations) ) {
478 sprintf(errmsg,"Bad sensor # (%d) in record, re-syncing", d_sensor);
479 VRPN_MSG_WARNING(errmsg);
481 return 0;
482 }
483
484
485
486 // Extract the important information
487 vrpn_uint8 *item = &decoded[3];
488
489 // This is a scale factor from the Isotrak manual
490 // This will convert the values to meters, the standard vrpn format
491 double mul = 1.6632 / 32767.;
492 float div = 1.f / 32767.f; // Fractional amount for angles
493
494 pos[0] = ( (vrpn_int8(item[1]) << 8) + item[0]) * mul; item += 2;
495 pos[1] = ( (vrpn_int8(item[1]) << 8) + item[0]) * mul; item += 2;
496 pos[2] = ( (vrpn_int8(item[1]) << 8) + item[0]) * mul; item += 2;
497 d_quat[3] = ( (vrpn_int8(item[1]) << 8) + item[0]) * div; item += 2;
498 d_quat[0] = ( (vrpn_int8(item[1]) << 8) + item[0]) * div; item += 2;
499 d_quat[1] = ( (vrpn_int8(item[1]) << 8) + item[0]) * div; item += 2;
500 d_quat[2] = ( (vrpn_int8(item[1]) << 8) + item[0]) * div;
501
502 //--------------------------------------------------------------------
503 // If this sensor has button on it, decode the button values
504 // into the button device and mainloop the button device so that
505 // it will report any changes.
506 //--------------------------------------------------------------------
507
508 if(stylus_buttons[d_sensor] != NULL)
509 {
510 char button = decoded[2];
511 if(button == '@' || button == '*')
512 {
513 stylus_buttons[d_sensor]->set_button(0, button == '@');
514
515 }
516
518 }
519
520 //--------------------------------------------------------------------
521 // Done with the decoding,
522 // set the report to ready
523 //--------------------------------------------------------------------
525 bufcount = 0;
526
527 #ifdef VERBOSE2
529 #endif
530
531 return 1;
532}
533
534
535// this routine is called when an "Stylus" button is encountered
536// by the tracker init string parser it sets up the VRPN button
537// device
538int vrpn_Tracker_Isotrak::add_stylus_button(const char *button_device_name, int sensor)
539{
540 // Make sure this is a valid sensor
541 if ( (sensor < 0) || (sensor >= num_stations) ) {
542 return -1;
543 }
544
545 // Add a new button device and set the pointer to point at it.
546 try { stylus_buttons[sensor] = new vrpn_Button_Server(button_device_name, d_connection, 1); }
547 catch (...) {
548 VRPN_MSG_ERROR("Cannot open button device");
549 return -1;
550 }
551
552 return 0;
553}
554
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.
int set_sensor_output_format(int sensor)
This routine sets the device for position + quaternion It puts a space at the end so that we can chec...
vrpn_Tracker_Isotrak(const char *name, vrpn_Connection *c, const char *port="/dev/ttyS1", long baud=19200, int enable_filtering=1, int numstations=vrpn_ISOTRAK_MAX_STATIONS, const char *additional_reset_commands=NULL)
The constructor is given the name of the tracker (the name of the sender it should use),...
virtual int get_report(void)
Gets a report if one is available, returns 0 if not, 1 if complete report.
vrpn_Button_Server * stylus_buttons[vrpn_ISOTRAK_MAX_STATIONS]
virtual void reset()
Reset the tracker.
int add_stylus_button(const char *button_device_name, int sensor)
Add a stylus (with button) to one of the sensors.
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_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_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
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 BINARY_RECORD_SIZE
const int vrpn_ISOTRAK_MAX_STATIONS
class VRPN_API vrpn_Button_Server