vrpn  07.33
Virtual Reality Peripheral Network
vrpn_Tracker_PDI.C
Go to the documentation of this file.
1 // $Header: /PDIvrpn.root/2.0.0/PDIVRPN/vrpn/vrpn_Tracker_G4.cpp 1 6/05/12 12:21p Ben $
2 #include "vrpn_Tracker_PDI.h"
3 
5 
6 #ifdef VRPN_USE_PDI
7 
8 using namespace std;
9 
10 #define CM_TO_METERS (1/100.0)
11 
12 // Constructor
13 vrpn_Tracker_G4::vrpn_Tracker_G4 (const char *name, vrpn_Connection *c, const char *filepath, vrpn_float64 Hz, const char *rcmd, vrpn_Tracker_G4_HubMap * pHMap) :
14  vrpn_Tracker(name, c), update_rate(Hz), m_pHMap(pHMap)
15  {
16  srcCalPath = LPCTSTR(filepath);
17  cmd = (char*)(rcmd); // extra commands - See SendCommand(char *scmd)
19  if(!(Initialize()))
20  {
21  cout<<"G4: Could not initialize\r\n";
23  }
24  else if (pHMap && !(InitDigIOBtns()))
25  {
26  cout<<"G4: Could not configure DigIO buttons\r\n";
28  }
29  else if(!(Connect()))
30  {
31  cout<<"G4: Could not connect\r\n";
32  isCont = false;
34  }
35  else if(!(SetupDevice()))
36  {
37  cout<<"G4: Could not setup device\r\n";
39  }
40  else if(!(StartCont())){
41  cout<<"G4: Failed to enter continuous mode\r\n";
43  }
44  else {
45  cout<<"G4: Initialization Complete\r\n";
47  }
48  }
49 
50 // Deconstructor
51 vrpn_Tracker_G4::~vrpn_Tracker_G4(void){
52  if(isCont)
53  StopCont();
54  Disconnect();
55 
56  if (m_pHMap)
57  {
58  delete m_pHMap;
59  }
60 }
61 
62 
63 // Called by the vrpn_Generic_Sever class in order to report its status.
64 void vrpn_Tracker_G4::mainloop()
65 {
66  struct timeval current_time;
67 
68  // Call the generic server mainloop routine, since this is a server
69  server_mainloop();
70 
71  // See if its time to generate a new report
72  vrpn_gettimeofday(&current_time, NULL);
73  if ( vrpn_TimevalDuration(current_time,timestamp) >= 1000000.0/update_rate) {
74  DisplayCont(current_time); // Sending a report is handled in ParseG4NativeFrame
75  }
76 }
77 
78  // NOTE: you need to be sure that if you are sending vrpn_float64 then
79 // the entire array needs to remain aligned to 8 byte boundaries
80 // (malloced data and static arrays are automatically alloced in
81 // this way). Assumes that there is enough room to store the
82 // entire message. Returns the number of characters sent.
83 int vrpn_Tracker_G4::encode_to(char *buf)
84 {
85  char *bufptr = buf;
86  int buflen = 1000;
87 
88  // Message includes: long sensor, long scrap, vrpn_float64 pos[3], vrpn_float64 quat[4]
89  // Byte order of each needs to be reversed to match network standard
90 
91  vrpn_buffer(&bufptr, &buflen, d_sensor);
92  vrpn_buffer(&bufptr, &buflen, d_sensor); // This is just to take up space to align
93 
94  vrpn_buffer(&bufptr, &buflen, pos[0]);
95  vrpn_buffer(&bufptr, &buflen, pos[1]);
96  vrpn_buffer(&bufptr, &buflen, pos[2]);
97 
98  vrpn_buffer(&bufptr, &buflen, quat[0]);
99  vrpn_buffer(&bufptr, &buflen, quat[1]);
100  vrpn_buffer(&bufptr, &buflen, quat[2]);
101  vrpn_buffer(&bufptr, &buflen, quat[3]);
102 
103  return 1000 - buflen;
104 }
105 
107 
108 // Initialize the device and some variables
109 BOOL vrpn_Tracker_G4::Initialize(VOID){
110 
111  hContEvent = NULL;
112  hwnd = NULL;
113  dwOverflowCount = 0;
114 
115  BOOL bRet = TRUE;
116 
117  pdiG4.Trace(TRUE, 5);
118 
119  bCnxReady = FALSE;
120  dwStationMap = 0;
121 
122  return bRet;
123 }
124 
125 
126 BOOL vrpn_Tracker_G4::InitDigIOBtns()
127 {
128  BOOL bRet = TRUE;
129 
130  if (m_pHMap)
131  {
132 
133  HUBMAP_ENTRY * pHub = m_pHMap->Begin();
134 
135  while (pHub)
136  {
137  if (pHub->nBtnCount)
138  {
139  pHub->pBtnSrv = new vrpn_Button_Server(pHub->BtnName, d_connection, pHub->nBtnCount );
140  if (pHub->pBtnSrv == NULL)
141  {
142  cout << "Cannot create button device " << pHub->BtnName << endl;
143  bRet = FALSE;
144  break;
145  }
146  }
147  pHub = pHub->Next();
148  }
149  }
150 
151  return bRet;
152 }
153 
154 // Connect to the G4 using the filepath provided in the config file.
155 BOOL vrpn_Tracker_G4::Connect(VOID)
156 {
157  if (!(pdiG4.CnxReady()))
158  {
159  if(pdiG4.ConnectG4(srcCalPath)){
160  cout<<"G4: Connected\r\n";
161  }
162  }
163  else{
164  cout<<"G4: Already Connected\r\n";
165  }
166 
167  return pdiG4.CnxReady();
168 }
169 
170 // End the connection to the G4
171 VOID vrpn_Tracker_G4::Disconnect(VOID)
172 {
173  string msg;
174  if (!(pdiG4.CnxReady()))
175  {
176  cout << "G4: Already disconnected\r\n";
177  }
178  else
179  {
180  pdiG4.Disconnect();
181  cout<< "G4: Disconnected\r\n";
182  }
183 
184 }
185 
186 // Set G4 to collect correct data, eg. cm and quaternions
187 BOOL vrpn_Tracker_G4::SetupDevice( VOID )
188 {
189  int i = 0;
190  pdiG4.GetStationMap( dwStationMap );
191  while(dwStationMap == 0 && i<30)
192  { // Make sure that the G4 can see its hubs and sensors
193  Sleep(50);// RF signals take a moment to work themselves out after connection
194  pdiG4.GetStationMap( dwStationMap );
195  i++;// Escape in the case that it can not find hubs 7 sensors
196  }
197 
198  OriUnits = E_PDI_ORI_QUATERNION;
199  pdiG4.SetPNOOriUnits( OriUnits );
200 
201  PosUnits = E_PDI_POS_METER; //ePDIposUnits(2);
202  pdiG4.SetPNOPosUnits( PosUnits );
203 
204  pdiG4.SetPnoBuffer( pMotionBuf, VRPN_PDI_BUFFER_SIZE );
205 
206  //pdiG4.StartPipeExport();
207  UpdateStationMap();
208 
209  if(strlen(cmd)>0)
210  {
211  bool end = true;
212  char *pch;
213  char *pcmd = cmd;
214  pch = strtok (pcmd,"\n");
215  while (pch != NULL)
216  {
217  pcmd += strlen(pch) + 1;
218  sendCommand(pch);
219  //printf ("%s\n",pch);
220  pch = strtok (pcmd, "\n");
221  }
222  }
223 
224  //CPDIbiterr cBE;
225  //pdiG4.GetBITErrs( cBE );
226 
227  //CHAR szc[100];
228  //LPTSTR sz = LPTSTR(szc);
229  //cBE.Parse( sz, 100 );
230 
231  //if (!(cBE.IsClear()))
232  //{
233  // pdiG4.ClearBITErrs();
234  //}
235 
236  UpdateStationMap();
237  if(dwStationMap != 0)
238  {
239  return TRUE;
240  }
241  return FALSE; // G4 has connected but cannot see any hubs or sensors
242 }
243 
244 // Updates the map of hubs and sensors
245 VOID vrpn_Tracker_G4::UpdateStationMap( VOID )
246 {
247  pdiG4.GetStationMap( dwStationMap );
248  printf("Set GetStationMap Result: %s\r\n", pdiG4.GetLastResultStr() );
249 }
250 
251 // Send additional commands to the G4
252 // Originally called during setup, but can be called anytime by client software
253 // since it is a public function.
254 void vrpn_Tracker_G4::sendCommand(char *scmd)
255 {
256  char command = scmd[0];
257  printf("G4: Received Command: %s\n",scmd);
258  switch(command)
259  {
260  case 'B':// Boresight
261  DoBoresightCmd(&scmd[1]);
262  break;
263  case 'X'://Pos Filter
264  case 'Y'://Att Filter
265  DoFilterCmd(scmd);
266  break;
267  case 'T'://Translation FOR
268  case 'R'://Rotation FOR
269  DoFORCmd(scmd);
270  break;
271  case 'I'://Increment,AutoIncrement
272  DoIncrementCmd(scmd);
273  break;
274  case 'N':
275  DoTipOffsetCmd(scmd);
276  break;
277  case 'U':// Set Position Units
278  //action = scmd[1] - 48;
279  //if(action<4)
280  //{
281  // PosUnits = ePDIposUnits(action);
282  // pdiG4.SetPNOPosUnits( PosUnits );
283 
284  //}
285  //else
286  //{
287  // printf("Improper Set Units Command\r\n");
288  //}
289  printf("\tIgnoring 'U' command: VRPN Position Units standard is Meters.\r\n");
290  break;
291  case 'O':// Set Orientation Units
292  //action = scmd[1] - 48;
293  //if(action<3)
294  //{
295  // OriUnits = ePDIoriUnits(action);
296  // pdiG4.SetPNOOriUnits(OriUnits);
297  //}
298  //else
299  //{
300  // printf("Improper Set Orientation Command\r\n");
301  //}
302  printf("\tIgnoring 'O' command: VRPN Orientation standard is Quaternion XYZW.\r\n");
303  break;
304  default:
305  printf("\tUnrecognized Command: %c\r\n", scmd[1]);
306  break;
307  }
308  Sleep(50);
309 }
310 #define CMD_ACTION_SET 1
311 #define CMD_ACTION_RESET 2
312 void vrpn_Tracker_G4::DoBoresightCmd(char *scmd)
313 {
314  PDI4vec fP;
315  PDIori threeVec = {0.0,0.0,0.0}; // default Euler
316  PDI4vec fourVec = {1.0,0.0,0.0,0.0}; // default Quaternion WXYZ for G4
317 
318  char delims[] = ",\n";
319  char eol[] = "\r\n";
320  char comma[] = ",";
321  char *pArgs = scmd;
322  //strcpy(pArgs, scmd);
323 
324  char *pAct=NULL;
325  char *pHub=NULL;
326  char *pSens=NULL;
327  char *pRef=NULL;
328  bool bValid = TRUE;
329 
330  int nParams=0;
331  int nAction;
332  int nHub;
333  int nSensor;
334 
335  pAct = strtok(pArgs,comma);
336  if (pAct == NULL)
337  {
338  bValid = FALSE;
339  }
340  else
341  {
342  nAction = pAct[0] - '0';
343  nParams++;
344  pHub = strtok(NULL,comma);
345  if (pHub == NULL )
346  {
347  bValid = FALSE;
348  }
349  else
350  {
351  nHub = (pHub[0] == '*') ? -1 : atoi(pHub);
352  nParams++;
353  pSens = strtok(NULL, delims);
354  if (pSens == NULL)
355  {
356  bValid = FALSE;
357  }
358  else
359  {
360  nSensor = (pSens[0] == '*') ? -1 : atoi(pSens);
361  nParams++;
362  pRef = strtok(NULL, eol);
363  if (pRef != NULL)
364  {
365  nParams += sscanf( pRef, "%f,%f,%f,%f", &fP[1],&fP[2],&fP[3],&fP[0] );
366  }
367  }
368  }
369  }
370 
371  if (!bValid)
372  {
373  printf("\tERROR: Invalid Boresight Command Syntax : B%s\r\n", scmd);
374  }
375  else
376  {
377  switch (nAction)
378  {
379  case (CMD_ACTION_SET):
380  if (nParams == 7)
381  {
382  pdiG4.SetSBoresight(nHub, nSensor, fP);
383  printf("\tSet Boresight Result: %s\r\n", pdiG4.GetLastResultStr() );
384  }
385  else if (nParams == 3)
386  {
387  pdiG4.SetSBoresight(nHub, nSensor, fourVec);
388  printf("\tSet Boresight Result: %s\r\n", pdiG4.GetLastResultStr() );
389  }
390  else
391  {
392  printf("\tERROR: Unexpected Boresight Argument count: %d\r\n", nParams);
393  }
394  break;
395  case (CMD_ACTION_RESET):
396  pdiG4.ResetSBoresight(nHub, nSensor);
397  printf("\tReset Boresight Result: %s\r\n", pdiG4.GetLastResultStr() );
398  break;
399  default:
400  printf("\tERROR: Unrecognized Boresight Action: %s\r\n", pAct);
401  break;
402  }
403  }
404 }
405 
406 
407 void vrpn_Tracker_G4::DoFilterCmd(char *scmd)
408 {
409  char delims[] = ",\n\r\\";
410  char eol[] = "\r\n";
411  char comma[] = ",";
412  char * pArgs = &scmd[1];
413  char cmd = scmd[0];
414 
415  char *pAct=NULL;
416  char *pHub=NULL;
417  char *pLev=NULL;
418  char *pCust=NULL;
419  bool bValid = TRUE;
420 
421  int nParams=0;
422  int nAction;
423  int nHub;
424  int nLev;
425  float F=0, FLow=0, FHigh=0, Factor=0;
426  CPDIfilter f;
427  char * pLevelName = 0;
428 
429  pAct = strtok(pArgs,comma);
430  if (pAct == NULL)
431  {
432  bValid = FALSE;
433  }
434  else
435  {
436  nAction = pAct[0] - '0';
437  nParams++;
438 
439  pHub = strtok(NULL,delims);
440  if (pHub == NULL )
441  {
442  bValid = FALSE;
443  }
444  else
445  {
446  nHub = (pHub[0] == '*') ? -1 : atoi(pHub);
447  nParams++;
448 
449  if (nAction == CMD_ACTION_SET)
450  {
451  pLev = strtok(NULL, delims);
452  if (pLev == NULL)
453  {
454  bValid = FALSE;
455  }
456  else
457  {
458  nLev = atoi(pLev);
459  nParams++;
460 
461  if (nLev == E_PDI_FILTER_CUSTOM)
462  {
463  pCust = strtok(NULL, eol);
464  if (pCust == NULL)
465  {
466  bValid = FALSE;
467  }
468  else
469  {
470  nParams += sscanf( pCust, "%f,%f,%f,%f", &f.m_fSensitivity,
471  &f.m_fLowValue,
472  &f.m_fHighValue,
473  &f.m_fMaxTransRate );
474  if (nParams != 7)
475  {
476  printf("\tERROR: Unexpected Filter Argument count: %d\r\n", nParams);
477  bValid = FALSE;
478  }
479  }
480  }
481  }
482  }
483  }
484  }
485 
486  if (!bValid)
487  {
488  printf("\tERROR: Invalid Filter Command Syntax: %s\r\n", scmd);
489  }
490  else
491  {
492  if (nLev == E_PDI_FILTER_NONE)
493  {
494  nAction = CMD_ACTION_RESET;
495  }
496 
497  switch (nAction)
498  {
499  case (CMD_ACTION_SET):
500  SetFilterPreset( nLev, f, &pLevelName);
501  if (cmd == 'X')
502  {
503  pdiG4.SetHPosFilter( nHub, f);
504  }
505  else
506  {
507  pdiG4.SetHAttFilter( nHub, f);
508  }
509  printf("\tSet Filter Cmd %c %s Result: %s\r\n", cmd, pLevelName, pdiG4.GetLastResultStr() );
510  break;
511 
512  case (CMD_ACTION_RESET):
513  if (cmd == 'X')
514  {
515  pdiG4.ResetHPosFilter( nHub );
516  }
517  else
518  {
519  pdiG4.ResetHAttFilter( nHub );
520  }
521  printf("\tReset Filter Cmd %c Result: %s\r\n", cmd, pdiG4.GetLastResultStr() );
522  break;
523 
524  default:
525  printf("\tERROR: Unrecognized Filter Action: %c\r\n", pAct);
526  break;
527  }
528 
529  }
530 }
531 
532 CPDIfilter FilterPresets[4] =
533 {
534  CPDIfilter(0.0f, 1.0f, 0.0f, 0.0f), // none
535  CPDIfilter(0.2f, 0.2f, 0.8f, 0.95f), // low,
536  CPDIfilter(0.05f, 0.05f, 0.8f, 0.95f), // med,
537  CPDIfilter(0.02f, 0.02f, 0.8f, 0.95f) // heavy
538 };
539 char * PresetNames[5] =
540 {
541  "NONE", "LIGHT", "MEDIUM", "HEAVY", "CUSTOM"
542 };
543 
544 void vrpn_Tracker_G4::SetFilterPreset( int nLev, CPDIfilter & f, char **pLevName )
545 {
546  switch (nLev)
547  {
548  case E_PDI_FILTER_NONE:
549  case E_PDI_FILTER_LIGHT:
550  case E_PDI_FILTER_MED:
551  case E_PDI_FILTER_HEAVY:
552  f = FilterPresets[nLev];
553  *pLevName = PresetNames[nLev];
554  break;
555  case E_PDI_FILTER_CUSTOM:
556  *pLevName = PresetNames[nLev];
557  break;
558  default:
559  f = FilterPresets[E_PDI_FILTER_HEAVY];
560  *pLevName = PresetNames[E_PDI_FILTER_HEAVY];
561  break;
562  }
563 }
564 
565 void vrpn_Tracker_G4::DoFORCmd( char *scmd )
566 {
567  char delims[] = ",\n\r\\";
568  char eol[] = "\r\n";
569  char comma[] = ",";
570  char * pArgs = &scmd[1];
571  char cmd = scmd[0];
572 
573  char *pAct=NULL;
574  char *pFOR=NULL;
575  bool bValid = TRUE;
576 
577  int nParams=0;
578  int nParamSpec=0;
579  int nAction;
580  PDIpos pos;
581  PDIqtrn qtrn;
582 
583  pAct = strtok(pArgs,comma);
584  if (pAct == NULL)
585  {
586  bValid = FALSE;
587  }
588  else
589  {
590  nAction = pAct[0] - '0';
591  nParams++;
592 
593  if (nAction == CMD_ACTION_SET)
594  {
595  pFOR = strtok( NULL, eol );
596  if (pFOR == NULL)
597  {
598  bValid = FALSE;
599  }
600  else if (cmd == 'T')
601  {
602  nParams += sscanf( pFOR, "%f,%f,%f", &pos[0], &pos[1], &pos[2] );
603  nParamSpec = 4;
604  }
605  else
606  {
607  nParams += sscanf( pFOR, "%f,%f,%f,%f", &qtrn[1], &qtrn[2], &qtrn[3], &qtrn[0] );
608  nParamSpec = 5;
609  }
610 
611  if (nParams != nParamSpec)
612  {
613  printf("\tERROR: Unexpected Frame of Reference %c Argument count: %d\r\n", cmd, nParams);
614  bValid = FALSE;
615  }
616  }
617  }
618 
619  if (!bValid)
620  {
621  printf("\tERROR: Invalid Frame of Reference Command Syntax: %s\r\n", scmd);
622  }
623  else
624  {
625  switch (nAction)
626  {
627  case (CMD_ACTION_SET):
628  if (cmd == 'T')
629  {
630  pdiG4.SetFrameOfRefTRANS( pos );
631  }
632  else
633  {
634  pdiG4.SetFrameOfRefROT( qtrn );
635  }
636  printf("\tSet Frame of Ref %c Result: %s\r\n", cmd, pdiG4.GetLastResultStr() );
637  break;
638 
639  case (CMD_ACTION_RESET):
640  if (cmd == 'T')
641  {
642  pdiG4.ResetFrameOfRefTRANS();
643  }
644  else
645  {
646  pdiG4.ResetFrameOfRefROT();
647  }
648  printf("\tReset Frams of Ref Cmd %c Result: %s\r\n", cmd, pdiG4.GetLastResultStr() );
649  break;
650 
651  default:
652  printf("\tERROR: Unrecognized Frame Of Reference Action: %c\r\n", pAct);
653  break;
654  }
655 
656  }
657 
658 }
659 
660 
661 
662 void vrpn_Tracker_G4::DoIncrementCmd(char *scmd)
663 {
664  char cmd = scmd[0];
665 
666  char delims[] = " ,\r\n\\";
667  char eol[] = "\r\n";
668  char comma[] = ",";
669  char *pArgs = &scmd[1];
670  //strcpy(pArgs, scmd);
671 
672  char *pAct=NULL;
673  char *pHub=NULL;
674  char *pSens=NULL;
675  char *pIncrs=NULL;
676  bool bValid = TRUE;
677 
678  int nParams=0;
679  int nParamSpec=5;
680  int nAction;
681  int nHub;
682  int nSensor;
683  float fPosIncr, fOriIncr;
684 
685 
686  pAct = strtok(pArgs,comma);
687  if (pAct == NULL)
688  {
689  bValid = FALSE;
690  }
691  else
692  {
693  nAction = pAct[0] - '0';
694  nParams++;
695  pHub = strtok(NULL,comma);
696  if (pHub == NULL )
697  {
698  bValid = FALSE;
699  }
700  else
701  {
702  nHub = (pHub[0] == '*') ? -1 : atoi(pHub);
703  nParams++;
704  pSens = strtok(NULL, delims);
705  if (pSens == NULL)
706  {
707  bValid = FALSE;
708  }
709  else
710  {
711  nSensor = (pSens[0] == '*') ? -1 : atoi(pSens);
712  nParams++;
713 
714  if (nAction == CMD_ACTION_SET)
715  {
716  pIncrs = strtok(NULL, eol);
717  if (pIncrs == NULL)
718  {
719  bValid = FALSE;
720  }
721  else
722  {
723  nParams += sscanf( pIncrs, "%f,%f", &fPosIncr, &fOriIncr );
724  }
725 
726  if (nParams != nParamSpec)
727  {
728  printf("\tERROR: Unexpected Increment Cmd Argument count: %d\r\n", nParams);
729  bValid = FALSE;
730  }
731  }
732  }
733  }
734  }
735 
736  if (!bValid)
737  {
738  printf("\tERROR: Invalid Increment Command Syntax : %s\r\n", scmd);
739  }
740  else
741  {
742  pdiG4.SetPNOOriUnits(E_PDI_ORI_EULER_DEGREE);
743  printf("\tSet Ori Units DEGREES Result: %s\r\n", pdiG4.GetLastResultStr() );
744  switch (nAction)
745  {
746  case (CMD_ACTION_SET):
747  pdiG4.SetSIncrement(nHub, nSensor, fPosIncr, fOriIncr);
748  printf("\tSet Increment Result: %s\r\n", pdiG4.GetLastResultStr() );
749  break;
750 
751  case (CMD_ACTION_RESET):
752  pdiG4.ResetSIncrement(nHub, nSensor);
753  printf("\tReset Increment Result: %s\r\n", pdiG4.GetLastResultStr() );
754  break;
755 
756  default:
757  printf("\tERROR: Unrecognized Increment Action: %s\r\n", pAct);
758  break;
759  }
760  pdiG4.SetPNOOriUnits(E_PDI_ORI_QUATERNION);
761  printf("\tSet Ori Units QUATERNION Result: %s\r\n", pdiG4.GetLastResultStr() );
762  }
763 }
764 
765 
766 
767 void vrpn_Tracker_G4::DoTipOffsetCmd(char *scmd)
768 {
769  char cmd = scmd[0];
770 
771  char delims[] = " ,\n\r\\";
772  char eol[] = "\r\n";
773  char comma[] = ",";
774  char *pArgs = &scmd[1];
775  //strcpy(pArgs, scmd);
776 
777  char *pAct=NULL;
778  char *pHub=NULL;
779  char *pSens=NULL;
780  char *pTO=NULL;
781  bool bValid = TRUE;
782 
783  int nParams=0;
784  int nParamSpec=6;
785  int nAction;
786  int nHub;
787  int nSensor;
788  PDIpos pos;
789 
790 
791  pAct = strtok(pArgs,comma);
792  if (pAct == NULL)
793  {
794  bValid = FALSE;
795  }
796  else
797  {
798  nAction = pAct[0] - '0';
799  nParams++;
800  pHub = strtok(NULL,comma);
801  if (pHub == NULL )
802  {
803  bValid = FALSE;
804  }
805  else
806  {
807  nHub = (pHub[0] == '*') ? -1 : atoi(pHub);
808  nParams++;
809  pSens = strtok(NULL, delims);
810  if (pSens == NULL)
811  {
812  bValid = FALSE;
813  }
814  else
815  {
816  nSensor = (pSens[0] == '*') ? -1 : atoi(pSens);
817  nParams++;
818 
819  if (nAction == CMD_ACTION_SET)
820  {
821  pTO = strtok(NULL, eol);
822  if (pTO == NULL)
823  {
824  bValid = FALSE;
825  }
826  else
827  {
828  nParams += sscanf( pTO, "%f,%f,%f", &pos[0], &pos[1], &pos[2] );
829  }
830 
831  if (nParams != nParamSpec)
832  {
833  printf("\tERROR: Unexpected Tip Offset Cmd Argument count: %d\r\n", nParams);
834  bValid = FALSE;
835  }
836  }
837  }
838  }
839  }
840 
841  if (!bValid)
842  {
843  printf("\tERROR: Invalid Tip Offset Command Syntax : %s\r\n", scmd);
844  }
845  else
846  {
847  switch (nAction)
848  {
849  case (CMD_ACTION_SET):
850  pdiG4.SetSTipOffset(nHub, nSensor, pos);
851  printf("\tSet Tip Offset Result: %s\r\n", pdiG4.GetLastResultStr() );
852  break;
853  case (CMD_ACTION_RESET):
854  pdiG4.ResetSTipOffset(nHub, nSensor);
855  printf("\tReset Tip Offset Result: %s\r\n", pdiG4.GetLastResultStr() );
856  break;
857  default:
858  printf("\tERROR: Unrecognized Tip Offset Action: %s\r\n", pAct);
859  break;
860  }
861  }
862 }
863 
864 
865 
866 
867 // Start Continuous Mode for the G4
868 BOOL vrpn_Tracker_G4::StartCont( VOID )
869 {
870  cout<<"G4: Start Continuous Mode\r\n";
871  BOOL bRet = FALSE;
872 
873 
874  if (!(pdiG4.StartContPnoG4(hwnd)))
875  {
876  }
877  else
878  {
879  dwOverflowCount = 0;
880  bRet = TRUE;
881  }
882 
883  isCont = true;
884  return bRet;
885 }
886 
887 // Stops Continuous Mode for the G4
888 BOOL vrpn_Tracker_G4::StopCont( VOID )
889 {
890  cout<<"G4: Stop Continuous Mode\r\n";
891  BOOL bRet = FALSE;
892 
893  if (!(pdiG4.StopContPnoG4()))
894  {
895  }
896  else
897  {
898  bRet = TRUE;
899  Sleep(1000);
900  }
901 
902  isCont = false;
903  ::ResetEvent(hContEvent);
904  return bRet;
905 }
906 
907 // Displays a frame of information taken from the continuous stream of
908 // Continuous Mode by calling ParseG4NativeFrame - not functioning now
909 BOOL vrpn_Tracker_G4::DisplayCont( timeval ct )
910 {
911  //cout<<"DisplayCont\r\n";
912  BOOL bRet = FALSE;
913 
914  PBYTE pBuf;
915  DWORD dwSize;
916  PBYTE pLastBuf = 0;
917  DWORD dwLastSize = 0;
918 
919 
920  if (!(pdiG4.LastPnoPtr(pBuf, dwSize)))
921  {
922  }
923  else if ((pBuf == 0) || (dwSize == 0))
924  {
925  }
926  else if (pLastBuf && (pBuf > pLastBuf))
927  {
928  ParseG4NativeFrame( pLastBuf+dwLastSize, dwSize+(pBuf-pLastBuf-dwLastSize), ct );
929  pLastBuf = pBuf;
930  dwLastSize = dwSize;
931  bRet = TRUE;
932  }
933  else if (pBuf != pLastBuf) // it wrapped in buffer
934  {
935  if (pLastBuf)
936  cout << "wrapped" << endl;
937 
938  pLastBuf = pBuf;
939  dwLastSize = dwSize;
940  ParseG4NativeFrame( pBuf, dwSize, ct );
941  bRet = TRUE;
942  }
943 
944  //cout<<"Leaving DisplayCont\r\n";
945  return bRet;
946 }
947 
948 // Displays a single frame of information from the G4 by collecting data
949 // and calling ParseG4NativeFrame
950 BOOL vrpn_Tracker_G4::DisplaySingle( timeval ct )
951 {
952  BOOL bRet = FALSE;
953  PBYTE pBuf;
954  DWORD dwSize;
955 
956  if (!(pdiG4.ReadSinglePnoBufG4(pBuf, dwSize)))
957  {
958  //cout<<"ReadSinglePno\r\n";
959  }
960  else if ((pBuf == 0) || (dwSize == 0))
961  {
962  }
963  else
964  {
965  ParseG4NativeFrame( pBuf, dwSize, ct );
966  bRet = TRUE;
967  }
968 
969  return bRet;
970 
971 }
972 
973 // Parses the data collected from a P or C command, packages it and sends it out to clients
974 // calling for it
975 void vrpn_Tracker_G4::ParseG4NativeFrame( PBYTE pBuf, DWORD dwSize, timeval current_time )
976 {
977  DWORD dw= 0;
978  char msgbuf[1000];
979  vrpn_int32 len;
980  LPG4_HUBDATA pHubFrame;
981 
982  while (dw < dwSize )
983  {
984  pHubFrame = (LPG4_HUBDATA)(&pBuf[dw]);
985 
986  dw += sizeof(G4_HUBDATA);
987 
988  UINT nHubID = pHubFrame->nHubID;
989  UINT nFrameNum = pHubFrame->nFrameCount;
990  UINT nSensorMap = pHubFrame->dwSensorMap;
991  UINT nDigIO = pHubFrame->dwDigIO;
992 
993  HUBMAP_ENTRY * pH = 0;
994  int nButtons = 0;
995  // handle digios if necessary
996  if (m_pHMap && (pH = m_pHMap->Find( nHubID )) && (nButtons = pH->nBtnCount) )
997  {
998  for (int i=0; i<nButtons; i++)
999  {
1000  pH->pBtnSrv->set_button(i, (nDigIO & (1<<i)) >> i);
1001  pH->pBtnSrv->mainloop();
1002  }
1003  }
1004 
1005  UINT nSensMask = 1;
1006 
1007  for (int j=0; j<G4_MAX_SENSORS_PER_HUB; j++)
1008  {
1009  if (((nSensMask << j) & nSensorMap) != 0)
1010  {
1011  G4_SENSORDATA * pSD = &(pHubFrame->sd[j]);
1012  d_sensor = (nHubID*10)+j;
1013  // transfer the data from the G4 data array to the vrpn_Tracker array
1014  pos[0] = pSD->pos[0];
1015  pos[1] = pSD->pos[1];
1016  pos[2] = pSD->pos[2];
1017 
1018  quat[0] = pSD->ori[1];
1019  quat[1] = pSD->ori[2];
1020  quat[2] = pSD->ori[3];
1021  quat[3] = pSD->ori[0];
1022 
1023  // Grab the current time and create a timestamp
1024  timestamp.tv_sec = current_time.tv_sec;
1025  timestamp.tv_usec = current_time.tv_usec;
1026  // check the connection and then send a message out along it.
1027  if (d_connection) {
1028  // Pack position report
1029  len = encode_to(msgbuf);
1030  d_connection->pack_message(len, timestamp,
1031  position_m_id, d_sender_id, msgbuf,
1033  }
1034  }
1035  }
1036 
1037 
1038 
1039  } // end while dwsize
1040 }
1041 
1042 // Constructor
1043 vrpn_Tracker_FastrakPDI::vrpn_Tracker_FastrakPDI (const char * name, vrpn_Connection *cn,
1044  vrpn_float64 Hz, const char * rcmd, unsigned int nStylusMap) :
1045  vrpn_Tracker(name, cn), update_rate(Hz)
1046  , m_nStylusMap(nStylusMap)
1047  , m_nHeaderSize(3)
1048  , m_nFrameSize(nStylusMap?33:31)
1049  {
1050  cmd = (char*)(rcmd);
1052  if(!(Initialize())){
1054  }
1055  else if (nStylusMap & !(InitStylusBtns()))
1056  {
1058  }
1059  else if(!(Connect())){
1061  }
1062  else if(!(SetupDevice())){
1064  cout << "FasTrakPDI: Device setup failed\r\n";
1065  }
1066  else if(!(StartCont())){
1067  isCont = FALSE;
1068  cout << "FastrakPDI: Failed to enter continuous mode\r\n";
1070  }
1071  else {
1072  cout << "FastrakPDI: Initialization Complete\r\n";
1074  }
1075  }
1076 
1077 // Deconstructor
1078 vrpn_Tracker_FastrakPDI::~vrpn_Tracker_FastrakPDI(void)
1079 {
1080  if(isCont)
1081  StopCont();
1082  Disconnect();
1083 
1084  for (int i=0; i<FT_MAX_SENSORS; i++)
1085  {
1086  if (FTstylusBtns[i])
1087  delete FTstylusBtns[i];
1088  }
1089  }
1090 
1091 // Called by the vrpn_Generic_Sever class in order to report its status.
1092 VOID vrpn_Tracker_FastrakPDI::mainloop()
1093 {
1094  struct timeval current_time;
1095 
1096  // Call the generic server mainloop routine, since this is a server
1097  server_mainloop();
1098 
1099  // See if its time to generate a new report
1100  vrpn_gettimeofday(&current_time, NULL);
1101  if ( vrpn_TimevalDuration(current_time,timestamp) >= 1000000.0/update_rate) {
1102  DisplayCont(current_time);
1103  }
1104 }
1105 
1106 // NOTE: you need to be sure that if you are sending vrpn_float64 then
1107 // the entire array needs to remain aligned to 8 byte boundaries
1108 // (malloced data and static arrays are automatically alloced in
1109 // this way). Assumes that there is enough room to store the
1110 // entire message. Returns the number of characters sent.
1111 int vrpn_Tracker_FastrakPDI::encode_to(char *buf)
1112 {
1113  char *bufptr = buf;
1114  int buflen = 1000;
1115 
1116  // Message includes: long sensor, long scrap, vrpn_float64 pos[3], vrpn_float64 quat[4]
1117  // Byte order of each needs to be reversed to match network standard
1118 
1119  vrpn_buffer(&bufptr, &buflen, d_sensor);
1120  vrpn_buffer(&bufptr, &buflen, d_sensor); // This is just to take up space to align
1121 
1122  vrpn_buffer(&bufptr, &buflen, pos[0]);
1123  vrpn_buffer(&bufptr, &buflen, pos[1]);
1124  vrpn_buffer(&bufptr, &buflen, pos[2]);
1125 
1126  vrpn_buffer(&bufptr, &buflen, d_quat[0]);
1127  vrpn_buffer(&bufptr, &buflen, d_quat[1]);
1128  vrpn_buffer(&bufptr, &buflen, d_quat[2]);
1129  vrpn_buffer(&bufptr, &buflen, d_quat[3]);
1130 
1131  return 1000 - buflen;
1132 }
1133 
1134 
1135 // Initialize the device and some variables
1136 BOOL vrpn_Tracker_FastrakPDI::Initialize(VOID)
1137 {
1138  BOOL bRet = TRUE;
1139 
1140  pos[0]=0; pos[1]=0; pos[2]=0;
1141  d_quat[0]=0; d_quat[1]=0; d_quat[2]=0; d_quat[3]=0;
1142 
1143  memset( FTstylusBtns, 0, sizeof(FTstylusBtns));
1144 
1145  hContEvent = NULL;
1146  hwnd = NULL;
1147  dwOverflowCount = 0;
1148 
1149 
1150  pdiDev.Trace(TRUE, 5); // Report debugging information to IDE output
1151 
1152  bCnxReady = FALSE;
1153  dwStationMap = 0;
1154 
1155  return bRet;
1156 }
1157 
1158 BOOL vrpn_Tracker_FastrakPDI::InitStylusBtns()
1159 {
1160  BOOL bRet = TRUE;
1161  int mask = 1;
1162  for (int i=0; i<FT_MAX_SENSORS; i++)
1163  {
1164  if (((1<<i) & m_nStylusMap) != 0)
1165  {
1166  char btnName[512];
1167  sprintf( btnName, "%sStylus%d", d_servicename, i+1);
1168  FTstylusBtns[i] = new vrpn_Button_Server( btnName, d_connection, 1 );
1169  if (FTstylusBtns[i] == NULL)
1170  {
1171  cout << "Cannot create button device " << btnName << endl;
1172  bRet = FALSE;
1173  }
1174  else
1175  {
1176  cout << "Button device " << btnName << " created." << endl;
1177  }
1178  }
1179  }
1180  return bRet;
1181 }
1182 // Connect to the Fastrak using the filepath provided in the config file
1183 // Sets tracker to default VRPN units and frames
1184 BOOL vrpn_Tracker_FastrakPDI::Connect( VOID )
1185 {
1186  if (!(pdiDev.CnxReady()))
1187  {
1188  pdiDev.SetSerialIF( &pdiSer );
1189 
1190  ePiCommType eType;
1191 
1192  eType = pdiDev.DiscoverCnx();
1193  DWORD attempts = 0;
1194  while (eType != PI_CNX_USB && eType != PI_CNX_SERIAL && attempts < 10){
1195  switch (eType)
1196  {
1197  case PI_CNX_USB:
1198  cout << "FastrakPDI: USB Connection\r\n";
1199  status = vrpn_TRACKER_SYNCING;
1200  break;
1201  case PI_CNX_SERIAL:
1202  cout << "FastrakPDI: Serial Connection\r\n";
1203  status = vrpn_TRACKER_SYNCING;
1204  break;
1205  default:
1206  printf("FastrakPDI: %s\r\n", pdiDev.GetLastResultStr() );
1207  eType = pdiDev.DiscoverCnx();
1208  attempts++;
1209  break;
1210  }
1211  Sleep(3000);
1212  }
1213 
1214  UpdateStationMap();
1215  num_sensors = pdiDev.StationCount();
1216 
1217  CPDIbiterr cBE;
1218  pdiDev.GetBITErrs( cBE );
1219 
1220  CHAR sz[100];
1221  cBE.Parse( sz, 100 );
1222 
1223  if(!(cBE.IsClear()))
1224  pdiDev.ClearBITErrs();
1225 
1226  // Set VRPN defaults for position, orientation and frame structure
1227  //m_nFrameSize = 28;
1228  pdiMDat.Empty();
1229  if (m_nStylusMap)
1230  {
1231  pdiMDat.Append(PDI_MODATA_STYLUS);
1232  }
1233  pdiMDat.Append( PDI_MODATA_POS );
1234  pdiMDat.Append( PDI_MODATA_QTRN );
1235  pdiDev.SetSDataList( -1, pdiMDat );
1236  pdiDev.SetMetric(TRUE);
1237  isMetric = TRUE;
1238  isBinary = TRUE;
1239 
1240  pdiDev.SetPnoBuffer( pMotionBuf, VRPN_PDI_BUFFER_SIZE );
1241 
1242  bCnxReady = pdiDev.CnxReady();
1243  }
1244  else{
1245  cout << "FastrakPDI: Already connected\r\n";
1246  bCnxReady = TRUE;
1247  }
1248 
1249  return bCnxReady;
1250 }
1251 
1252 // End the connection to the Fastrak
1253 VOID vrpn_Tracker_FastrakPDI::Disconnect(VOID)
1254 {
1255  string msg;
1256  if (!(pdiDev.CnxReady()))
1257  {
1258  cout << "FastrakPDI: Already disconnected\r\n";
1259  }
1260  else
1261  {
1262  pdiDev.Disconnect();
1263  cout << "FastrakPDI: Disconnected\r\n";
1264  }
1265 
1266 }
1267 
1268 // Parses rcmd and calls SendCommand for each line
1269 BOOL vrpn_Tracker_FastrakPDI::SetupDevice( VOID )
1270 {
1271  char * pcmd = cmd;
1272  if(strlen(pcmd) > 0){
1273  char * pch = strchr(pcmd,'\\');
1274  while (pch != NULL){
1275  pch[0] = '\0';
1276  SendCommand(pcmd);
1277  pcmd = pch + sizeof(char);
1278  pch = strchr(pcmd,'\\');
1279  }
1280  SendCommand(pcmd);
1281  }
1282 
1283  // warn the user if tracker is in ASCII mode
1284  if (isBinary == FALSE)
1285  cout << "FastrakPDI: Warning! Tracker is still in ASCII mode!\r\n";
1286 
1287  return TRUE;
1288 }
1289 
1290 // Updates the map of hubs and sensors
1291 VOID vrpn_Tracker_FastrakPDI::UpdateStationMap( VOID )
1292 {
1293  pdiDev.GetStationMap( dwStationMap );
1294 }
1295 
1296 // Sends commands to the tracker, the syntax is explained in vrpn_FastrakPDI.cfg
1297 VOID vrpn_Tracker_FastrakPDI::SendCommand(char *scmd){
1298  char szCmd[PI_MAX_CMD_BUF_LEN] = "\0";
1299  DWORD dwRspSize = 5000;
1300  char szRsp[5000] = "\0";
1301  bool parseErr = false;
1302 
1303  // print scmd to server screen
1304  cout << "FastrakPDI: reset command ";
1305  unsigned int i = 0;
1306  while (scmd[i] != '\0'){
1307  // ignore carriage returns and new lines (cleans up output)
1308  if (scmd[i] != '\n' && scmd[i] != '\r')
1309  cout << scmd[i];
1310  i++;
1311  }
1312 
1313  for (i = 0; i < strlen(scmd); i++){
1314  switch (scmd[i]){
1315  case '^': // convert ^ to control+char ascii equivalent
1316  i++;
1317  switch (scmd[i]){
1318  case 'K':
1319  strncat(szCmd, "\x0b", 1);
1320  break;
1321  case 'Q':
1322  strncat(szCmd, "\x11", 1);
1323  break;
1324  case 'S':
1325  strncat(szCmd, "\x13", 1);
1326  break;
1327  case 'Y':
1328  strncat(szCmd, "\x19", 1);
1329  break;
1330  default: // no match, flip parseErr
1331  i = strlen(scmd);
1332  parseErr = true;
1333  }
1334  break;
1335  case '<': // check for <>, which represents a carriage return
1336  i++;
1337  if (scmd[i] != '>'){
1338  i = strlen(scmd);
1339  parseErr = true;
1340  }
1341  else
1342  strncat(szCmd, "\r\n", 1);
1343  break;
1344  case '\r': // ignore carriage returns.
1345  break;
1346  case '\n': // ignore new lines
1347  break;
1348  case '\xff': // ignore eof
1349  break;
1350  default:
1351  strncat(szCmd, &scmd[i], 1);
1352  }
1353  }
1354 
1355  // CPDIdev::TxtCmd() and CPDIdev::SetBinary() functions are not exposed in CPDIfastrak
1356  // but can be called from the base class (CPDIdev)
1357 
1358  // Typecasts pdiDev as a CPDIdev in pointer pDev
1359  CPDIdev * pDev = (CPDIdev*) (&pdiDev);
1360 
1361  if (parseErr == true)
1362  cout << "\r\n\t<unrecognized command>\r\n";
1363  else{
1364  strncat(szCmd, "\0", 1);
1365  switch (szCmd[0]){
1366  case 'C': // Continuous pno command
1367  case 'c': // Ignored since this would conflict with VRPN directly
1368  cout << "\r\n\t<command ignored, use P instead>\r\n";
1369  break;
1370  case 0x19: // reset command
1371  pDev->TxtCmd(szCmd,dwRspSize,szRsp);
1372  cout << "\r\n\t<tracker will be set to VRPN defaults on reconnect>\r\n";
1373  Disconnect();
1374  Sleep(2500);
1375  Connect();
1376  break;
1377  case 'U': // units command
1378  case 'u':
1379  pDev->TxtCmd(szCmd,dwRspSize,szRsp);
1380  if (isBinary == TRUE)
1381  pdiDev.GetMetric(isMetric);
1382  else{
1383  // if tracker is in ASCII mode, switch to binary for GetMetric function
1384  pDev->SetBinary(TRUE);
1385  pdiDev.GetMetric(isMetric);
1386  pDev->SetBinary(FALSE);
1387  }
1388  if (isMetric == FALSE)
1389  cout << "\r\n\t<position units set to inches>\r\n";
1390  else
1391  cout << "\r\n\t<position units set to meters>\r\n";
1392  break;
1393  case 'F': // format (binary or ascii) command
1394  case 'f':
1395  pDev->TxtCmd(szCmd,dwRspSize,szRsp);
1396  pdiDev.GetBinary(isBinary);
1397  if (isBinary == FALSE)
1398  cout << "\r\n\t<response frames set to ASCII>\r\n";
1399  else
1400  cout << "\r\n\t<response frames set to binary>\r\n";
1401  break;
1402  case 'O': // data list format command
1403  cout << "\r\n\t<pno frame format changed (use extreme caution)>";
1404  case 'P': // print single pno command
1405  if (isBinary == TRUE)
1406  cout << "\r\n\t<suggestion: use ASCII response frames (reset command F)>";
1407  default:
1408  pDev->TxtCmd(szCmd,dwRspSize,szRsp);
1409  if (dwRspSize != 0 && dwRspSize != 5000){
1410  if (isBinary == TRUE)
1411  cout << "\r\n\t<binary response bgn>\r\n\t";
1412  else
1413  cout << "\r\n\t<ASCII response bgn>\r\n\t";
1414  for (i = 0; i < dwRspSize; i++){
1415  if (szRsp[i] != '\r')
1416  printf("%c",szRsp[i]);
1417  if (szRsp[i] == '\n')
1418  printf("\t");
1419  }
1420  if (isBinary == TRUE)
1421  cout << "\r\n\t<binary response end>\r\n";
1422  else
1423  cout << "\r\n\t<ASCII response end>\r\n";
1424  }
1425  else
1426  cout << "\r\n\t<command sent>\r\n";
1427  }
1428  }
1429  //Sleep(500);
1430 }
1431 
1432 // Start Continuous Mode for the Fastrak
1433 BOOL vrpn_Tracker_FastrakPDI::StartCont( VOID )
1434 {
1435  cout << "FastrakPDI: Start Continuous Mode\r\n";
1436  BOOL bRet = FALSE;
1437 
1438 
1439  if (!(pdiDev.StartContPno(hwnd)))
1440  {
1441  }
1442  else
1443  {
1444  isCont = TRUE;
1445  dwOverflowCount = 0;
1446  bRet = TRUE;
1447  }
1448 
1449  return bRet;
1450 }
1451 
1452 // Stops Continuous Mode for the Fastrak
1453 BOOL vrpn_Tracker_FastrakPDI::StopCont( VOID )
1454 {
1455  cout << "FastrakPDI: Stop Continuous Mode\r\n";
1456  BOOL bRet = FALSE;
1457 
1458  if (!(pdiDev.StopContPno()))
1459  {
1460  }
1461  else
1462  {
1463  isCont = FALSE;
1464  bRet = TRUE;
1465  Sleep(1000);
1466  }
1467 
1468  ::ResetEvent(hContEvent);
1469  return bRet;
1470 }
1471 
1472 // Displays a frame of information taken from the continuous stream of
1473 // Continuous Mode by calling displayFrame
1474 BOOL vrpn_Tracker_FastrakPDI::DisplayCont( timeval ct )
1475 {
1476  BOOL bRet = FALSE;
1477 
1478  PBYTE pBuf;
1479  DWORD dwSize;
1480  PBYTE pLastBuf = 0;
1481  DWORD dwLastSize = 0;
1482 
1483  if (!(pdiDev.LastPnoPtr(pBuf, dwSize)))
1484  {
1485  }
1486  else if ((pBuf == 0) || (dwSize == 0))
1487  {
1488  }
1489  else if (pLastBuf && (pBuf > pLastBuf))
1490  {
1491  ParseFastrakFrame( pLastBuf+dwLastSize, dwSize+(pBuf-pLastBuf-dwLastSize), ct );
1492  pLastBuf = pBuf;
1493  dwLastSize = dwSize;
1494  bRet = TRUE;
1495  }
1496  else if (pBuf != pLastBuf) // it wrapped in buffer
1497  {
1498  if (pLastBuf)
1499  cout << "wrapped\r\n";
1500 
1501  pLastBuf = pBuf;
1502  dwLastSize = dwSize;
1503  ParseFastrakFrame( pBuf, dwSize, ct );
1504  bRet = TRUE;
1505  }
1506 
1507  return bRet;
1508 }
1509 
1510 // Displays a single frame of information from the Fastrak by collecting data
1511 // and calling DisplayFrame
1512 VOID vrpn_Tracker_FastrakPDI::DisplaySingle( timeval ct )
1513 {
1514  BOOL bExit = FALSE;
1515 
1516  PBYTE pBuf;
1517  DWORD dwSize;
1518 
1519  cout << endl;
1520 
1521  if (!(pdiDev.ReadSinglePnoBuf(pBuf, dwSize)))
1522  {
1523  bExit = TRUE;
1524  }
1525  else if ((pBuf == 0) || (dwSize == 0))
1526  {
1527  }
1528  else
1529  {
1530  ParseFastrakFrame( pBuf, dwSize, ct );
1531  }
1532 }
1533 
1534 // Parses the data collected from a DisplaySingle or DisplayContinuous command, packages it and sends it
1535 // out to clients calling for it
1536 VOID vrpn_Tracker_FastrakPDI::ParseFastrakFrame( PBYTE pBuf, DWORD dwSize, timeval current_time )
1537 {
1538 
1539  DWORD index = 0;
1540  char msgbuf[1000];
1541  vrpn_int32 len;
1542 
1543  while (index < dwSize){
1544  DWORD dw = index;
1545  BYTE ucSensor = pBuf[dw+1];
1546  BYTE ucInitCommand = pBuf[dw];
1547  BYTE ucErrorNum = pBuf[dw+2];
1548  d_sensor = atoi((char*)(&ucSensor));
1549 
1550  // skip rest of header
1551  dw += m_nHeaderSize;//3;
1552 
1553  // Catch command response frames sent when tracker is in continuous mode
1554  // and don't parse them but do provide output to the server screen
1555  if (ucInitCommand != '0'){
1556  printf("FastrakPDI: received record type %x while in continuous mode, record error byte was %x \r\n", ucInitCommand, ucErrorNum);
1557  }
1558  else{
1559 
1560  if (m_nStylusMap)
1561  {
1562  if ((m_nStylusMap & (1 << (d_sensor-1))) != 0) //FTstylusBtns[ucSensor-1])
1563  {
1564  CHAR StyFlag = pBuf[dw+1];
1565 
1566  FTstylusBtns[d_sensor-1]->set_button(0, StyFlag - '0');
1567  FTstylusBtns[d_sensor-1]->mainloop();
1568  }
1569  dw +=2;
1570  }
1571 
1572  PFLOAT pPno = (PFLOAT)(&pBuf[dw]); // Position and Orientation data
1573 
1574  if (isMetric == TRUE){
1575  pos[0] = float(pPno[0])*CM_TO_METERS;
1576  pos[1] = float(pPno[1])*CM_TO_METERS;
1577  pos[2] = float(pPno[2])*CM_TO_METERS;
1578  }
1579  else{
1580  pos[0] = float(pPno[0]);
1581  pos[1] = float(pPno[1]);
1582  pos[2] = float(pPno[2]);
1583  }
1584 
1585  // Fastrak quaternion format is WXYZ, VRPN is XYZW
1586  d_quat[0] = float(pPno[4]);
1587  d_quat[1] = float(pPno[5]);
1588  d_quat[2] = float(pPno[6]);
1589  d_quat[3] = float(pPno[3]);
1590 
1591 
1592  // Grab the current time and create a timestamp
1593  timestamp.tv_sec = current_time.tv_sec;
1594  timestamp.tv_usec = current_time.tv_usec;
1595  if (d_connection) {
1596  // Pack position report
1597  len = encode_to(msgbuf);
1598  d_connection->pack_message(len, timestamp,
1599  position_m_id, d_sender_id, msgbuf, vrpn_CONNECTION_LOW_LATENCY);
1600  }
1601  }
1602  index += m_nFrameSize;//28;
1603  }
1604 }
1605 
1606 // Constructor
1607 vrpn_Tracker_LibertyPDI::vrpn_Tracker_LibertyPDI (const char * name, vrpn_Connection *cn,
1608  vrpn_float64 Hz, const char * rcmd, unsigned int nStylusMap) :
1609  vrpn_Tracker(name, cn), update_rate(Hz)
1610  , m_nStylusMap(nStylusMap)
1611  , m_nHeaderSize(8)
1612  , m_nFrameSize(nStylusMap?40:36)
1613  {
1614  cmd = (char*)(rcmd);
1616  if(!(Initialize())){
1618  }
1619  else if (nStylusMap & !(InitStylusBtns()))
1620  {
1622  }
1623  else if(!(Connect())){
1625  }
1626  else if(!(SetupDevice())){
1628  }
1629  else if(!(StartCont())){
1630  isCont = FALSE;
1631  cout << "LibertyPDI: Failed to enter continuous mode\r\n";
1633  }
1634  else {
1635  cout << "LibertyPDI: Initialization Complete\r\n";
1637  }
1638  }
1639 
1640 // Deconstructor
1641 vrpn_Tracker_LibertyPDI::~vrpn_Tracker_LibertyPDI(void){
1642  if(isCont)
1643  StopCont();
1644  Disconnect();
1645 
1646  for (int i=0; i<LIBERTY_MAX_SENSORS; i++)
1647  {
1648  if (StylusBtns[i])
1649  delete StylusBtns[i];
1650  }
1651  }
1652 
1653 // Called by the vrpn_Generic_Sever class in order to report its status.
1654 VOID vrpn_Tracker_LibertyPDI::mainloop()
1655 {
1656  struct timeval current_time;
1657 
1658  // Call the generic server mainloop routine, since this is a server
1659  server_mainloop();
1660 
1661  // See if its time to generate a new report
1662  vrpn_gettimeofday(&current_time, NULL);
1663  if ( vrpn_TimevalDuration(current_time,timestamp) >= 1000000.0/update_rate) {
1664  DisplayCont(current_time);
1665  }
1666 }
1667 
1668 // NOTE: you need to be sure that if you are sending vrpn_float64 then
1669 // the entire array needs to remain aligned to 8 byte boundaries
1670 // (malloced data and static arrays are automatically alloced in
1671 // this way). Assumes that there is enough room to store the
1672 // entire message. Returns the number of characters sent.
1673 int vrpn_Tracker_LibertyPDI::encode_to(char *buf)
1674 {
1675  char *bufptr = buf;
1676  int buflen = 1000;
1677 
1678  // Message includes: long sensor, long scrap, vrpn_float64 pos[3], vrpn_float64 quat[4]
1679  // Byte order of each needs to be reversed to match network standard
1680 
1681  vrpn_buffer(&bufptr, &buflen, d_sensor);
1682  vrpn_buffer(&bufptr, &buflen, d_sensor); // This is just to take up space to align
1683 
1684  vrpn_buffer(&bufptr, &buflen, pos[0]);
1685  vrpn_buffer(&bufptr, &buflen, pos[1]);
1686  vrpn_buffer(&bufptr, &buflen, pos[2]);
1687 
1688  vrpn_buffer(&bufptr, &buflen, d_quat[0]);
1689  vrpn_buffer(&bufptr, &buflen, d_quat[1]);
1690  vrpn_buffer(&bufptr, &buflen, d_quat[2]);
1691  vrpn_buffer(&bufptr, &buflen, d_quat[3]);
1692 
1693  return 1000 - buflen;
1694 }
1695 
1696 
1697 // Initialize the device and some variables
1698 BOOL vrpn_Tracker_LibertyPDI::Initialize(VOID){
1699 
1700  pos[0]=0; pos[1]=0; pos[2]=0;
1701  d_quat[0]=0; d_quat[1]=0; d_quat[2]=0; d_quat[3]=0;
1702 
1703  memset( StylusBtns, 0, sizeof(StylusBtns));
1704 
1705  hContEvent = NULL;
1706  hwnd = NULL;
1707  dwOverflowCount = 0;
1708 
1709  BOOL bRet = TRUE;
1710 
1711  pdiDev.Trace(TRUE, 5); // Report debugging information to IDE output
1712 
1713  bCnxReady = FALSE;
1714  dwStationMap = 0;
1715 
1716  return bRet;
1717 }
1718 
1719 
1720 BOOL vrpn_Tracker_LibertyPDI::InitStylusBtns()
1721 {
1722  BOOL bRet = TRUE;
1723  int mask = 1;
1724  for (int i=0; i<LIBERTY_MAX_SENSORS; i++)
1725  {
1726  if (((1<<i) & m_nStylusMap) != 0)
1727  {
1728  char btnName[512];
1729  sprintf( btnName, "%sStylus%d", d_servicename, i+1);
1730  StylusBtns[i] = new vrpn_Button_Server( btnName, d_connection, 1 );
1731  if (StylusBtns[i] == NULL)
1732  {
1733  cout << "Cannot create button device " << btnName << endl;
1734  bRet = FALSE;
1735  }
1736  else
1737  {
1738  cout << "Button device " << btnName << " created." << endl;
1739  }
1740  }
1741  }
1742  return bRet;
1743 }
1744 
1745 // Connect to the Liberty using the filepath provided in the config file.
1746 // Sets tracker to default VRPN units and frames
1747 BOOL vrpn_Tracker_LibertyPDI::Connect( VOID )
1748 {
1749  if (!(pdiDev.CnxReady()))
1750  {
1751  pdiDev.SetSerialIF( &pdiSer );
1752 
1753  ePiCommType eType;
1754 
1755  eType = pdiDev.DiscoverCnx();
1756  DWORD attempts = 0;
1757  while (eType != PI_CNX_USB && eType != PI_CNX_SERIAL && attempts < 10){
1758  switch (eType)
1759  {
1760  case PI_CNX_USB:
1761  cout << "LibertyPDI: USB Connection\r\n";
1762  status = vrpn_TRACKER_SYNCING;
1763  break;
1764  case PI_CNX_SERIAL:
1765  cout << "LibertyPDI: Serial Connection\r\n";
1766  status = vrpn_TRACKER_SYNCING;
1767  break;
1768  default:
1769  printf("LibertyPDI: %s\r\n", pdiDev.GetLastResultStr() );
1770  eType = pdiDev.DiscoverCnx();
1771  attempts++;
1772  break;
1773  }
1774  Sleep(3000);
1775  }
1776 
1777  UpdateStationMap();
1778  num_sensors = pdiDev.StationCount();
1779 
1780  CPDIbiterr cBE;
1781  pdiDev.GetBITErrs( cBE );
1782 
1783  CHAR sz[100];
1784  cBE.Parse( sz, 100 );
1785 
1786  if(!(cBE.IsClear()))
1787  pdiDev.ClearBITErrs();
1788 
1789  // Set VRPN defaults for position, orientation and frame structure
1790  pdiMDat.Empty();
1791  if (m_nStylusMap)
1792  {
1793  pdiMDat.Append(PDI_MODATA_STYLUS);
1794  }
1795  pdiMDat.Append( PDI_MODATA_POS );
1796  pdiMDat.Append( PDI_MODATA_QTRN );
1797  pdiDev.SetSDataList( -1, pdiMDat );
1798 
1799  pdiDev.SetMetric(TRUE);
1800  isMetric = TRUE;
1801  isBinary = TRUE;
1802 
1803  // The CPDIdev class will use this space, even if we don't access it directly,
1804  // which allows us to specify the size of the buffer
1805  pdiDev.SetPnoBuffer( pMotionBuf, VRPN_PDI_BUFFER_SIZE );
1806 
1807  bCnxReady = pdiDev.CnxReady();
1808  }
1809  else{
1810  cout << "LibertyPDI: Already connected\r\n";
1811  bCnxReady = TRUE;
1812  }
1813 
1814  return bCnxReady;
1815 }
1816 
1817 // End the connection to the Liberty
1818 VOID vrpn_Tracker_LibertyPDI::Disconnect(VOID)
1819 {
1820  string msg;
1821  if (!(pdiDev.CnxReady()))
1822  {
1823  cout << "LibertyPDI: Already disconnected\r\n";
1824  }
1825  else
1826  {
1827  pdiDev.Disconnect();
1828  cout << "LibertyPDI: Disconnected\r\n";
1829  }
1830 
1831 }
1832 
1833 // Send reset commands to tracker
1834 BOOL vrpn_Tracker_LibertyPDI::SetupDevice( VOID )
1835 {
1836  char * pcmd = cmd;
1837  if(strlen(pcmd) > 0){
1838  char * pch = strchr(pcmd,'\\');
1839  while (pch != NULL){
1840  pch[0] = '\0';
1841  SendCommand(pcmd);
1842  pcmd = pch + sizeof(char);
1843  pch = strchr(pcmd,'\\');
1844  }
1845  SendCommand(pcmd);
1846  }
1847 
1848  // warn the user if tracker is in ASCII mode
1849  if (isBinary == FALSE)
1850  cout << "LibertyPDI: Warning! Tracker is still in ASCII mode!\r\n";
1851 
1852  return TRUE;
1853 }
1854 
1855 // Updates the map of hubs and sensors
1856 VOID vrpn_Tracker_LibertyPDI::UpdateStationMap( VOID )
1857 {
1858  pdiDev.GetStationMap( dwStationMap );
1859 }
1860 
1861 // Sends commands to the tracker, the syntax is explained in vrpn_LibertyPDI.cfg
1862 VOID vrpn_Tracker_LibertyPDI::SendCommand(char *scmd)
1863 {
1864  char szCmd[PI_MAX_CMD_BUF_LEN] = "\0";
1865  DWORD dwRspSize = 5000;
1866  char szRsp[5000] = "\0";
1867  bool parseErr = false;
1868 
1869  // print scmd to server screen
1870  cout << "LibertyPDI: reset command ";
1871  unsigned int i = 0;
1872  while (scmd[i] != '\0'){
1873  // ignore carriage returns and new lines (cleans up output)
1874  if (scmd[i] != '\n' && scmd[i] != '\r')
1875  cout << scmd[i];
1876  i++;
1877  }
1878 
1879  // parse scmd and create a liberty friendly command string szCmd
1880  for (i = 0; i < strlen(scmd); i++){
1881  switch (scmd[i]){
1882  case '^': // convert ^ to control+char ascii equivalent
1883  i++;
1884  switch (scmd[i]){
1885  case 'A':
1886  strncat(szCmd, "\x01", 1);
1887  break;
1888  case 'B':
1889  strncat(szCmd, "\x02", 1);
1890  break;
1891  case 'D':
1892  strncat(szCmd, "\x04", 1);
1893  break;
1894  case 'E':
1895  strncat(szCmd, "\x05", 1);
1896  break;
1897  case 'F':
1898  strncat(szCmd, "\x06", 1);
1899  break;
1900  case 'G':
1901  strncat(szCmd, "\x07", 1);
1902  break;
1903  case 'K':
1904  strncat(szCmd, "\x0b", 1);
1905  break;
1906  case 'L':
1907  strncat(szCmd, "\x0c", 1);
1908  break;
1909  case 'N':
1910  strncat(szCmd, "\x0e", 1);
1911  break;
1912  case 'O':
1913  strncat(szCmd, "\x0f", 1);
1914  break;
1915  case 'P':
1916  strncat(szCmd, "\x10", 1);
1917  break;
1918  case 'R':
1919  strncat(szCmd, "\x12", 1);
1920  break;
1921  case 'S':
1922  strncat(szCmd, "\x13", 1);
1923  break;
1924  case 'T':
1925  strncat(szCmd, "\x14", 1);
1926  break;
1927  case 'U':
1928  strncat(szCmd, "\x15", 1);
1929  break;
1930  case 'V':
1931  strncat(szCmd, "\x16", 1);
1932  break;
1933  case 'W':
1934  strncat(szCmd, "\x17", 1);
1935  break;
1936  case 'X':
1937  strncat(szCmd, "\x18", 1);
1938  break;
1939  case 'Y':
1940  strncat(szCmd, "\x19", 1);
1941  break;
1942  case 'Z':
1943  strncat(szCmd, "\x1a", 1);
1944  break;
1945  default: // no match, flip parseErr
1946  i = strlen(scmd);
1947  parseErr = true;
1948  }
1949  break;
1950  case '<': // check for <>, which represents a carriage return
1951  i++;
1952  if (scmd[i] != '>'){
1953  i = strlen(scmd);
1954  parseErr = true;
1955  }
1956  else
1957  strncat(szCmd, "\r\n", 1);
1958  break;
1959  case '\r': // ignore carriage returns.
1960  break;
1961  case '\n': // ignore new lines
1962  break;
1963  case '\xff': // ignore eof
1964  break;
1965  default:
1966  strncat(szCmd, &scmd[i], 1);
1967  }
1968  }
1969 
1970  if (parseErr == true)
1971  cout << "\r\n\t<unrecognized command>\r\n";
1972  else{
1973  strncat(szCmd, "\0", 1);
1974  switch (szCmd[0]){
1975  case 'C': // Continuous pno command
1976  case 'c': // Ignored since this would conflict with VRPN directly
1977  cout << "\r\n\t<command ignored, use P instead>\r\n";
1978  break;
1979  case 0x19: // reset command
1980  pdiDev.TxtCmd(szCmd,dwRspSize,szRsp);
1981  cout << "\r\n\t<tracker will be set to VRPN defaults on reconnect>\r\n";
1982  Disconnect();
1983  Sleep(2500);
1984  Connect();
1985  break;
1986  case 'U': // units command
1987  pdiDev.TxtCmd(szCmd,dwRspSize,szRsp);
1988  if (isBinary == TRUE)
1989  pdiDev.GetMetric(isMetric);
1990  else{
1991  pdiDev.SetBinary(TRUE);
1992  pdiDev.GetMetric(isMetric);
1993  pdiDev.SetBinary(FALSE);
1994  }
1995  if (isMetric == FALSE)
1996  cout << "\r\n\t<position units set to inches>\r\n";
1997  else
1998  cout << "\r\n\t<position units set to meters>\r\n";
1999  break;
2000  case 'F': // format (binary or ascii) command
2001  pdiDev.TxtCmd(szCmd,dwRspSize,szRsp);
2002  pdiDev.GetBinary(isBinary);
2003  if (isBinary == FALSE)
2004  cout << "\r\n\t<response frames set to ascii>\r\n";
2005  else
2006  cout << "\r\n\t<response frames set to binary>\r\n";
2007  break;
2008  case 'O': // data list format command
2009  pdiDev.TxtCmd(szCmd,dwRspSize,szRsp);
2010  cout << "\r\n\t<pno frame format changed (use extreme caution)>\r\n";
2011  break;
2012  default:
2013  pdiDev.TxtCmd(szCmd,dwRspSize,szRsp);
2014  if (isBinary == TRUE && dwRspSize != 0 && dwRspSize != 5000){
2015  char * pRsp = szRsp;
2016  pRsp += 4;
2017  if (*pRsp == 0x00 || *pRsp == 0x20)
2018  cout << "\r\n\t<binary response contained no error>\r\n";
2019  else{
2020  pRsp += 2;
2021  cout << "\r\n\t<binary error response bgn>\r\n\t";
2022  for (int i = 2; i < 2 + short(*pRsp); i++){
2023  if (szRsp[i] != '\r')
2024  printf("%c",pRsp[i]);
2025  if (szRsp[i] == '\n')
2026  printf("\t");
2027  }
2028  cout << "\r\n\t<binary error response end>\r\n";
2029  }
2030  }
2031  else if (dwRspSize != 0 && dwRspSize != 5000){
2032  cout << "\r\n\t<ASCII response bgn>\r\n\t";
2033  for (unsigned int i = 0; i < dwRspSize; i++){
2034  if (szRsp[i] != '\r')
2035  printf("%c",szRsp[i]);
2036  if (szRsp[i] == '\n')
2037  printf("\t");
2038  }
2039  cout << "\r\n\t<ASCII response end>\r\n";
2040  }
2041  else
2042  cout << "\r\n\t<command sent>\r\n";
2043  }
2044  }
2045 }
2046 
2047 // Start Continuous Mode for the Liberty
2048 BOOL vrpn_Tracker_LibertyPDI::StartCont( VOID )
2049 {
2050  cout << "LibertyPDI: Start Continuous Mode\r\n";
2051  BOOL bRet = FALSE;
2052 
2053 
2054  if (!(pdiDev.StartContPno(hwnd)))
2055  {
2056  }
2057  else
2058  {
2059  isCont = TRUE;
2060  dwOverflowCount = 0;
2061  bRet = TRUE;
2062  }
2063 
2064  return bRet;
2065 }
2066 
2067 // Stops Continuous Mode for the Liberty
2068 BOOL vrpn_Tracker_LibertyPDI::StopCont( VOID )
2069 {
2070  cout << "LibertyPDI: Stop Continuous Mode\r\n";
2071  BOOL bRet = FALSE;
2072 
2073  if (!(pdiDev.StopContPno()))
2074  {
2075  }
2076  else
2077  {
2078  isCont = FALSE;
2079  bRet = TRUE;
2080  Sleep(1000);
2081  }
2082 
2083  ::ResetEvent(hContEvent);
2084  return bRet;
2085 }
2086 
2087 // Displays a frame of information taken from the continuous stream of
2088 // Continuous Mode by calling displayFrame
2089 BOOL vrpn_Tracker_LibertyPDI::DisplayCont( timeval ct )
2090 {
2091  BOOL bRet = FALSE;
2092 
2093  PBYTE pBuf;
2094  DWORD dwSize;
2095  PBYTE pLastBuf = 0;
2096  DWORD dwLastSize = 0;
2097 
2098  if (!(pdiDev.LastPnoPtr(pBuf, dwSize)))
2099  {
2100  }
2101  else if ((pBuf == 0) || (dwSize == 0))
2102  {
2103  }
2104  else if (pLastBuf && (pBuf > pLastBuf))
2105  {
2106  ParseLibertyFrame( pLastBuf+dwLastSize, dwSize+(pBuf-pLastBuf-dwLastSize), ct );
2107  pLastBuf = pBuf;
2108  dwLastSize = dwSize;
2109  bRet = TRUE;
2110  }
2111  else if (pBuf != pLastBuf) // it wrapped in buffer
2112  {
2113  if (pLastBuf)
2114  cout << "wrapped\n";
2115 
2116  pLastBuf = pBuf;
2117  dwLastSize = dwSize;
2118  ParseLibertyFrame( pBuf, dwSize, ct );
2119  bRet = TRUE;
2120  }
2121 
2122  return bRet;
2123 }
2124 
2125 // Displays a single frame of information from the Liberty by collecting data
2126 // and calling DisplayFrame
2127 VOID vrpn_Tracker_LibertyPDI::DisplaySingle( timeval ct )
2128 {
2129  BOOL bExit = FALSE;
2130 
2131  PBYTE pBuf;
2132  DWORD dwSize;
2133 
2134  cout << endl;
2135 
2136  if (!(pdiDev.ReadSinglePnoBuf(pBuf, dwSize)))
2137  {
2138  bExit = TRUE;
2139  }
2140  else if ((pBuf == 0) || (dwSize == 0))
2141  {
2142  }
2143  else
2144  {
2145  ParseLibertyFrame( pBuf, dwSize, ct );
2146  }
2147 }
2148 
2149 // Parses the data collected from a DisplaySingle or DisplayContinuous command, packages it and sends it
2150 // out to clients calling for it
2151 VOID vrpn_Tracker_LibertyPDI::ParseLibertyFrame( PBYTE pBuf, DWORD dwSize, timeval current_time )
2152 {
2153 
2154  DWORD index = 0;
2155  char msgbuf[1000];
2156  vrpn_int32 len;
2157 
2158  while (index < dwSize){
2159  DWORD dw = index;
2160  BYTE ucSensor = pBuf[dw+2];
2161  BYTE ucInitCommand = pBuf[dw+3];
2162  BYTE ucErrorNum = pBuf[dw+4];
2163  SHORT shSize = pBuf[dw+6];
2164  d_sensor = unsigned short(ucSensor);
2165 
2166  // skip rest of header
2167  dw += m_nHeaderSize;//8;
2168 
2169  // Catch command response frames sent when tracker is in continuous mode
2170  // and don't parse them but do provide output to the server screen
2171  if (ucInitCommand != 'C' && ucInitCommand != 'P'){
2172  printf("LibertyPDI: received command %x while in continuous mode, tracker response was %x \r\n", ucInitCommand, ucErrorNum);
2173  }
2174  else{
2175  if (m_nStylusMap)
2176  {
2177  if ((m_nStylusMap & (1 << (d_sensor-1))) != 0)
2178  {
2179  DWORD m_dwStylus = *((DWORD*)&pBuf[dw]);
2180 
2181  StylusBtns[d_sensor-1]->set_button(0, m_dwStylus);
2182  StylusBtns[d_sensor-1]->mainloop();
2183  }
2184  dw += sizeof(DWORD);;
2185  }
2186 
2187 
2188  PFLOAT pPno = (PFLOAT)(&pBuf[dw]); // Position and Orientation data
2189 
2190  if (isMetric == TRUE){
2191  pos[0] = float(pPno[0])*CM_TO_METERS;
2192  pos[1] = float(pPno[1])*CM_TO_METERS;
2193  pos[2] = float(pPno[2])*CM_TO_METERS;
2194  }
2195  else{
2196  pos[0] = float(pPno[0]);
2197  pos[1] = float(pPno[1]);
2198  pos[2] = float(pPno[2]);
2199  }
2200 
2201  // Liberty quaternion format is WXYZ, VRPN is XYZW
2202  d_quat[0] = float(pPno[4]);
2203  d_quat[1] = float(pPno[5]);
2204  d_quat[2] = float(pPno[6]);
2205  d_quat[3] = float(pPno[3]);
2206 
2207  // Grab the current time and create a timestamp
2208  timestamp.tv_sec = current_time.tv_sec;
2209  timestamp.tv_usec = current_time.tv_usec;
2210  if (d_connection) {
2211  // Pack position report
2212  len = encode_to(msgbuf);
2213  d_connection->pack_message(len, timestamp,
2214  position_m_id, d_sender_id, msgbuf, vrpn_CONNECTION_LOW_LATENCY);
2215  }
2216  }
2217  index += m_nFrameSize;
2218  }
2219 }
2220 
2221 #endif
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
const int vrpn_TRACKER_REPORT_READY
Definition: vrpn_Tracker.h:37
class VRPN_API vrpn_Button_Server
int register_server_handlers(void)
Definition: vrpn_Tracker.C:318
STL namespace.
Generic connection class not specific to the transport mechanism.
#define VRPN_SUPPRESS_EMPTY_OBJECT_WARNING()
const int vrpn_TRACKER_FAIL
Definition: vrpn_Tracker.h:40
const int vrpn_TRACKER_SYNCING
Definition: vrpn_Tracker.h:35
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
VRPN_API int vrpn_buffer(char **insertPt, vrpn_int32 *buflen, const timeval t)
Utility routine for placing a timeval struct into a buffer that is to be sent as a message...
Definition: vrpn_Shared.C:241
unsigned long vrpn_TimevalDuration(struct timeval endT, struct timeval startT)
Return number of microseconds between startT and endT.
Definition: vrpn_Shared.C:129