vrpn  07.33
Virtual Reality Peripheral Network
vrpn_Tracker_DTrack.C
Go to the documentation of this file.
1 // vrpn_Tracker_DTrack.C
2 //
3 // Advanced Realtime Tracking GmbH's (http://www.ar-tracking.de) DTrack/DTrack2 client
4 
5 // developed by David Nahon for Virtools VR Pack (http://www.virtools.com)
6 // (07/20/2004) improved by Advanced Realtime Tracking GmbH (http://www.ar-tracking.de)
7 // (07/02/2007, 06/29/2009) upgraded by Advanced Realtime Tracking GmbH to support new devices
8 // (08/25/2010) a correction added by Advanced Realtime Tracking GmbH
9 // (12/01/2010) support of 3dof objects added by Advanced Realtime Tracking GmbH
10 //
11 // Recommended settings within DTrack's 'Settings / Network' or DTrack2's 'Settings / Output' dialog:
12 // 'ts', '6d', '6df' or '6df2', '6dcal', '3d' (optional)
13 
14 /* Configuration file:
15 
16 ################################################################################
17 # Advanced Realtime Tracking GmbH (http://www.ar-tracking.de) DTrack/DTrack2 client
18 #
19 # creates as many vrpn_Tracker as there are bodies or Flysticks, starting with the bodies
20 # creates 2 analogs per Flystick
21 # creates 8 buttons per Flystick
22 #
23 # NOTE: when using DTrack's older output format for Flystick data ('6df'), the numbering
24 # of Flystick buttons differs from DTrack documentation (for compatibility with
25 # older vrpn releases)
26 #
27 # Arguments:
28 # char name_of_this_device[]
29 # int udp_port (DTrack sends data to this UDP port)
30 #
31 # Optional arguments:
32 # float time_to_reach_joy (in seconds; see below)
33 # int number_of_bodies, number_of_flysticks (fixed numbers of bodies and Flysticks)
34 # int renumbered_ids[] (vrpn_Tracker IDs of bodies and Flysticks)
35 # char "3d" (activates 3dof marker output if available;
36 # always last argument if "-" is not present)
37 # char "-" (activates tracing; always last argument)
38 #
39 # NOTE: time_to_reach_joy is the time needed to reach the maximum value (1.0 or -1.0) of the
40 # joystick of older 'Flystick' devices when the corresponding button is pressed
41 # (one of the last buttons amongst the 8); not necessary for newer 'Flystick2' devices
42 # with its analog joystick
43 #
44 # NOTE: if fixed numbers of bodies and Flysticks should be used, both arguments
45 # number_of_bodies and number_of_flysticks have to be set
46 #
47 # NOTE: renumbering of tracker IDs is only possible, if fixed numbers of bodies and
48 # Flysticks are set; there has to be an argument present for each body/Flystick
49 
50 #vrpn_Tracker_DTrack DTrack 5000
51 #vrpn_Tracker_DTrack DTrack 5000 -
52 #vrpn_Tracker_DTrack DTrack 5000 3d
53 #vrpn_Tracker_DTrack DTrack 5000 3d -
54 #vrpn_Tracker_DTrack DTrack 5000 0.5
55 #vrpn_Tracker_DTrack DTrack 5000 0.5 2 2
56 #vrpn_Tracker_DTrack DTrack 5000 0.5 2 2 2 1 0 3
57 #vrpn_Tracker_DTrack DTrack 5000 0.5 2 2 2 1 0 3 3d -
58 
59 ################################################################################
60 */
61 
62 // usually the following should work:
63 
64 #ifndef _WIN32
65  #define OS_UNIX // for Unix (Linux, Irix, ...)
66 #else
67  #define OS_WIN // for MS Windows (2000, XP, ...)
68 #endif
69 
70 #include <stdio.h> // for NULL, printf, fprintf, etc
71 #include <stdlib.h> // for strtod, exit, free, malloc, etc
72 #include <string.h> // for strncmp, memset, strcat, etc
73 
74 #include "quat.h" // for Q_RAD_TO_DEG, etc
75 #include "vrpn_Connection.h" // for vrpn_CONNECTION_LOW_LATENCY, etc
76 #include "vrpn_Shared.h" // for timeval, INVALID_SOCKET, etc
77 #include "vrpn_Tracker_DTrack.h"
78 #include "vrpn_Types.h" // for vrpn_float64
79 #include "vrpn_MessageMacros.h" // for VRPN_MSG_INFO, VRPN_MSG_WARNING, VRPN_MSG_ERROR
80 
81 #ifdef OS_WIN
82  #ifdef VRPN_USE_WINSOCK2
83  #include <winsock2.h> // struct timeval is defined here
84  #else
85  #include <winsock.h> // struct timeval is defined here
86  #endif
87 #endif
88 #ifdef OS_UNIX
89  #include <netinet/in.h> // for sockaddr_in, INADDR_ANY, etc
90  #include <sys/socket.h> // for bind, recv, socket, AF_INET, etc
91  #include <unistd.h> // for close
92  #include <sys/select.h> // for select, FD_SET, FD_SETSIZE, etc
93 #endif
94 
95 
96 // There is a problem with linking on SGIs related to standard libraries.
97 #ifndef sgi
98 
99 // --------------------------------------------------------------------------
100 // Globals:
101 
102 #define DTRACK2VRPN_BUTTONS_PER_FLYSTICK 8 // number of vrpn buttons per Flystick (fixed)
103 #define DTRACK2VRPN_ANALOGS_PER_FLYSTICK 2 // number of vrpn analogs per Flystick (fixed)
104 
105 #define UDPRECEIVE_BUFSIZE 20000 // size of udp buffer for DTrack data (one frame; in bytes)
106 
107 // --------------------------------------------------------------------------
108 
109 // Local error codes:
110 
111 #define DTRACK_ERR_NONE 0 // no error
112 #define DTRACK_ERR_TIMEOUT 1 // timeout while receiving data
113 #define DTRACK_ERR_UDP 2 // UDP receive error
114 #define DTRACK_ERR_PARSE 3 // error in UDP packet
115 
116 // Local prototypes:
117 
118 static char* string_nextline(char* str, char* start, int len);
119 static char* string_get_i(char* str, int* i);
120 static char* string_get_ui(char* str, unsigned int* ui);
121 static char* string_get_d(char* str, double* d);
122 static char* string_get_f(char* str, float* f);
123 static char* string_get_block(char* str, const char* fmt, int* idat, float* fdat);
124 
125 static vrpn_Tracker_DTrack::socket_type udp_init(unsigned short port);
126 static int udp_exit(vrpn_Tracker_DTrack::socket_type sock);
127 static int udp_receive(vrpn_Tracker_DTrack::socket_type sock, void *buffer, int maxlen, int tout_us);
128 
129 
130 // --------------------------------------------------------------------------
131 // Constructor:
132 // name (i): device name
133 // c (i): vrpn_Connection
134 // dtrackPort (i): DTrack UDP port
135 // timeToReachJoy (i): time needed to reach the maximum value of the joystick
136 // fixNbody, fixNflystick (i): fixed numbers of DTrack bodies and Flysticks (-1 if not wanted)
137 // fixId (i): renumbering of targets; must have exactly (fixNbody + fixNflystick) elements (NULL if not wanted)
138 // actTracing (i): activate trace output
139 
141  int dtrackPort, float timeToReachJoy,
142  int fixNbody, int fixNflystick, int* fixId,
143  bool act3DOFout, bool actTracing) :
144  vrpn_Tracker(name, c),
145  vrpn_Button_Filter(name, c),
146  vrpn_Analog(name, c)
147 {
148 
149  int i;
150 
151  // Dunno how many of these there are yet...
152 
153  num_sensors = 0;
154  num_channel = 0;
155  num_buttons = 0;
156 
157  // init variables: general
158  output_3dof_marker = act3DOFout;
159  tracing = actTracing;
160  tracing_frames = 0;
161 
162  tim_first.tv_sec = tim_first.tv_usec = 0; // also used to recognize the first frame
163  tim_last.tv_sec = tim_last.tv_usec = 0;
164 
165  // init variables: DTrack data
166 
167  if(fixNbody >= 0 && fixNflystick >= 0){ // fixed numbers of bodies and Flysticks should be used
168  use_fix_numbering = true;
169 
170  fix_nbody = fixNbody;
171  fix_nflystick = fixNflystick;
172 
173  fix_idbody.resize(fix_nbody + fix_nflystick);
174  fix_idflystick.resize(fix_nflystick);
175 
176  if(fixId){ // take the renumbering information for bodies and Flysticks
177  for(i=0; i<fix_nbody + fix_nflystick; i++){ // take the renumbering information for bodies
178  fix_idbody[i] = fixId[i];
179  }
180 
181  for(i=0; i<fix_nflystick; i++){ // take the renumbering information for Flysticks (vrpn button data)
182  fix_idflystick[i] = fixId[i + fix_nbody];
183  }
184 
185  for(i=0; i<fix_nbody; i++){ // remove all bodies from the Flystick renumbering data
186  for(int j=0; j<fix_nflystick; j++){
187  if(fixId[i] < fixId[j + fix_nbody] && fix_idflystick[j] > 0){ // be sure to avoid crazy numbers...
188  fix_idflystick[j]--;
189  }
190  }
191  }
192  }else{ // take identity numbering for bodies and Flysticks
193  for(i=0; i<fix_nbody + fix_nflystick; i++){
194  fix_idbody[i] = i;
195  }
196  for(i=0; i<fix_nflystick; i++){
197  fix_idflystick[i] = i;
198  }
199  }
200  }else{ // no fixed numbers
201  use_fix_numbering = false;
202  }
203 
204  warning_nbodycal = false;
205 
206  // init variables: preparing data for VRPN
207 
208  if(timeToReachJoy > 1e-20){
209  joy_incPerSec = 1.f / timeToReachJoy; // increase of 'joystick' channel
210  }else{
211  joy_incPerSec = 1e20f; // so it reaches immediately
212  }
213 
214  // init: communicating with DTrack
215 
216  if(!dtrack_init(dtrackPort)){
217  exit(EXIT_FAILURE);
218  }
219 }
220 
221 // Destructor:
222 
224 {
225 
226  dtrack_exit();
227 }
228 
229 
230 // --------------------------------------------------------------------------
231 // Main loop:
232 
233 // This function should be called each time through the main loop
234 // of the server code. It checks for a report from the tracker and
235 // sends it if there is one.
236 
238 {
239  struct timeval timestamp;
240  long tts, ttu;
241  float dt;
242  int nbody, nflystick, i;
243  int newid;
244 
245  // call the generic server mainloop, since we are a server:
246 
247  server_mainloop();
248 
249  // get data from DTrack:
250 
251  if(!dtrack_receive()){
252  if(d_lasterror != DTRACK_ERR_TIMEOUT){
253  fprintf(stderr, "vrpn_Tracker_DTrack: Receive Error from DTrack.\n");
254  }
255  return;
256  }
257 
258  tracing_frames++;
259 
260  // get time stamp:
261 
263 
264  if(act_timestamp >= 0){ // use DTrack time stamp if available
265  tts = (long )act_timestamp;
266  ttu = (long )((act_timestamp - tts) * 1000000);
267  tts += timestamp.tv_sec - timestamp.tv_sec % 86400; // add day part of vrpn time stamp
268 
269  if(tts >= timestamp.tv_sec + 43200 - 1800){ // shift closer to vrpn time stamp
270  tts -= 86400;
271  }else if(tts <= timestamp.tv_sec - 43200 - 1800){
272  tts += 86400;
273  }
274 
275  timestamp.tv_sec = tts;
276  timestamp.tv_usec = ttu;
277  }
278 
279  if(tim_first.tv_sec == 0 && tim_first.tv_usec == 0){
280  tim_first = tim_last = timestamp;
281  }
282 
283  dt = (float )vrpn_TimevalDurationSeconds(timestamp, tim_last);
284  tim_last = timestamp;
285 
286  if(tracing && ((tracing_frames % 10) == 0)){
287  printf("framenr %u time %.3lf\n", act_framecounter, vrpn_TimevalDurationSeconds(timestamp, tim_first));
288  }
289 
290  // find number of targets visible for vrpn to choose the correct vrpn ID numbers:
291  // (1) takes fixed number of bodies and Flysticks, if defined in the configuration file
292  // (2) otherwise uses the '6dcal' line in DTrack's output, that gives the total number of
293  // calibrated targets, if available
294  // (3) otherwise tracks the maximum number of appeared targets
295 
296  if(use_fix_numbering){ // fixed numbers should be used
297  nbody = fix_nbody; // number of bodies visible for vrpn
298  nflystick = fix_nflystick; // number of Flysticks visible for vrpn
299  }else if(act_has_bodycal_format){ // DTrack/DTrack2 sent information about the number of calibrated targets
300  nbody = act_num_bodycal; // number of bodies visible for vrpn
301  nflystick = act_num_flystick; // number of Flysticks visible for vrpn
302  }else{ // else track the maximum number of appeared targets (at least)
303  if(!warning_nbodycal){ // mention warning (once)
304  fprintf(stderr, "vrpn_Tracker_DTrack warning: no DTrack '6dcal' data available.\n");
305  warning_nbodycal = true;
306  }
307 
308  nbody = act_num_body; // number of bodies visible for vrpn
309  nflystick = act_num_flystick; // number of Flysticks visible for vrpn
310  }
311 
312  // report tracker data to vrpn:
313 
314  num_sensors = nbody + nflystick; // total number of targets visible for vrpn
315  num_buttons = nflystick * DTRACK2VRPN_BUTTONS_PER_FLYSTICK; // 8 buttons per Flystick
316  num_channel = nflystick * DTRACK2VRPN_ANALOGS_PER_FLYSTICK; // 2 channels per joystick/Flystick
317 
318  for(i=0; i<act_num_body; i++){ // DTrack standard bodies
319  if(act_body[i].id < nbody){ // there might be more DTrack standard bodies than wanted
320  if(act_body[i].quality >= 0){ // report position only if body is tracked
321  if(use_fix_numbering){
322  newid = fix_idbody[act_body[i].id]; // renumbered ID
323  }else{
324  newid = act_body[i].id;
325  }
326 
327  dtrack2vrpn_body(newid, "", act_body[i].id, act_body[i].loc, act_body[i].rot, timestamp);
328  }
329  }
330  }
331 
332  if(num_channel >= static_cast<int>(joy_last.size())){ // adjust length of vector for current joystick value
333  size_t j0 = joy_last.size();
334 
335  joy_simulate.resize(num_channel);
336  joy_last.resize(num_channel);
337 
338  for(size_t j=j0; j< static_cast<size_t>(num_channel); j++){
339  joy_simulate[j] = false;
340  joy_last[j] = 0;
341  }
342  }
343 
344  for(i=0; i<(int)act_num_flystick; i++){ // DTrack Flysticks
345  if(act_flystick[i].id < nflystick){ // there might be more DTrack Flysticks than wanted
346  if(act_flystick[i].quality >= 0){ // report position only if Flystick is tracked
347  if(use_fix_numbering){
348  newid = fix_idbody[act_flystick[i].id + nbody]; // renumbered ID for position
349  }else{
350  newid = act_flystick[i].id + nbody;
351  }
352 
353  dtrack2vrpn_body(newid, "f", act_flystick[i].id, act_flystick[i].loc, act_flystick[i].rot,
354  timestamp);
355  }
356 
357  if(use_fix_numbering){
358  newid = fix_idflystick[act_flystick[i].id]; // renumbered ID for buttons and analogs
359  }else{
360  newid = act_flystick[i].id;
361  }
362 
363  dtrack2vrpn_flystickbuttons(newid, act_flystick[i].id,
364  act_flystick[i].num_button, act_flystick[i].button, timestamp);
365 
366  dtrack2vrpn_flystickanalogs(newid, act_flystick[i].id,
367  act_flystick[i].num_joystick, act_flystick[i].joystick, dt, timestamp);
368  }
369  }
370 
371  if (output_3dof_marker) {
372  int offset = num_sensors;
373  num_sensors += act_num_marker;
374  for(i=0; i<act_num_marker; i++){ // DTrack 3dof marker
375  dtrack2vrpn_marker(offset + i, "m", act_marker[i].id, act_marker[i].loc, timestamp);
376  }
377  }
378 
379  // finish main loop:
380 
381  vrpn_Analog::report_changes(); // report any analog event;
382  vrpn_Button::report_changes(); // report any button event;
383 }
384 
385 
386 // -----------------------------------------------------------------------------------------
387 // Helpers:
388 
389 
390 // ---------------------------------------------------------------------------------------------------
391 // ---------------------------------------------------------------------------------------------------
392 // Preparing data for VRPN:
393 // these functions convert DTrack data to vrpn data
394 
395 // Preparing marker data:
396 // id (i): VRPN body ID
397 // str_dtrack (i): DTrack marker name (just used for trace output)
398 // id_dtrack (i): DTrack marker ID (just used for trace output)
399 // loc (i): position
400 // timestamp (i): timestamp for body data
401 // return value (o): 0 ok, -1 error
402 
403 int vrpn_Tracker_DTrack::dtrack2vrpn_marker(int id, const char* str_dtrack, int id_dtrack,
404  const float* loc, struct timeval timestamp)
405 {
406 
407  d_sensor = id;
408 
409  // position (plus converting to unit meter):
410 
411  pos[0] = loc[0] / 1000.;
412  pos[1] = loc[1] / 1000.;
413  pos[2] = loc[2] / 1000.;
414 
415  // orientation: none
416 
417  q_make(d_quat, 1, 0, 0, 0);
418 
419  // pack and deliver tracker report:
420 
421  if(d_connection){
422  char msgbuf[1000];
423  int len = vrpn_Tracker::encode_to(msgbuf);
424 
427  {
428  fprintf(stderr, "vrpn_Tracker_DTrack: cannot write message: tossing.\n");
429  }
430  }
431 
432  // tracing:
433 
434  if(tracing && ((tracing_frames % 10) == 0)){
435 
436  printf("marker id (DTrack vrpn): %s%d %d pos (x y z): %.4f %.4f %.4f\n",
437  str_dtrack, id_dtrack, id, pos[0], pos[1], pos[2]);
438  }
439 
440  return 0;
441 }
442 
443 
444 // Preparing body data:
445 // id (i): VRPN body ID
446 // str_dtrack (i): DTrack body name ('body' or 'Flystick'; just used for trace output)
447 // id_dtrack (i): DTrack body ID (just used for trace output)
448 // loc (i): position
449 // rot (i): orientation (3x3 rotation matrix)
450 // timestamp (i): timestamp for body data
451 // return value (o): 0 ok, -1 error
452 
453 int vrpn_Tracker_DTrack::dtrack2vrpn_body(int id, const char* str_dtrack, int id_dtrack,
454  const float* loc, const float* rot, struct timeval timestamp)
455 {
456 
457  d_sensor = id;
458 
459  // position (plus converting to unit meter):
460 
461  pos[0] = loc[0] / 1000.;
462  pos[1] = loc[1] / 1000.;
463  pos[2] = loc[2] / 1000.;
464 
465  // orientation:
466 
467  q_matrix_type destMatrix; // if this is not good, just build the matrix and do a matrixToQuat
468 
469  destMatrix[0][0] = rot[0];
470  destMatrix[0][1] = rot[1];
471  destMatrix[0][2] = rot[2];
472  destMatrix[0][3] = 0.0;
473 
474  destMatrix[1][0] = rot[3];
475  destMatrix[1][1] = rot[4];
476  destMatrix[1][2] = rot[5];
477  destMatrix[1][3] = 0.0;
478 
479  destMatrix[2][0] = rot[6];
480  destMatrix[2][1] = rot[7];
481  destMatrix[2][2] = rot[8];
482  destMatrix[2][3] = 0.0;
483 
484  destMatrix[3][0] = 0.0;
485  destMatrix[3][1] = 0.0;
486  destMatrix[3][2] = 0.0;
487  destMatrix[3][3] = 1.0;
488 
489  q_from_row_matrix(d_quat, destMatrix);
490 
491  // pack and deliver tracker report:
492 
493  if(d_connection){
494  char msgbuf[1000];
495  int len = vrpn_Tracker::encode_to(msgbuf);
496 
499  {
500  fprintf(stderr, "vrpn_Tracker_DTrack: cannot write message: tossing.\n");
501  }
502  }
503 
504  // tracing:
505 
506  if(tracing && ((tracing_frames % 10) == 0)){
507  q_vec_type yawPitchRoll;
508 
509  q_to_euler(yawPitchRoll, d_quat);
510 
511  printf("body id (DTrack vrpn): %s%d %d pos (x y z): %.4f %.4f %.4f euler (y p r): %.3f %.3f %.3f\n",
512  str_dtrack, id_dtrack, id, pos[0], pos[1], pos[2],
513  Q_RAD_TO_DEG(yawPitchRoll[0]), Q_RAD_TO_DEG(yawPitchRoll[1]), Q_RAD_TO_DEG(yawPitchRoll[2]));
514  }
515 
516  return 0;
517 }
518 
519 
520 // Preparing Flystick button data:
521 // id (i): VRPN Flystick ID
522 // id_dtrack (i): DTrack Flystick ID (just used for trace output)
523 // num_but (i): number of buttons
524 // but (i): button state (1 pressed, 0 not pressed)
525 // timestamp (i): timestamp for button data
526 // return value (o): 0 ok, -1 error
527 
528 int vrpn_Tracker_DTrack::dtrack2vrpn_flystickbuttons(int id, int id_dtrack,
529  int num_but, const int* but, struct timeval timestamp)
530 {
531  int n, i, ind;
532 
534 
535  // buttons:
536  // NOTE: numbering of two buttons (B2 and B4) differs from DTrack documentation, as long as the 'older'
537  // output format '6df' is used by DTrack!
538 
540  i = 0;
541  while(i < n){
542  buttons[ind++] = but[i];
543  i++;
544  }
545  while(i < DTRACK2VRPN_BUTTONS_PER_FLYSTICK){ // fill remaining buttons
546  buttons[ind++] = 0;
547  i++;
548  }
549 
550  if(act_has_old_flystick_format){ // for backward compatibility!
551  // NOTE: numbering of two buttons (button 2 and button 4) differs from DTrack documentation!
552  buttons[id * DTRACK2VRPN_BUTTONS_PER_FLYSTICK + 1] = but[3];
553  buttons[id * DTRACK2VRPN_BUTTONS_PER_FLYSTICK + 3] = but[1];
554  }
555 
556  vrpn_Button::timestamp = timestamp; // timestamp for button event (explicitly necessary)
557 
558  // tracing:
559 
560  if(tracing && ((tracing_frames % 10) == 0)){
561  printf("flystick id (DTrack vrpn): f%d %d but ", id_dtrack, id);
562  for(i=0; i<n; i++){
563  printf(" %d", but[i]);
564  }
565  printf("\n");
566  }
567 
568  return 0;
569 }
570 
571 
572 // Preparing Flystick analog data:
573 // id (i): VRPN Flystick ID
574 // id_dtrack (i): DTrack Flystick ID (just used for trace output)
575 // num_ana (i): number of analogs
576 // ana (i): analog state (-1 <= ana <= 1)
577 // dt (i): time since last change
578 // timestamp (i): timestamp for analog data
579 // return value (o): 0 ok, -1 error
580 
581 int vrpn_Tracker_DTrack::dtrack2vrpn_flystickanalogs(int id, int id_dtrack,
582  int num_ana, const float* ana, float dt, struct timeval timestamp)
583 {
584  int n, i, ind;
585  float f;
586 
588 
589  // analogs:
590  // NOTE: simulation of time varying floating values, if a joystick action of an older
591  // 'Flystick' device is recognized!
592 
594  i = 0;
595  while(i < n){
596  f = ana[i];
597 
598  // simulation of time varying floating values (actually just necessary for older
599  // 'Flystick' devices and for backward compatibility):
600 
601  if(f == 0){ // zero position: reset everything
602  joy_simulate[ind] = false;
603  }else if((f > 0.99 || f < -0.99) && joy_last[ind] == 0){ // extreme change: start simulation
604  joy_simulate[ind] = true;
605  }
606 
607  if(joy_simulate[ind]){ // simulation of time varying floating values
608  if(f > 0){
609  f = joy_last[ind] + joy_incPerSec * dt;
610 
611  if(f >= 1){
612  f = 1;
613  joy_simulate[ind] = false;
614  }
615  }else{
616  f = joy_last[ind] - joy_incPerSec * dt;
617 
618  if(f <= -1){
619  f = -1;
620  joy_simulate[ind] = false;
621  }
622  }
623  }
624 
625  joy_last[ind] = f;
626  channel[ind++] = f;
627  i++;
628  }
629  while(i < DTRACK2VRPN_ANALOGS_PER_FLYSTICK){ // fill remaining analogs
630  channel[ind++] = 0;
631  i++;
632  }
633 
634  vrpn_Analog::timestamp = timestamp; // timestamp for analog event (explicitly necessary)
635 
636  // tracing:
637 
638  if(tracing && ((tracing_frames % 10) == 0)){
639  printf("flystick id (DTrack vrpn): f%d %d ana ", id_dtrack, id);
640  for(i=0; i<n; i++){
641  printf(" %.2f", ana[i]);
642  }
643  printf("\n");
644  }
645 
646  return 0;
647 }
648 
649 
650 // ---------------------------------------------------------------------------------------------------
651 // ---------------------------------------------------------------------------------------------------
652 // Communication with DTrack:
653 // these functions receive and parse data packets from DTrack
654 
655 // Initializing communication with DTrack:
656 //
657 // udpport (i): UDP port number to receive data from DTrack
658 //
659 // return value (o): initialization was successful (boolean)
660 
661 bool vrpn_Tracker_DTrack::dtrack_init(int udpport)
662 {
663 
664  d_udpbuf = NULL;
665  d_lasterror = DTRACK_ERR_NONE;
666 
667  // create UDP socket:
668 
669  if(udpport <= 0 || udpport > 65535){
670  fprintf(stderr, "vrpn_Tracker_DTrack: Illegal UDP port %d.\n", udpport);
671  return false;
672  }
673 
674  d_udpsock = udp_init((unsigned short )udpport);
675 
676  if(d_udpsock == INVALID_SOCKET){
677  fprintf(stderr, "vrpn_Tracker_DTrack: Cannot Initialize UDP Socket.\n");
678  return false;
679  }
680 
681  d_udptimeout_us = 0;
682 
683  // create UDP buffer:
684 
685  d_udpbufsize = UDPRECEIVE_BUFSIZE;
686 
687  d_udpbuf = (char *)malloc(d_udpbufsize);
688 
689  if(d_udpbuf == NULL){
690  udp_exit(d_udpsock);
691  d_udpsock = INVALID_SOCKET;
692  fprintf(stderr, "vrpn_Tracker_DTrack: Cannot Allocate Memory for UDP Buffer.\n");
693  return false;
694  }
695 
696  // reset actual DTrack data:
697 
698  act_framecounter = 0;
699  act_timestamp = -1;
700 
701  act_num_marker = act_num_body = act_num_flystick = 0;
702  act_has_bodycal_format = false;
703  act_has_old_flystick_format = false;
704 
705  return true;
706 }
707 
708 
709 // Deinitializing communication with DTrack:
710 //
711 // return value (o): deinitialization was successful (boolean)
712 
713 bool vrpn_Tracker_DTrack::dtrack_exit(void)
714 {
715 
716  // release buffer:
717 
718  if(d_udpbuf != NULL){
719  free(d_udpbuf);
720  }
721 
722  // release UDP socket:
723 
724  if(d_udpsock != INVALID_SOCKET){
725  udp_exit(d_udpsock);
726  }
727 
728  return true;
729 }
730 
731 
732 // ---------------------------------------------------------------------------------------------------
733 // Receive and process one DTrack data packet (UDP; ASCII protocol):
734 //
735 // return value (o): receiving was successful (boolean)
736 
737 bool vrpn_Tracker_DTrack::dtrack_receive(void)
738 {
739  char* s;
740  int i, j, k, l, n, len, id;
741  char sfmt[20];
742  int iarr[3];
743  float f;
744  int loc_num_bodycal, loc_num_flystick1, loc_num_meatool;
745 
746  if(d_udpsock == INVALID_SOCKET){
747  d_lasterror = DTRACK_ERR_UDP;
748  return false;
749  }
750 
751  // defaults:
752 
753  act_framecounter = 0;
754  act_timestamp = -1;
755 
756  loc_num_bodycal = -1; // i.e. not available
757  loc_num_flystick1 = loc_num_meatool = 0;
758 
759  act_has_bodycal_format = false;
760 
761  // receive UDP packet:
762 
763  len = udp_receive(d_udpsock, d_udpbuf, d_udpbufsize-1, d_udptimeout_us);
764 
765  if(len == -1){
766  d_lasterror = DTRACK_ERR_TIMEOUT;
767  return false;
768  }
769  if(len <= 0){
770  d_lasterror = DTRACK_ERR_UDP;
771  return false;
772  }
773 
774  s = d_udpbuf;
775  s[len] = '\0';
776 
777  // process lines:
778 
779  d_lasterror = DTRACK_ERR_PARSE;
780 
781  do{
782  // line for frame counter:
783 
784  if(!strncmp(s, "fr ", 3)){
785  s += 3;
786 
787  if(!(s = string_get_ui(s, &act_framecounter))){ // get frame counter
788  act_framecounter = 0;
789  return false;
790  }
791 
792  continue;
793  }
794 
795  // line for timestamp:
796 
797  if(!strncmp(s, "ts ", 3)){
798  s += 3;
799 
800  if(!(s = string_get_d(s, &act_timestamp))){ // get time stamp
801  act_timestamp = -1;
802  return false;
803  }
804 
805  continue;
806  }
807 
808  // line for additional information about number of calibrated bodies:
809 
810  if(!strncmp(s, "6dcal ", 6)){
811  s += 6;
812 
813  act_has_bodycal_format = true;
814 
815  if(!(s = string_get_i(s, &loc_num_bodycal))){ // get number of calibrated bodies
816  return false;
817  }
818 
819  continue;
820  }
821 
822  // line for 3dof marker data:
823 
824  if(!strncmp(s, "3d ", 3)){
825  s += 3;
826  act_num_marker = 0;
827 
828  if(!(s = string_get_i(s, &n))){ // get number of standard bodies (in line)
829  return false;
830  }
831 
832  if (static_cast<unsigned>(n) > act_marker.size()) {
833  act_marker.resize(n);
834  }
835 
836  for(i=0; i<n; i++){ // get data of standard bodies
837  if(!(s = string_get_block(s, "if", &id, &f))){
838  return false;
839  }
840 
841  act_marker[act_num_marker].id = id;
842 
843  if(!(s = string_get_block(s, "fff", NULL, act_marker[act_num_marker].loc))){
844  return false;
845  }
846 
847  act_num_marker++;
848  }
849 
850  continue;
851  }
852 
853  // line for standard body data:
854 
855  if(!strncmp(s, "6d ", 3)){
856  s += 3;
857 
858  for(i=0; i<act_num_body; i++){ // disable all existing data
859  memset(&act_body[i], 0, sizeof(vrpn_dtrack_body_type));
860  act_body[i].id = i;
861  act_body[i].quality = -1;
862  }
863 
864  if(!(s = string_get_i(s, &n))){ // get number of standard bodies (in line)
865  return false;
866  }
867 
868  for(i=0; i<n; i++){ // get data of standard bodies
869  if(!(s = string_get_block(s, "if", &id, &f))){
870  return false;
871  }
872 
873  if(id >= act_num_body){ // adjust length of vector
874  act_body.resize(id + 1);
875 
876  for(j=act_num_body; j<=id; j++){
877  memset(&act_body[j], 0, sizeof(vrpn_dtrack_body_type));
878  act_body[j].id = j;
879  act_body[j].quality = -1;
880  }
881 
882  act_num_body = id + 1;
883  }
884 
885  act_body[id].id = id;
886  act_body[id].quality = f;
887 
888  if(!(s = string_get_block(s, "fff", NULL, act_body[id].loc))){
889  return false;
890  }
891 
892  if(!(s = string_get_block(s, "fffffffff", NULL, act_body[id].rot))){
893  return false;
894  }
895  }
896 
897  continue;
898  }
899 
900  // line for Flystick data (older format):
901 
902  if(!strncmp(s, "6df ", 4)){
903  s += 4;
904 
905  act_has_old_flystick_format = true;
906 
907  if(!(s = string_get_i(s, &n))){ // get number of calibrated Flysticks
908  return false;
909  }
910 
911  loc_num_flystick1 = n;
912 
913  if(n > act_num_flystick){ // adjust length of vector
914  act_flystick.resize(n);
915 
916  act_num_flystick = n;
917  }
918 
919  for(i=0; i<n; i++){ // get data of Flysticks
920  if(!(s = string_get_block(s, "ifi", iarr, &f))){
921  return false;
922  }
923 
924  if(iarr[0] != i){ // not expected
925  return false;
926  }
927 
928  act_flystick[i].id = iarr[0];
929  act_flystick[i].quality = f;
930 
931  act_flystick[i].num_button = 8;
932  k = iarr[1];
933  for(j=0; j<8; j++){
934  act_flystick[i].button[j] = k & 0x01;
935  k >>= 1;
936  }
937 
938  act_flystick[i].num_joystick = 2; // additionally to buttons 5-8
939  if(iarr[1] & 0x20){
940  act_flystick[i].joystick[0] = -1;
941  }else if(iarr[1] & 0x80){
942  act_flystick[i].joystick[0] = 1;
943  }else{
944  act_flystick[i].joystick[0] = 0;
945  }
946  if(iarr[1] & 0x10){
947  act_flystick[i].joystick[1] = -1;
948  }else if(iarr[1] & 0x40){
949  act_flystick[i].joystick[1] = 1;
950  }else{
951  act_flystick[i].joystick[1] = 0;
952  }
953 
954  if(!(s = string_get_block(s, "fff", NULL, act_flystick[i].loc))){
955  return false;
956  }
957 
958  if(!(s = string_get_block(s, "fffffffff", NULL, act_flystick[i].rot))){
959  return false;
960  }
961  }
962 
963  continue;
964  }
965 
966  // line for Flystick data (newer format):
967 
968  if(!strncmp(s, "6df2 ", 5)){
969  s += 5;
970 
971  act_has_old_flystick_format = false;
972 
973  if(!(s = string_get_i(s, &n))){ // get number of calibrated Flysticks
974  return false;
975  }
976 
977  if(n > act_num_flystick){ // adjust length of vector
978  act_flystick.resize(n);
979 
980  act_num_flystick = n;
981  }
982 
983  if(!(s = string_get_i(s, &n))){ // get number of Flysticks
984  return false;
985  }
986 
987  for(i=0; i<n; i++){ // get data of Flysticks
988  if(!(s = string_get_block(s, "ifii", iarr, &f))){
989  return false;
990  }
991 
992  if(iarr[0] != i){ // not expected
993  return false;
994  }
995 
996  act_flystick[i].id = iarr[0];
997  act_flystick[i].quality = f;
998 
999  if(iarr[1] > vrpn_DTRACK_FLYSTICK_MAX_BUTTON){
1000  return false;
1001  }
1002  if(iarr[2] > vrpn_DTRACK_FLYSTICK_MAX_JOYSTICK){
1003  return false;
1004  }
1005  act_flystick[i].num_button = iarr[1];
1006  act_flystick[i].num_joystick = iarr[2];
1007 
1008  if(!(s = string_get_block(s, "fff", NULL, act_flystick[i].loc))){
1009  return false;
1010  }
1011 
1012  if(!(s = string_get_block(s, "fffffffff", NULL, act_flystick[i].rot))){
1013  return false;
1014  }
1015 
1016  strcpy(sfmt, "");
1017  j = 0;
1018  while(j < act_flystick[i].num_button){
1019  strcat(sfmt, "i");
1020  j += 32;
1021  }
1022  j = 0;
1023  while(j < act_flystick[i].num_joystick){
1024  strcat(sfmt, "f");
1025  j++;
1026  }
1027 
1028  if(!(s = string_get_block(s, sfmt, iarr, act_flystick[i].joystick))){
1029  return false;
1030  }
1031 
1032  k = l = 0;
1033  for(j=0; j<act_flystick[i].num_button; j++){
1034  act_flystick[i].button[j] = iarr[k] & 0x01;
1035  iarr[k] >>= 1;
1036 
1037  l++;
1038  if(l == 32){
1039  k++;
1040  l = 0;
1041  }
1042  }
1043  }
1044 
1045  continue;
1046  }
1047 
1048  // line for measurement tool data:
1049 
1050  if(!strncmp(s, "6dmt ", 5)){
1051  s += 5;
1052 
1053  if(!(s = string_get_i(s, &n))){ // get number of calibrated measurement tools
1054  return false;
1055  }
1056 
1057  loc_num_meatool = n;
1058 
1059  continue;
1060  }
1061 
1062  // ignore unknown line identifiers (could be valid in future DTracks)
1063 
1064  }while((s = string_nextline(d_udpbuf, s, d_udpbufsize)) != NULL);
1065 
1066  // set number of calibrated standard bodies, if necessary:
1067 
1068  if(loc_num_bodycal >= 0){ // '6dcal' information was available
1069  act_num_bodycal = loc_num_bodycal - loc_num_flystick1 - loc_num_meatool;
1070  }
1071 
1072  d_lasterror = DTRACK_ERR_NONE;
1073  return true;
1074 }
1075 
1076 
1077 // ---------------------------------------------------------------------------------------------------
1078 // ---------------------------------------------------------------------------------------------------
1079 // Parsing DTrack data:
1080 
1081 // Search next line in buffer:
1082 // str (i): buffer (total)
1083 // start (i): start position within buffer
1084 // len (i): buffer length in bytes
1085 // return (i): begin of line, NULL if no new line in buffer
1086 
1087 static char* string_nextline(char* str, char* start, int len)
1088 {
1089  char* s = start;
1090  char* se = str + len;
1091  int crlffound = 0;
1092 
1093  while(s < se){
1094  if(*s == '\r' || *s == '\n'){ // crlf
1095  crlffound = 1;
1096  }else{
1097  if(crlffound){ // begin of new line found
1098  return (*s) ? s : NULL; // first character is '\0': end of buffer
1099  }
1100  }
1101 
1102  s++;
1103  }
1104 
1105  return NULL; // no new line found in buffer
1106 }
1107 
1108 
1109 // Read next 'int' value from string:
1110 // str (i): string
1111 // i (o): read value
1112 // return value (o): pointer behind read value in str; NULL in case of error
1113 
1114 static char* string_get_i(char* str, int* i)
1115 {
1116  char* s;
1117 
1118  *i = (int )strtol(str, &s, 0);
1119  return (s == str) ? NULL : s;
1120 }
1121 
1122 
1123 // Read next 'unsigned int' value from string:
1124 // str (i): string
1125 // ui (o): read value
1126 // return value (o): pointer behind read value in str; NULL in case of error
1127 
1128 static char* string_get_ui(char* str, unsigned int* ui)
1129 {
1130  char* s;
1131 
1132  *ui = (unsigned int )strtoul(str, &s, 0);
1133  return (s == str) ? NULL : s;
1134 }
1135 
1136 
1137 // Read next 'double' value from string:
1138 // str (i): string
1139 // d (o): read value
1140 // return value (o): pointer behind read value in str; NULL in case of error
1141 
1142 static char* string_get_d(char* str, double* d)
1143 {
1144  char* s;
1145 
1146  *d = strtod(str, &s);
1147  return (s == str) ? NULL : s;
1148 }
1149 
1150 
1151 // Read next 'float' value from string:
1152 // str (i): string
1153 // f (o): read value
1154 // return value (o): pointer behind read value in str; NULL in case of error
1155 
1156 static char* string_get_f(char* str, float* f)
1157 {
1158  char* s;
1159 
1160  *f = (float )strtod(str, &s); // strtof() only available in GNU-C
1161  return (s == str) ? NULL : s;
1162 }
1163 
1164 
1165 // Process next block '[...]' in string:
1166 // str (i): string
1167 // fmt (i): format string ('i' for 'int', 'f' for 'float')
1168 // idat (o): array for 'int' values (long enough due to fmt)
1169 // fdat (o): array for 'float' values (long enough due to fmt)
1170 // return value (o): pointer behind read value in str; NULL in case of error
1171 
1172 static char* string_get_block(char* str, const char* fmt, int* idat, float* fdat)
1173 {
1174  char* strend;
1175  int index_i, index_f;
1176 
1177  if((str = strchr(str, '[')) == NULL){ // search begin of block
1178  return NULL;
1179  }
1180  if((strend = strchr(str, ']')) == NULL){ // search end of block
1181  return NULL;
1182  }
1183 
1184  str++; // (temporarily) remove delimiters
1185  *strend = '\0';
1186 
1187  index_i = index_f = 0;
1188 
1189  while(*fmt){
1190  switch(*fmt++){
1191  case 'i':
1192  if((str = string_get_i(str, &idat[index_i++])) == NULL){
1193  *strend = ']';
1194  return NULL;
1195  }
1196  break;
1197 
1198  case 'f':
1199  if((str = string_get_f(str, &fdat[index_f++])) == NULL){
1200  *strend = ']';
1201  return NULL;
1202  }
1203  break;
1204 
1205  default: // unknown format character
1206  *strend = ']';
1207  return NULL;
1208  }
1209  }
1210 
1211  // ignore additional data inside the block
1212 
1213  *strend = ']';
1214  return strend + 1;
1215 }
1216 
1217 
1218 // ---------------------------------------------------------------------------------------------------
1219 // ---------------------------------------------------------------------------------------------------
1220 // Handling UDP data:
1221 
1222 #ifdef OS_UNIX
1223 #define INVALID_SOCKET -1
1224 #endif
1225 
1226 // Initialize UDP socket:
1227 // port (i): port number
1228 // return value (o): socket number, NULL if error
1229 
1230 static vrpn_Tracker_DTrack::socket_type udp_init(unsigned short port)
1231 {
1233  struct sockaddr_in name;
1234 
1235  // initialize socket dll (only Windows):
1236 
1237 #ifdef OS_WIN
1238  {
1239  WORD vreq;
1240  WSADATA wsa;
1241 
1242  vreq = MAKEWORD(2, 0);
1243 
1244  if(WSAStartup(vreq, &wsa) != 0){
1245  return NULL;
1246  }
1247  }
1248 #endif
1249 
1250  // create socket:
1251 
1252 #ifdef OS_UNIX
1253  {
1254  int usock;
1255 
1256  usock = socket(PF_INET, SOCK_DGRAM, 0);
1257 
1258  if(usock < 0){
1259  return INVALID_SOCKET;
1260  }
1261  sock = usock;
1262  }
1263 #endif
1264 #ifdef OS_WIN
1265  {
1266  SOCKET wsock;
1267 
1268  wsock = socket(PF_INET, SOCK_DGRAM, 0);
1269 
1270  if(wsock == INVALID_SOCKET){
1271  WSACleanup();
1272  return INVALID_SOCKET;
1273  }
1274 
1275  sock = wsock;
1276  }
1277 #endif
1278 
1279  // name socket:
1280 
1281  name.sin_family = AF_INET;
1282  name.sin_port = htons(port);
1283  name.sin_addr.s_addr = htonl(INADDR_ANY);
1284 
1285  if(bind(sock, (struct sockaddr *) &name, sizeof(name)) < 0){
1286  udp_exit(sock);
1287  return INVALID_SOCKET;
1288  }
1289 
1290  return sock;
1291 }
1292 
1293 
1294 // Deinitialize UDP socket:
1295 // sock (i): socket number
1296 // return value (o): 0 ok, -1 error
1297 
1298 static int udp_exit(vrpn_Tracker_DTrack::socket_type sock)
1299 {
1300  int err;
1301 
1302 #ifdef OS_UNIX
1303  err = close(sock);
1304 #endif
1305 #ifdef OS_WIN
1306  err = closesocket(sock);
1307  WSACleanup();
1308 #endif
1309 
1310  if(err < 0){
1311  return -1;
1312  }
1313 
1314  return 0;
1315 }
1316 
1317 
1318 // Receive UDP data:
1319 // - tries to receive packets, as long as data are available
1320 // sock (i): socket number
1321 // buffer (o): buffer for UDP data
1322 // maxlen (i): length of buffer
1323 // tout_us (i): timeout in us (micro sec)
1324 // return value (o): number of received bytes, <0 if error/timeout occurred
1325 
1326 // Don't tell us about the FD_SET causing a conditional expression to be constant
1327 #ifdef _WIN32
1328 #pragma warning ( disable : 4127 )
1329 #endif
1330 
1331 static int udp_receive(vrpn_Tracker_DTrack::socket_type sock, void *buffer, int maxlen, int tout_us)
1332 {
1333  int nbytes, err;
1334  fd_set set;
1335  struct timeval tout;
1336 
1337  // waiting for data:
1338 
1339  FD_ZERO(&set);
1340  FD_SET(sock, &set);
1341 
1342  tout.tv_sec = tout_us / 1000000;
1343  tout.tv_usec = tout_us % 1000000;
1344 
1345  switch((err = select(FD_SETSIZE, &set, NULL, NULL, &tout))){
1346  case 1:
1347  break; // data available
1348  case 0:
1349  return -1; // timeout
1350  default:
1351  return -2; // error
1352  }
1353 
1354  // receiving packet:
1355 
1356  while(1){
1357 
1358  // receive one packet:
1359 
1360  nbytes = recv(sock, (char *)buffer, maxlen, 0);
1361 
1362  if(nbytes < 0){ // receive error
1363  return -3;
1364  }
1365 
1366  // check, if more data available: if so, receive another packet
1367 
1368  FD_ZERO(&set);
1369  FD_SET(sock, &set);
1370 
1371  tout.tv_sec = 0; // timeout with value of zero, thus no waiting
1372  tout.tv_usec = 0;
1373 
1374  if(select(FD_SETSIZE, &set, NULL, NULL, &tout) != 1){
1375 
1376  // no more data available: check length of received packet and return
1377 
1378  if(nbytes >= maxlen){ // buffer overflow
1379  return -4;
1380  }
1381 
1382  return nbytes;
1383  }
1384  }
1385 }
1386 
1387 #endif
1388 
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
virtual void report_changes(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY, const struct timeval time=vrpn_ANALOG_NOW)
Send a report only if something has changed (for servers) Optionally, tell what time to stamp the val...
Definition: vrpn_Analog.C:71
#define INVALID_SOCKET
vrpn_int32 num_buttons
Definition: vrpn_Button.h:47
#define DTRACK_ERR_TIMEOUT
Header containing macros formerly duplicated in a lot of implementation files.
#define DTRACK_ERR_PARSE
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
vrpn_float64 channel[vrpn_CHANNEL_MAX]
Definition: vrpn_Analog.h:38
#define vrpn_DTRACK_FLYSTICK_MAX_BUTTON
Generic connection class not specific to the transport mechanism.
vrpn_int32 d_sensor
Definition: vrpn_Tracker.h:94
#define UDPRECEIVE_BUFSIZE
double vrpn_TimevalDurationSeconds(struct timeval endT, struct timeval startT)
Return the number of seconds between startT and endT as a floating-point value.
Definition: vrpn_Shared.C:135
virtual void report_changes(void)
Definition: vrpn_Button.C:422
vrpn_int32 num_channel
Definition: vrpn_Analog.h:40
vrpn_Connection * d_connection
Connection that this object talks to.
virtual int pack_message(vrpn_uint32 len, struct timeval time, vrpn_int32 type, vrpn_int32 sender, const char *buffer, vrpn_uint32 class_of_service)
Pack a message that will be sent the next time mainloop() is called. Turn off the RELIABLE flag if yo...
vrpn_Tracker_DTrack(const char *name, vrpn_Connection *c, int dtrackPort, float timeToReachJoy=0.f, int fixNbody=-1, int fixNflystick=-1, int *fixId=NULL, bool act3DOFout=false, bool actTracing=false)
vrpn_int32 num_sensors
Definition: vrpn_Tracker.h:114
virtual int encode_to(char *buf)
Definition: vrpn_Tracker.C:533
#define SOCKET
Definition: vrpn_Shared.h:46
#define vrpn_DTRACK_FLYSTICK_MAX_JOYSTICK
struct timeval timestamp
Definition: vrpn_Button.h:48
#define DTRACK_ERR_NONE
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
#define DTRACK2VRPN_BUTTONS_PER_FLYSTICK
All button servers should derive from this class, which provides the ability to turn any of the butto...
Definition: vrpn_Button.h:65
vrpn_int32 d_sender_id
Sender ID registered with the connection.
#define DTRACK2VRPN_ANALOGS_PER_FLYSTICK
virtual void mainloop()
This function should be called each time through the main loop of the server code. It checks for a report from the tracker and sends it if there is one.
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
#define DTRACK_ERR_UDP
unsigned char buttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:44
struct timeval timestamp
Definition: vrpn_Analog.h:41
vrpn_int32 position_m_id
Definition: vrpn_Tracker.h:80