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)
24 unsigned char reset[10];
35 reset[resetLen++] = (char) (13);
38 reset[resetLen++] =
'Y';
42 reset[resetLen++] =
't';
44 reset[resetLen++] =
'W';
45 reset[resetLen++] = (char) (11);
47 reset[resetLen++] = (char) (25);
49 for (i = 0; i < resetLen; i++) {
54 perror(
"3Space: Failed writing to tracker");
66 unsigned char scrap[80];
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]);
73 fprintf(stderr,
"[0x%02X]",scrap[i]);
76 fprintf(stderr,
"\n");
84 perror(
" 3Space write failed");
90 unsigned char statusmsg[56];
92 fprintf(stderr,
" Got %d of 55 characters for status\n",ret);
94 if ( (statusmsg[0]!=
'2') || (statusmsg[54]!=(
char)(10)) ) {
97 fprintf(stderr,
" Tracker: status is (");
98 for (i = 0; i < 55; i++) {
99 if (isprint(statusmsg[i])) {
100 fprintf(stderr,
"%c",statusmsg[i]);
102 fprintf(stderr,
"[0x%02X]",statusmsg[i]);
105 fprintf(stderr,
")\n Bad status report from tracker, retrying reset\n");
119 perror(
" 3Space write failed");
129 perror(
" 3Space write failed");
131 fprintf(stderr,
" 3Space set to continuous mode\n");
134 fprintf(stderr,
" (at the end of 3Space reset routine)\n");
156 if ( (
buffer[0] & 0x80) == 0) {
187 unsigned char decode[17];
190 const unsigned char mask[8] = {0x01, 0x02, 0x04, 0x08,
191 0x10, 0x20, 0x40, 0x80 };
206 for (i=0; i<7; i++) {
208 if ( (
buffer[7] & mask[i]) != 0) {
209 decode[i] |= (
unsigned char)(0x80);
214 for (i=7; i<14; i++) {
216 if ( (
buffer[15] & mask[i-7]) != 0) {
217 decode[i] |= (
unsigned char)(0x80);
222 for (i=14; i<17; i++) {
224 if ( (
buffer[19] & mask[i-14]) != 0) {
225 decode[i] |= (
unsigned char)(0x80);
235 unsigned char * unbufPtr = &decode[3];
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);
252 for (i=0; i<4; i++) {