vrpn 07.35
Virtual Reality Peripheral Network
 
Loading...
Searching...
No Matches
vrpn_Dyna.C
Go to the documentation of this file.
1/* Modification to make it work with the SeerReal D4D Headtracker,
2 needs to send "0" instead of "V" to set it to continuous mode.
3 Untested if this still works with the Dynasight Tracker.*/
4#ifdef _WIN32
5#include <io.h>
6#else
7#endif
8#include <stdio.h> // for fprintf, stderr, NULL
9#include <string.h> // for strlen
10
11#include "vrpn_Dyna.h"
12#include "vrpn_Serial.h" // for vrpn_write_characters, etc
13#include "vrpn_Shared.h" // for vrpn_SleepMsecs, timeval, etc
14#include "vrpn_Types.h" // for vrpn_float64
15
17
18#define T_ERROR (-1)
19#define T_OK (0)
20
21/* return codes for status check */
22#define T_PDYN_SPEW_MODE (-50)
23#define T_PDYN_NO_DATA (-51)
24
25#define lOOO_OOOO (0x80)
26#define llll_OOOO (0xf0)
27#define OOOO_OOll (0x03)
28#define OOOO_llOO (0x0c)
29
31 char *name, vrpn_Connection *c, int cSensors,
32 const char *port, long baud ) :
33vrpn_Tracker_Serial(name,c,port,baud), cResets(0), cSensors(cSensors)
34{
36 fprintf(stderr, "\nvrpn_Tracker_Dyna: too many sensors requested ... only %d allowed (%d specified)", VRPN_DYNA_MAX_SENSORS, cSensors );
38 }
39 fprintf(stderr, "\nvrpn_Tracker_Dyna: starting up ...");
40 d_sensor = 0 ; // sensor id is always 0 (first sensor is 0);
41}
42
44 fprintf(stderr, "vrpn_Tracker_Dyna:Shutting down...\n");
45}
46
47
48int vrpn_Tracker_Dyna::get_status()
49{
50 int bytesRead;
51 unsigned char statusBuffer[256];
52
53 my_flush();
54
55 /* send request for status record */
56
57 vrpn_write_characters(serial_fd,(const unsigned char *) "\021", 1);
59 vrpn_SleepMsecs(1000.0*2);
60
61 /* do non-blocking read of status record */
62 bytesRead = vrpn_read_available_characters(serial_fd, statusBuffer, 8);
63 // T_PDYN_STATUS_RECORD_LENGTH =8;
64
65 if ( bytesRead == 8 )
66 {
67 /* we have correct length- check a few chars to make sure this is a valid
68 * record
69 */
70 if ( ((statusBuffer[0] & lOOO_OOOO) != lOOO_OOOO) ||
71 ((statusBuffer[1] & lOOO_OOOO) != lOOO_OOOO) )
72 return(T_ERROR);
73
74 /* otherwise, all is well */
75 return(T_OK);
76 }
77
78 /* if we get here, we either got too much data or not enough */
79
80 /* no data means it's probably disconnected or not turned on */
81 if ( bytesRead == 0 )
82 {
83// fprintf(stderr, "No data\n");
84 return(T_PDYN_NO_DATA);
85 }
86
87 /* if we got too much data, chances are that it's in continuous mode */
88 if ( bytesRead > 8)
89 {
90 fprintf(stderr, "3\n");
91 return(T_PDYN_SPEW_MODE);
92 }
93
94 /* if we get here, i have no idea what's going on- could be garbage on the
95 * serial line, wrong baud rate, or that the Dynasight is flaking out.
96 */
97 return(T_ERROR);
98
99} /* t_pdyn_get_status */
100
101#define MAX_TRIAL 10
102
104 //static int numResets = 0; // How many resets have we tried?;
105 static const char T_PDYN_C_CTL_C[4] ="\003\003\003";
106 static const int T_PDYN_RECORD_LENGTH = 8;
107
108 vrpn_write_characters(serial_fd, (const unsigned char*)T_PDYN_C_CTL_C, strlen(T_PDYN_C_CTL_C));
109 vrpn_write_characters(serial_fd,(const unsigned char *) "4", 1); // set to polling mode;
110
111 /* pause 1 second to allow the Dynasight buffer to stabilize */
112 vrpn_SleepMsecs(1000.0*1);
113
114 status = get_status();
115
116 if ( status != T_OK ) {
117
118 /* if no data, tracker probably not connected. just bag it. */
119 if ( status == T_PDYN_NO_DATA )
120 {
121 fprintf(stderr, "vrpn_Tracker_Dyna::reset(): no data (is tracker turned on?)\n");
123 return;
124
125 }
126
127 }else {
128 fprintf(stderr, "vrpn_Tracker_Dyna: return valid status report\n");
129 reportLength = T_PDYN_RECORD_LENGTH;
130
131 // set it to continues mode;
132 /* clear any leftover data */
133 my_flush();
134
135 /* set the Dynasight to continuous mode */
136 vrpn_write_characters(serial_fd, (const unsigned char*)T_PDYN_C_CTL_C, strlen(T_PDYN_C_CTL_C));
137 //vrpn_write_characters(serial_fd, (const unsigned char *)"V", 1);
138 vrpn_write_characters(serial_fd, (const unsigned char *)"0", 1);
139 //T_PDYN_C_CONTINUOUS = "V"
140 vrpn_SleepMsecs(1000.0*1);
141 //vrpn_gettimeofday(&timestamp, NULL); // Set watchdog now;
142 timestamp.tv_sec = -1;
143 status = vrpn_TRACKER_SYNCING; // We are trying for a new reading;
144 return;
145 }
146
147}
148
150 int ret;
152 if ((ret=vrpn_read_available_characters(serial_fd, buffer, 1)) != 1 ||
153 (buffer[0] & llll_OOOO) != lOOO_OOOO) {
154 return 0;
155 }
158 bufcount= ret;
159 }
162 reportLength-bufcount);
163 if (ret < 0) {
164 fprintf(stderr,"%s@%d: Error reading\n", __FILE__, __LINE__);
166 return 0;
167 }
168 bufcount += ret;
169 if (bufcount < reportLength) { // Not done -- go back for more
170 return 0;
171 }
172 }
173
174 if (!valid_report()) {
175 //fprintf(stderr,"no valid report");
176 bufcount = 0;
178 return 0;
179 }
180 //else fprintf(stderr,"got valid report");
181 decode_record();
183 bufcount=0;
184
185 return 1;
186}
187
188int vrpn_Tracker_Dyna::valid_report() {
189
190 /* Check to see that the first four bits of the *
191 * first two bytes correspond to 1000. */
192 if ( ( (buffer[0] & llll_OOOO) != lOOO_OOOO) ||
193 ( (buffer[1] & llll_OOOO) != lOOO_OOOO) )
194 return (0);
195
196
197 /* Make sure no other consecutive bytes have 1000 in the high nibble */
198 for (unsigned i = 2; i <reportLength-1; i += 2)
199 {
200 if ( ( (buffer[i] & llll_OOOO) == lOOO_OOOO) &&
201 ( (buffer[i+1] & llll_OOOO) == lOOO_OOOO))
202 {
203 fprintf(stderr,
204 "vrpn_Tracker_Dyna: found two more status bytes in the Dynasight's output list\n");
205 return (0);
206 }
207 }
208
209 /* Next, check that the Dynasight is tracking or in caution mode. If *
210 * it is not, then we can't be sure of what data is being sent back, *
211 * copy the most recent tracked data into the buffer. */
212 if ( ( (buffer[1] & OOOO_OOll) == 0 ) ||
213 ( (buffer[1] & OOOO_OOll) == 1 ) )
214 {
215 return (1);
216 }
217
218 /* Also, for this passive Dynasight mode, we want to make sure that we *
219 * only getting records from the single (00) sensor, not any active *
220 * sensors. */
221 if ( (buffer[0] & OOOO_llOO) != 0)
222 {
223 fprintf(stderr, "trackerlib: Invalid target number for passive Dynasight\n");
224 return (0);
225 }
226
227 /* If we've made it this far, we have a target that is being tracked, *
228 * and it is the correct target. So, copy this into the *
229 * lastTrackedData array.
230 for (i = 0; i < trackerPtr->reportLength; i++)
231 lastTrackedData[i] = buffer[bufIndex + i];
232 */
233 return (1);
234}
235
236
237/*****************************************************************************
238 *
239 decode_record - decodes a continuous binary encoded record
240 for one station.
241
242 output:
243 decodedMatrix- represents the row_matrix to transform the receiver
244 coordinates to tracker coordinates
245
246 - T_ERROR is returned if the record is munged, T_OK otherwise
247 *
248 *****************************************************************************/
249
250int vrpn_Tracker_Dyna::decode_record()
251// q_vec_type decoded_pos;
252{
253 unsigned i;
254
255 unsigned char exp;
256 char x_high, y_high, z_high;
257 unsigned char x_low, y_low, z_low;
258 long x, y, z;
259
260 if ( (buffer[0] & lOOO_OOOO) == 0 )
261 {
262 fprintf(stderr, "bogus data to vrpn_Tracker_Dyna_decode_record:\n");
263 for ( i = 0; i < reportLength; i++ )
264 fprintf(stderr, "%x ", buffer[i]);
265 fprintf(stderr, "\n");
266 return(T_ERROR);
267 }
268
269 /* Pull out the exponent to shift by */
270 //exp = ( (buffer[0] & OOOO_OOlO)<<1 ) | (buffer[0] & OOOO_OOOl);
271 exp = (unsigned char)( ( (buffer[0] & 0x2)<<1 ) | (buffer[0] & 0x1) );
272
273 x_high = (char)buffer[2];
274 x_low = (char)buffer[3];
275 y_high = (char)buffer[4];
276 y_low = (char)buffer[5];
277 z_high = (char)buffer[6];
278 z_low = (char)buffer[7];
279
280 /* Shift high order byte, combine with low order, and then *
281 * shift by the exponent. */
282 x = (long)( ((short)x_high<<8) | ((short)x_low) ) << exp;
283 y = (long)( ((short)y_high<<8) | ((short)y_low) ) << exp;
284 z = (long)( ((short)z_high<<8) | ((short)z_low) ) << exp;
285 //fprintf(stderr,"x,y,z: %i,%i,%i\n",x,y,z);
286
287 /* Convert to meters -- 1 unit = 0.05mm = 0.00005m */
288 pos[0] = (double)x * 0.00005;
289 pos[1] = (double)y * 0.00005;
290 pos[2] = (double)z * 0.00005;
291
292 /* For the single-target Dynasight, we assume that the orientation *
293 * the target is that same as that of the Dynasight. */
294 d_quat[0] = 0.0;
295 d_quat[1] = 0.0;
296 d_quat[2] = 0.0;
297 d_quat[3] = 1.0;
298
299 return(T_OK);
300
301} /* t_pdyn_decode_record */
Generic connection class not specific to the transport mechanism.
virtual int get_report(void)
Gets a report if one is available, returns 0 if not, 1 if complete report.
Definition vrpn_Dyna.C:149
virtual void reset()
Reset the tracker.
Definition vrpn_Dyna.C:103
vrpn_Tracker_Dyna(char *name, vrpn_Connection *c, int cSensors=1, const char *port="/dev/ttyd3", long baud=38400)
Definition vrpn_Dyna.C:30
virtual ~vrpn_Tracker_Dyna()
Definition vrpn_Dyna.C:43
unsigned char buffer[VRPN_TRACKER_BUF_SIZE]
vrpn_Tracker_Serial(const char *name, vrpn_Connection *c, const char *port="/dev/ttyS1", long baud=38400)
vrpn_float64 d_quat[4]
vrpn_int32 d_sensor
vrpn_float64 pos[3]
struct timeval timestamp
#define VRPN_API
#define T_PDYN_NO_DATA
Definition vrpn_Dyna.C:23
#define lOOO_OOOO
Definition vrpn_Dyna.C:25
#define OOOO_OOll
Definition vrpn_Dyna.C:27
#define T_OK
Definition vrpn_Dyna.C:19
#define T_ERROR
Definition vrpn_Dyna.C:18
#define llll_OOOO
Definition vrpn_Dyna.C:26
#define T_PDYN_SPEW_MODE
Definition vrpn_Dyna.C:22
#define OOOO_llOO
Definition vrpn_Dyna.C:28
#define VRPN_DYNA_MAX_SENSORS
Definition vrpn_Dyna.h:11
class VRPN_API vrpn_Connection
Definition vrpn_Dyna.h:8
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
int vrpn_drain_output_buffer(int comm)
Wait until all of the characters in the output buffer are sent, then return.
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_RESETTING
const int vrpn_TRACKER_SYNCING
const int vrpn_TRACKER_PARTIAL