vrpn 07.35
Virtual Reality Peripheral Network
 
Loading...
Searching...
No Matches
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.
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 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 int get_report(void)
Returns 0 if didn't get a complete report, 1 if it did.
virtual void reset()
Reset the tracker.
Definition vrpn_3Space.C:21
unsigned char buffer[VRPN_TRACKER_BUF_SIZE]
vrpn_float64 d_quat[4]
vrpn_int32 d_sensor
vrpn_float64 pos[3]
void print_latest_report(void)
struct timeval timestamp
#define T_3_BINARY_TO_METERS
Definition vrpn_3Space.C:19
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
@ vrpn_TEXT_WARNING
@ vrpn_TEXT_ERROR
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
void vrpn_SleepMsecs(double dMilliSecs)
#define vrpn_gettimeofday
Definition vrpn_Shared.h:99
const int vrpn_TRACKER_FAIL
const int vrpn_TRACKER_SYNCING
const int vrpn_TRACKER_PARTIAL