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