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