vrpn  07.33
Virtual Reality Peripheral Network
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 ) :
33 vrpn_Tracker_Serial(name,c,port,baud), cSensors(cSensors), cResets(0)
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 
48 int 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, (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, (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;
151  if (status == vrpn_TRACKER_SYNCING) {
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  }
160  if (status == vrpn_TRACKER_PARTIAL) {
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 
188 int 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 
250 int 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 */
T_ERROR
#define T_ERROR
Definition: vrpn_Dyna.C:18
vrpn_Tracker::d_sensor
vrpn_int32 d_sensor
Definition: vrpn_Tracker.h:94
vrpn_Tracker_Dyna::~vrpn_Tracker_Dyna
virtual ~vrpn_Tracker_Dyna()
Definition: vrpn_Dyna.C:43
vrpn_Types.h
vrpn_Tracker_Serial::buffer
unsigned char buffer[VRPN_TRACKER_BUF_SIZE]
Definition: vrpn_Tracker.h:155
vrpn_drain_output_buffer
int vrpn_drain_output_buffer(int comm)
Wait until all of the characters in the output buffer are sent, then return.
Definition: vrpn_Serial.C:485
vrpn_Tracker_Serial::serial_fd
int serial_fd
Definition: vrpn_Tracker.h:153
vrpn_Tracker::d_quat
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
vrpn_Tracker::timestamp
struct timeval timestamp
Definition: vrpn_Tracker.h:100
vrpn_Serial.h
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
vrpn_Dyna.h
vrpn_SleepMsecs
void vrpn_SleepMsecs(double dMsecs)
Definition: vrpn_Shared.C:157
vrpn_Shared.h
vrpn_TRACKER_PARTIAL
const int vrpn_TRACKER_PARTIAL
Definition: vrpn_Tracker.h:38
OOOO_llOO
#define OOOO_llOO
Definition: vrpn_Dyna.C:28
lOOO_OOOO
#define lOOO_OOOO
Definition: vrpn_Dyna.C:25
OOOO_OOll
#define OOOO_OOll
Definition: vrpn_Dyna.C:27
vrpn_Connection
Generic connection class not specific to the transport mechanism.
Definition: vrpn_Connection.h:510
vrpn_TRACKER_FAIL
const int vrpn_TRACKER_FAIL
Definition: vrpn_Tracker.h:40
vrpn_TRACKER_RESETTING
const int vrpn_TRACKER_RESETTING
Definition: vrpn_Tracker.h:39
llll_OOOO
#define llll_OOOO
Definition: vrpn_Dyna.C:26
vrpn_gettimeofday
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
vrpn_Tracker::status
int status
Definition: vrpn_Tracker.h:129
vrpn_Tracker_Serial
Definition: vrpn_Tracker.h:144
vrpn_Tracker_Dyna::reset
virtual void reset()
Reset the tracker.
Definition: vrpn_Dyna.C:103
vrpn_read_available_characters
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
Definition: vrpn_Serial.C:512
vrpn_TRACKER_SYNCING
const int vrpn_TRACKER_SYNCING
Definition: vrpn_Tracker.h:35
vrpn_Tracker_Serial::bufcount
vrpn_uint32 bufcount
Definition: vrpn_Tracker.h:157
vrpn_Tracker_Dyna::vrpn_Tracker_Dyna
vrpn_Tracker_Dyna(char *name, vrpn_Connection *c, int cSensors=1, const char *port="/dev/ttyd3", long baud=38400)
Definition: vrpn_Dyna.C:30
vrpn_Tracker_Dyna::cSensors
int cSensors
Definition: vrpn_Dyna.h:50
vrpn_Tracker_Dyna::get_report
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
vrpn_write_characters
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
vrpn_Tracker::pos
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
T_PDYN_NO_DATA
#define T_PDYN_NO_DATA
Definition: vrpn_Dyna.C:23
T_PDYN_SPEW_MODE
#define T_PDYN_SPEW_MODE
Definition: vrpn_Dyna.C:22
VRPN_API
#define VRPN_API
Definition: vrpn_Configure.h:646
VRPN_DYNA_MAX_SENSORS
#define VRPN_DYNA_MAX_SENSORS
Definition: vrpn_Dyna.h:11
T_OK
#define T_OK
Definition: vrpn_Dyna.C:19