vrpn  07.33
Virtual Reality Peripheral Network
vrpn_3Space.C
Go to the documentation of this file.
1 #include <ctype.h> // for isprint
2 #include <math.h> // for sqrt
3 #include <stdio.h> // for fprintf, stderr, perror, etc
4 
5 #include "quat.h" // for Q_W, Q_X, Q_Y, Q_Z
6 #include "vrpn_3Space.h"
7 #include "vrpn_BaseClass.h" // for ::vrpn_TEXT_ERROR, etc
8 #include "vrpn_Serial.h" // for vrpn_write_characters, etc
9 #include "vrpn_Shared.h" // for vrpn_SleepMsecs, etc
10 #include "vrpn_Tracker.h" // for vrpn_TRACKER_FAIL, etc
11 #include "vrpn_Types.h" // for vrpn_int16, vrpn_float64
12 
13 // This constant turns the tracker binary values in the range -32768 to
14 // 32768 to meters.
15 #define T_3_DATA_MAX (32768.0)
16 #define T_3_INCH_RANGE (65.48)
17 #define T_3_CM_RANGE (T_3_INCH_RANGE * 2.54)
18 #define T_3_METER_RANGE (T_3_CM_RANGE / 100.0)
19 #define T_3_BINARY_TO_METERS (T_3_METER_RANGE / T_3_DATA_MAX)
20 
22 {
23  int i,resetLen,ret;
24  unsigned char reset[10];
25 
26  // Send the tracker a string that should reset it. The first time we
27  // try this, just do the normal ^Y reset. Later, try to reset
28  // to the factory defaults. Then toggle the extended mode.
29  // Then put in a carriage return to try and break it out of
30  // a query mode if it is in one. These additions are cumulative: by the
31  // end, we're doing them all.
32  resetLen = 0;
33  d_numResets++; // We're trying another reset
34  if (d_numResets > 1) { // Try to get it out of a query loop if its in one
35  reset[resetLen++] = (char) (13); // Return key -> get ready
36  }
37  if (d_numResets > 7) {
38  reset[resetLen++] = 'Y'; // Put tracker into tracking (not point) mode
39  }
40  if (d_numResets > 3) { // Get a little more aggressive
41  if (d_numResets > 4) { // Even more aggressive
42  reset[resetLen++] = 't'; // Toggle extended mode (in case it is on)
43  }
44  reset[resetLen++] = 'W'; // Reset to factory defaults
45  reset[resetLen++] = (char) (11); // Ctrl + k --> Burn settings into EPROM
46  }
47  reset[resetLen++] = (char) (25); // Ctrl + Y -> reset the tracker
49  for (i = 0; i < resetLen; i++) {
50  if (vrpn_write_characters(serial_fd, &reset[i], 1) == 1) {
51  vrpn_SleepMsecs(1000*2); // Wait 2 seconds each character
52  } else {
53  send_text_message("Failed writing to tracker", timestamp, vrpn_TEXT_ERROR, d_numResets);
54  perror("3Space: Failed writing to tracker");
56  return;
57  }
58  }
59  vrpn_SleepMsecs(1000.0*10); // Sleep to let the reset happen
60 
61  // Get rid of the characters left over from before the reset
63 
64  // Make sure that the tracker has stopped sending characters
65  vrpn_SleepMsecs(1000.0*2);
66  unsigned char scrap[80];
67  if ( (ret = vrpn_read_available_characters(serial_fd, scrap, 80)) != 0) {
68  fprintf(stderr," 3Space warning: got >=%d characters after reset:\n",ret);
69  for (i = 0; i < ret; i++) {
70  if (isprint(scrap[i])) {
71  fprintf(stderr,"%c",scrap[i]);
72  } else {
73  fprintf(stderr,"[0x%02X]",scrap[i]);
74  }
75  }
76  fprintf(stderr, "\n");
77  vrpn_flush_input_buffer(serial_fd); // Flush what's left
78  }
79 
80  // Asking for tracker status
81  if (vrpn_write_characters(serial_fd, (const unsigned char *) "S", 1) == 1) {
82  vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
83  } else {
84  perror(" 3Space write failed");
86  return;
87  }
88 
89  // Read Status
90  unsigned char statusmsg[56];
91  if ( (ret = vrpn_read_available_characters(serial_fd, statusmsg, 55)) != 55){
92  fprintf(stderr, " Got %d of 55 characters for status\n",ret);
93  }
94  if ( (statusmsg[0]!='2') || (statusmsg[54]!=(char)(10)) ) {
95  int i;
96  statusmsg[55] = '\0'; // Null-terminate the string
97  fprintf(stderr, " Tracker: status is (");
98  for (i = 0; i < 55; i++) {
99  if (isprint(statusmsg[i])) {
100  fprintf(stderr,"%c",statusmsg[i]);
101  } else {
102  fprintf(stderr,"[0x%02X]",statusmsg[i]);
103  }
104  }
105  fprintf(stderr, ")\n Bad status report from tracker, retrying reset\n");
106  return;
107  } else {
108  send_text_message("Got status (tracker back up)!", timestamp, vrpn_TEXT_ERROR, 0);
109  d_numResets = 0; // Success, use simple reset next time
110  }
111 
112  // Set output format to be position,quaternion
113  // These are a capitol 'o' followed by comma-separated values that
114  // indicate data sets according to appendix F of the 3Space manual,
115  // then followed by character 13 (octal 15).
116  if (vrpn_write_characters(serial_fd, (const unsigned char *)"O2,11\015", 6) == 6) {
117  vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
118  } else {
119  perror(" 3Space write failed");
121  return;
122  }
123 
124  // Set data format to BINARY mode
125  vrpn_write_characters(serial_fd, (const unsigned char *)"f", 1);
126 
127  // Set tracker to continuous mode
128  if (vrpn_write_characters(serial_fd,(const unsigned char *) "C", 1) != 1)
129  perror(" 3Space write failed");
130  else {
131  fprintf(stderr, " 3Space set to continuous mode\n");
132  }
133 
134  fprintf(stderr, " (at the end of 3Space reset routine)\n");
135  vrpn_gettimeofday(&timestamp, NULL); // Set watchdog now
136  status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
137 }
138 
139 
141 {
142  int ret;
143 
144  // The reports are each 20 characters long, and each start with a
145  // byte that has the high bit set and no other bytes have the high
146  // bit set. If we're synching, read a byte at a time until we find
147  // one with the high bit set.
148  if (status == vrpn_TRACKER_SYNCING) {
149  // Try to get a character. If none, just return.
151  return 0;
152  }
153 
154  // If the high bit isn't set, we don't want it we
155  // need to look at the next one, so just return
156  if ( (buffer[0] & 0x80) == 0) {
157  send_text_message("Syncing (high bit not set)", timestamp, vrpn_TEXT_WARNING);
158  return 0;
159  }
160 
161  // Got the first character of a report -- go into PARTIAL mode
162  // and say that we got one character at this time.
163  bufcount = 1;
166  }
167 
168  // Read as many bytes of this 20 as we can, storing them
169  // in the buffer. We keep track of how many have been read so far
170  // and only try to read the rest. The routine that calls this one
171  // makes sure we get a full reading often enough (ie, it is responsible
172  // for doing the watchdog timing to make sure the tracker hasn't simply
173  // stopped sending characters).
175  20-bufcount);
176  if (ret == -1) {
177  send_text_message("Error reading, resetting", timestamp, vrpn_TEXT_ERROR);
179  return 0;
180  }
181  bufcount += ret;
182  if (bufcount < 20) { // Not done -- go back for more
183  return 0;
184  }
185 
186  { // Decode the report
187  unsigned char decode[17];
188  int i;
189 
190  const unsigned char mask[8] = {0x01, 0x02, 0x04, 0x08,
191  0x10, 0x20, 0x40, 0x80 };
192  // Clear the MSB in the first byte
193  buffer[0] &= 0x7F;
194 
195  // Decode the 3Space binary representation into standard
196  // 8-bit bytes. This is done according to page 4-4 of the
197  // 3Space user's manual, which says that the high-order bits
198  // of each group of 7 bytes is packed into the 8th byte of the
199  // group. Decoding involves setting those bits in the bytes
200  // iff their encoded counterpart is set and then skipping the
201  // byte that holds the encoded bits.
202  // We decode from buffer[] into decode[] (which is 3 bytes
203  // shorter due to the removal of the bit-encoding bytes).
204 
205  // decoding from buffer[0-6] into decode[0-6]
206  for (i=0; i<7; i++) {
207  decode[i] = buffer[i];
208  if ( (buffer[7] & mask[i]) != 0) {
209  decode[i] |= (unsigned char)(0x80);
210  }
211  }
212 
213  // decoding from buffer[8-14] into decode[7-13]
214  for (i=7; i<14; i++) {
215  decode[i] = buffer[i+1];
216  if ( (buffer[15] & mask[i-7]) != 0) {
217  decode[i] |= (unsigned char)(0x80);
218  }
219  }
220 
221  // decoding from buffer[16-18] into decode[14-16]
222  for (i=14; i<17; i++) {
223  decode[i] = buffer[i+2];
224  if ( (buffer[19] & mask[i-14]) != 0) {
225  decode[i] |= (unsigned char)(0x80);
226  }
227  }
228 
229  // Parse out sensor number, which is the second byte and is
230  // stored as the ASCII number of the sensor, with numbers
231  // starting from '1'. We turn it into a zero-based unit number.
232  d_sensor = decode[1] - '1';
233 
234  // Position
235  unsigned char * unbufPtr = &decode[3];
236  pos[0] = vrpn_unbuffer_from_little_endian<vrpn_int16>(unbufPtr) * T_3_BINARY_TO_METERS;
237  pos[1] = vrpn_unbuffer_from_little_endian<vrpn_int16>(unbufPtr) * T_3_BINARY_TO_METERS;
238  pos[2] = vrpn_unbuffer_from_little_endian<vrpn_int16>(unbufPtr) * T_3_BINARY_TO_METERS;
239 
240  // Quarternion orientation. The 3Space gives quaternions
241  // as w,x,y,z while the VR code handles them as x,y,z,w,
242  // so we need to switch the order when decoding. Also the
243  // tracker does not normalize the quaternions.
244  d_quat[Q_W] = vrpn_unbuffer_from_little_endian<vrpn_int16>(unbufPtr);
245  d_quat[Q_X] = vrpn_unbuffer_from_little_endian<vrpn_int16>(unbufPtr);
246  d_quat[Q_Y] = vrpn_unbuffer_from_little_endian<vrpn_int16>(unbufPtr);
247  d_quat[Q_Z] = vrpn_unbuffer_from_little_endian<vrpn_int16>(unbufPtr);
248 
249  //Normalize quaternion
250  double norm = sqrt ( d_quat[0]*d_quat[0] + d_quat[1]*d_quat[1]
251  + d_quat[2]*d_quat[2] + d_quat[3]*d_quat[3]);
252  for (i=0; i<4; i++) {
253  d_quat[i] /= norm;
254  }
255 
256  // Done with the decoding, set the report to ready
257  // Ready for another report
259  bufcount = 0;
260  }
261 
262  return 1; // Got a report.
263 #ifdef VERBOSE
265 #endif
266 }
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
Definition: vrpn_Serial.C:643
void vrpn_SleepMsecs(double dMsecs)
Definition: vrpn_Shared.C:157
#define T_3_BINARY_TO_METERS
Definition: vrpn_3Space.C:19
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
Definition: vrpn_Serial.C:435
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
unsigned char buffer[VRPN_TRACKER_BUF_SIZE]
Definition: vrpn_Tracker.h:155
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
const int vrpn_TRACKER_FAIL
Definition: vrpn_Tracker.h:40
vrpn_int32 d_sensor
Definition: vrpn_Tracker.h:94
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
Definition: vrpn_Serial.C:512
const int vrpn_TRACKER_PARTIAL
Definition: vrpn_Tracker.h:38
const int vrpn_TRACKER_SYNCING
Definition: vrpn_Tracker.h:35
vrpn_uint32 bufcount
Definition: vrpn_Tracker.h:157
int send_text_message(const char *msg, struct timeval timestamp, vrpn_TEXT_SEVERITY type=vrpn_TEXT_NORMAL, vrpn_uint32 level=0)
Sends a NULL-terminated text message from the device d_sender_id.
virtual void reset()
Reset the tracker.
Definition: vrpn_3Space.C:21
virtual int get_report(void)
Returns 0 if didn't get a complete report, 1 if it did.
Definition: vrpn_3Space.C:140
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
struct timeval timestamp
Definition: vrpn_Tracker.h:100
void print_latest_report(void)
Definition: vrpn_Tracker.C:306
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95