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++) {
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
void vrpn_SleepMsecs(double dMsecs)
#define T_3_BINARY_TO_METERS
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
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]
const int vrpn_TRACKER_FAIL
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)
const int vrpn_TRACKER_PARTIAL
const int vrpn_TRACKER_SYNCING
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.
virtual int get_report(void)
Returns 0 if didn't get a complete report, 1 if it did.
#define vrpn_gettimeofday
void print_latest_report(void)