10#define CM_TO_METERS (1/100.0)
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) :
16 srcCalPath = LPCTSTR(filepath);
18 register_server_handlers();
21 cout<<
"G4: Could not initialize\r\n";
24 else if (pHMap && !(InitDigIOBtns()))
26 cout<<
"G4: Could not configure DigIO buttons\r\n";
31 cout<<
"G4: Could not connect\r\n";
35 else if(!(SetupDevice()))
37 cout<<
"G4: Could not setup device\r\n";
40 else if(!(StartCont())){
41 cout<<
"G4: Failed to enter continuous mode\r\n";
45 cout<<
"G4: Initialization Complete\r\n";
51vrpn_Tracker_G4::~vrpn_Tracker_G4(
void){
61 fprintf(stderr,
"vrpn_Tracker_G4::~vrpn_Tracker_G4(): delete failed\n");
69void vrpn_Tracker_G4::mainloop()
71 struct timeval current_time;
79 DisplayCont(current_time);
88int vrpn_Tracker_G4::encode_to(
char *buf)
108 return 1000 - buflen;
114BOOL vrpn_Tracker_G4::Initialize(VOID){
122 pdiG4.Trace(TRUE, 5);
131BOOL vrpn_Tracker_G4::InitDigIOBtns()
138 HUBMAP_ENTRY * pHub = m_pHMap->Begin();
144 try { pHub->pBtnSrv =
new vrpn_Button_Server(pHub->BtnName, d_connection, pHub->nBtnCount ); }
147 cout <<
"Cannot create button device " << pHub->BtnName << endl;
160BOOL vrpn_Tracker_G4::Connect(VOID)
162 if (!(pdiG4.CnxReady()))
164 if(pdiG4.ConnectG4(srcCalPath)){
165 cout<<
"G4: Connected\r\n";
169 cout<<
"G4: Already Connected\r\n";
172 return pdiG4.CnxReady();
176VOID vrpn_Tracker_G4::Disconnect(VOID)
179 if (!(pdiG4.CnxReady()))
181 cout <<
"G4: Already disconnected\r\n";
186 cout<<
"G4: Disconnected\r\n";
192BOOL vrpn_Tracker_G4::SetupDevice( VOID )
195 pdiG4.GetStationMap( dwStationMap );
196 while(dwStationMap == 0 && i<30)
199 pdiG4.GetStationMap( dwStationMap );
203 OriUnits = E_PDI_ORI_QUATERNION;
204 pdiG4.SetPNOOriUnits( OriUnits );
206 PosUnits = E_PDI_POS_METER;
207 pdiG4.SetPNOPosUnits( PosUnits );
209 pdiG4.SetPnoBuffer( pMotionBuf, VRPN_PDI_BUFFER_SIZE );
221 pch = strtok (pcmd,
"\n");
224 pcmd += strlen(pch) + 1;
227 pch = strtok (pcmd,
"\n");
244 if(dwStationMap != 0)
252VOID vrpn_Tracker_G4::UpdateStationMap( VOID )
254 pdiG4.GetStationMap( dwStationMap );
255 printf(
"Set GetStationMap Result: %s\r\n", pdiG4.GetLastResultStr() );
261void vrpn_Tracker_G4::sendCommand(
char *scmd)
263 char command = scmd[0];
264 printf(
"G4: Received Command: %s\n",scmd);
268 DoBoresightCmd(&scmd[1]);
279 DoIncrementCmd(scmd);
282 DoTipOffsetCmd(scmd);
296 printf(
"\tIgnoring 'U' command: VRPN Position Units standard is Meters.\r\n");
309 printf(
"\tIgnoring 'O' command: VRPN Orientation standard is Quaternion XYZW.\r\n");
312 printf(
"\tUnrecognized Command: %c\r\n", scmd[1]);
317#define CMD_ACTION_SET 1
318#define CMD_ACTION_RESET 2
319void vrpn_Tracker_G4::DoBoresightCmd(
char *scmd)
322 PDIori threeVec = {0.0,0.0,0.0};
323 PDI4vec fourVec = {1.0,0.0,0.0,0.0};
325 char delims[] =
",\n";
342 pAct = strtok(pArgs,comma);
349 nAction = pAct[0] -
'0';
351 pHub = strtok(NULL,comma);
358 nHub = (pHub[0] ==
'*') ? -1 : atoi(pHub);
360 pSens = strtok(NULL, delims);
367 nSensor = (pSens[0] ==
'*') ? -1 : atoi(pSens);
369 pRef = strtok(NULL, eol);
372 nParams += sscanf( pRef,
"%f,%f,%f,%f", &fP[1],&fP[2],&fP[3],&fP[0] );
380 printf(
"\tERROR: Invalid Boresight Command Syntax : B%s\r\n", scmd);
386 case (CMD_ACTION_SET):
389 pdiG4.SetSBoresight(nHub, nSensor, fP);
390 printf(
"\tSet Boresight Result: %s\r\n", pdiG4.GetLastResultStr() );
392 else if (nParams == 3)
394 pdiG4.SetSBoresight(nHub, nSensor, fourVec);
395 printf(
"\tSet Boresight Result: %s\r\n", pdiG4.GetLastResultStr() );
399 printf(
"\tERROR: Unexpected Boresight Argument count: %d\r\n", nParams);
402 case (CMD_ACTION_RESET):
403 pdiG4.ResetSBoresight(nHub, nSensor);
404 printf(
"\tReset Boresight Result: %s\r\n", pdiG4.GetLastResultStr() );
407 printf(
"\tERROR: Unrecognized Boresight Action: %s\r\n", pAct);
414void vrpn_Tracker_G4::DoFilterCmd(
char *scmd)
416 char delims[] =
",\n\r\\";
419 char * pArgs = &scmd[1];
432 float F=0, FLow=0, FHigh=0, Factor=0;
434 char * pLevelName = 0;
436 pAct = strtok(pArgs,comma);
443 nAction = pAct[0] -
'0';
446 pHub = strtok(NULL,delims);
453 nHub = (pHub[0] ==
'*') ? -1 : atoi(pHub);
456 if (nAction == CMD_ACTION_SET)
458 pLev = strtok(NULL, delims);
468 if (nLev == E_PDI_FILTER_CUSTOM)
470 pCust = strtok(NULL, eol);
477 nParams += sscanf( pCust,
"%f,%f,%f,%f", &f.m_fSensitivity,
480 &f.m_fMaxTransRate );
483 printf(
"\tERROR: Unexpected Filter Argument count: %d\r\n", nParams);
495 printf(
"\tERROR: Invalid Filter Command Syntax: %s\r\n", scmd);
499 if (nLev == E_PDI_FILTER_NONE)
501 nAction = CMD_ACTION_RESET;
506 case (CMD_ACTION_SET):
507 SetFilterPreset( nLev, f, &pLevelName);
510 pdiG4.SetHPosFilter( nHub, f);
514 pdiG4.SetHAttFilter( nHub, f);
516 printf(
"\tSet Filter Cmd %c %s Result: %s\r\n", cmd, pLevelName, pdiG4.GetLastResultStr() );
519 case (CMD_ACTION_RESET):
522 pdiG4.ResetHPosFilter( nHub );
526 pdiG4.ResetHAttFilter( nHub );
528 printf(
"\tReset Filter Cmd %c Result: %s\r\n", cmd, pdiG4.GetLastResultStr() );
532 printf(
"\tERROR: Unrecognized Filter Action: %c\r\n", *pAct);
539CPDIfilter FilterPresets[4] =
541 CPDIfilter(0.0f, 1.0f, 0.0f, 0.0f),
542 CPDIfilter(0.2f, 0.2f, 0.8f, 0.95f),
543 CPDIfilter(0.05f, 0.05f, 0.8f, 0.95f),
544 CPDIfilter(0.02f, 0.02f, 0.8f, 0.95f)
546char * PresetNames[5] =
548 "NONE",
"LIGHT",
"MEDIUM",
"HEAVY",
"CUSTOM"
551void vrpn_Tracker_G4::SetFilterPreset(
int nLev, CPDIfilter & f,
char **pLevName )
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];
562 case E_PDI_FILTER_CUSTOM:
563 *pLevName = PresetNames[nLev];
566 f = FilterPresets[E_PDI_FILTER_HEAVY];
567 *pLevName = PresetNames[E_PDI_FILTER_HEAVY];
572void vrpn_Tracker_G4::DoFORCmd(
char *scmd )
574 char delims[] =
",\n\r\\";
577 char * pArgs = &scmd[1];
590 pAct = strtok(pArgs,comma);
597 nAction = pAct[0] -
'0';
600 if (nAction == CMD_ACTION_SET)
602 pFOR = strtok( NULL, eol );
609 nParams += sscanf( pFOR,
"%f,%f,%f", &pos[0], &pos[1], &pos[2] );
614 nParams += sscanf( pFOR,
"%f,%f,%f,%f", &qtrn[1], &qtrn[2], &qtrn[3], &qtrn[0] );
618 if (nParams != nParamSpec)
620 printf(
"\tERROR: Unexpected Frame of Reference %c Argument count: %d\r\n", cmd, nParams);
628 printf(
"\tERROR: Invalid Frame of Reference Command Syntax: %s\r\n", scmd);
634 case (CMD_ACTION_SET):
637 pdiG4.SetFrameOfRefTRANS( pos );
641 pdiG4.SetFrameOfRefROT( qtrn );
643 printf(
"\tSet Frame of Ref %c Result: %s\r\n", cmd, pdiG4.GetLastResultStr() );
646 case (CMD_ACTION_RESET):
649 pdiG4.ResetFrameOfRefTRANS();
653 pdiG4.ResetFrameOfRefROT();
655 printf(
"\tReset Frams of Ref Cmd %c Result: %s\r\n", cmd, pdiG4.GetLastResultStr() );
659 printf(
"\tERROR: Unrecognized Frame Of Reference Action: %c\r\n", *pAct);
669void vrpn_Tracker_G4::DoIncrementCmd(
char *scmd)
673 char delims[] =
" ,\r\n\\";
676 char *pArgs = &scmd[1];
690 float fPosIncr, fOriIncr;
693 pAct = strtok(pArgs,comma);
700 nAction = pAct[0] -
'0';
702 pHub = strtok(NULL,comma);
709 nHub = (pHub[0] ==
'*') ? -1 : atoi(pHub);
711 pSens = strtok(NULL, delims);
718 nSensor = (pSens[0] ==
'*') ? -1 : atoi(pSens);
721 if (nAction == CMD_ACTION_SET)
723 pIncrs = strtok(NULL, eol);
730 nParams += sscanf( pIncrs,
"%f,%f", &fPosIncr, &fOriIncr );
733 if (nParams != nParamSpec)
735 printf(
"\tERROR: Unexpected Increment Cmd Argument count: %d\r\n", nParams);
745 printf(
"\tERROR: Invalid Increment Command Syntax : %s\r\n", scmd);
749 pdiG4.SetPNOOriUnits(E_PDI_ORI_EULER_DEGREE);
750 printf(
"\tSet Ori Units DEGREES Result: %s\r\n", pdiG4.GetLastResultStr() );
753 case (CMD_ACTION_SET):
754 pdiG4.SetSIncrement(nHub, nSensor, fPosIncr, fOriIncr);
755 printf(
"\tSet Increment Result: %s\r\n", pdiG4.GetLastResultStr() );
758 case (CMD_ACTION_RESET):
759 pdiG4.ResetSIncrement(nHub, nSensor);
760 printf(
"\tReset Increment Result: %s\r\n", pdiG4.GetLastResultStr() );
764 printf(
"\tERROR: Unrecognized Increment Action: %s\r\n", pAct);
767 pdiG4.SetPNOOriUnits(E_PDI_ORI_QUATERNION);
768 printf(
"\tSet Ori Units QUATERNION Result: %s\r\n", pdiG4.GetLastResultStr() );
774void vrpn_Tracker_G4::DoTipOffsetCmd(
char *scmd)
778 char delims[] =
" ,\n\r\\";
781 char *pArgs = &scmd[1];
798 pAct = strtok(pArgs,comma);
805 nAction = pAct[0] -
'0';
807 pHub = strtok(NULL,comma);
814 nHub = (pHub[0] ==
'*') ? -1 : atoi(pHub);
816 pSens = strtok(NULL, delims);
823 nSensor = (pSens[0] ==
'*') ? -1 : atoi(pSens);
826 if (nAction == CMD_ACTION_SET)
828 pTO = strtok(NULL, eol);
835 nParams += sscanf( pTO,
"%f,%f,%f", &pos[0], &pos[1], &pos[2] );
838 if (nParams != nParamSpec)
840 printf(
"\tERROR: Unexpected Tip Offset Cmd Argument count: %d\r\n", nParams);
850 printf(
"\tERROR: Invalid Tip Offset Command Syntax : %s\r\n", scmd);
856 case (CMD_ACTION_SET):
857 pdiG4.SetSTipOffset(nHub, nSensor, pos);
858 printf(
"\tSet Tip Offset Result: %s\r\n", pdiG4.GetLastResultStr() );
860 case (CMD_ACTION_RESET):
861 pdiG4.ResetSTipOffset(nHub, nSensor);
862 printf(
"\tReset Tip Offset Result: %s\r\n", pdiG4.GetLastResultStr() );
865 printf(
"\tERROR: Unrecognized Tip Offset Action: %s\r\n", pAct);
875BOOL vrpn_Tracker_G4::StartCont( VOID )
877 cout<<
"G4: Start Continuous Mode\r\n";
881 if (!(pdiG4.StartContPnoG4(hwnd)))
895BOOL vrpn_Tracker_G4::StopCont( VOID )
897 cout<<
"G4: Stop Continuous Mode\r\n";
900 if (!(pdiG4.StopContPnoG4()))
910 ::ResetEvent(hContEvent);
916BOOL vrpn_Tracker_G4::DisplayCont( timeval ct )
924 if (!(pdiG4.LastPnoPtr(pBuf, dwSize)))
927 else if ((pBuf == 0) || (dwSize == 0))
930 else if (pLastBuf && (pBuf > pLastBuf))
932 ParseG4NativeFrame( pLastBuf+dwLastSize, dwSize+(pBuf-pLastBuf-dwLastSize), ct );
937 else if (pBuf != pLastBuf)
940 cout <<
"wrapped" << endl;
944 ParseG4NativeFrame( pBuf, dwSize, ct );
954BOOL vrpn_Tracker_G4::DisplaySingle( timeval ct )
960 if (!(pdiG4.ReadSinglePnoBufG4(pBuf, dwSize)))
964 else if ((pBuf == 0) || (dwSize == 0))
969 ParseG4NativeFrame( pBuf, dwSize, ct );
979void vrpn_Tracker_G4::ParseG4NativeFrame( PBYTE pBuf, DWORD dwSize, timeval current_time )
984 LPG4_HUBDATA pHubFrame;
988 pHubFrame = (LPG4_HUBDATA)(&pBuf[dw]);
990 dw +=
sizeof(G4_HUBDATA);
992 UINT nHubID = pHubFrame->nHubID;
993 UINT nFrameNum = pHubFrame->nFrameCount;
994 UINT nSensorMap = pHubFrame->dwSensorMap;
995 UINT nDigIO = pHubFrame->dwDigIO;
997 HUBMAP_ENTRY * pH = 0;
1000 if (m_pHMap && (pH = m_pHMap->Find( nHubID )) && (nButtons = pH->nBtnCount) )
1002 for (
int i=0; i<nButtons; i++)
1004 pH->pBtnSrv->set_button(i, (nDigIO & (1<<i)) >> i);
1005 pH->pBtnSrv->mainloop();
1011 for (
int j=0; j<G4_MAX_SENSORS_PER_HUB; j++)
1013 if (((nSensMask << j) & nSensorMap) != 0)
1015 G4_SENSORDATA * pSD = &(pHubFrame->sd[j]);
1016 d_sensor = (nHubID*10)+j;
1018 pos[0] = pSD->pos[0];
1019 pos[1] = pSD->pos[1];
1020 pos[2] = pSD->pos[2];
1022 quat[0] = pSD->ori[1];
1023 quat[1] = pSD->ori[2];
1024 quat[2] = pSD->ori[3];
1025 quat[3] = pSD->ori[0];
1028 timestamp.tv_sec = current_time.tv_sec;
1029 timestamp.tv_usec = current_time.tv_usec;
1033 len = encode_to(msgbuf);
1034 d_connection->pack_message(len, timestamp,
1035 position_m_id, d_sender_id, msgbuf,
1047vrpn_Tracker_FastrakPDI::vrpn_Tracker_FastrakPDI (
const char * name,
vrpn_Connection *cn,
1048 vrpn_float64 Hz,
const char * rcmd,
unsigned int nStylusMap) :
1050 , m_nStylusMap(nStylusMap)
1052 , m_nFrameSize(nStylusMap?33:31)
1054 cmd = (
char*)(rcmd);
1055 register_server_handlers();
1056 if(!(Initialize())){
1059 else if (nStylusMap & !(InitStylusBtns()))
1063 else if(!(Connect())){
1066 else if(!(SetupDevice())){
1068 cout <<
"FasTrakPDI: Device setup failed\r\n";
1070 else if(!(StartCont())){
1072 cout <<
"FastrakPDI: Failed to enter continuous mode\r\n";
1076 cout <<
"FastrakPDI: Initialization Complete\r\n";
1082vrpn_Tracker_FastrakPDI::~vrpn_Tracker_FastrakPDI(
void)
1088 for (
int i=0; i<FT_MAX_SENSORS; i++)
1090 if (FTstylusBtns[i]) {
1092 delete FTstylusBtns[i];
1094 fprintf(stderr,
"vrpn_Tracker_FastrakPDI::~vrpn_Tracker_FastrakPDI(): delete failed\n");
1102VOID vrpn_Tracker_FastrakPDI::mainloop()
1104 struct timeval current_time;
1112 DisplayCont(current_time);
1121int vrpn_Tracker_FastrakPDI::encode_to(
char *buf)
1141 return 1000 - buflen;
1146BOOL vrpn_Tracker_FastrakPDI::Initialize(VOID)
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;
1153 memset( FTstylusBtns, 0,
sizeof(FTstylusBtns));
1157 dwOverflowCount = 0;
1160 pdiDev.Trace(TRUE, 5);
1168BOOL vrpn_Tracker_FastrakPDI::InitStylusBtns()
1172 for (
int i=0; i<FT_MAX_SENSORS; i++)
1174 if (((1<<i) & m_nStylusMap) != 0)
1177 sprintf( btnName,
"%sStylus%d", d_servicename, i+1);
1181 cout <<
"Cannot create button device " << btnName << endl;
1186 cout <<
"Button device " << btnName <<
" created." << endl;
1194BOOL vrpn_Tracker_FastrakPDI::Connect( VOID )
1196 if (!(pdiDev.CnxReady()))
1198 pdiDev.SetSerialIF( &pdiSer );
1202 eType = pdiDev.DiscoverCnx();
1204 while (eType != PI_CNX_USB && eType != PI_CNX_SERIAL && attempts < 10){
1208 cout <<
"FastrakPDI: USB Connection\r\n";
1212 cout <<
"FastrakPDI: Serial Connection\r\n";
1216 printf(
"FastrakPDI: %s\r\n", pdiDev.GetLastResultStr() );
1217 eType = pdiDev.DiscoverCnx();
1225 num_sensors = pdiDev.StationCount();
1228 pdiDev.GetBITErrs( cBE );
1231 cBE.Parse( sz, 100 );
1233 if(!(cBE.IsClear()))
1234 pdiDev.ClearBITErrs();
1241 pdiMDat.Append(PDI_MODATA_STYLUS);
1243 pdiMDat.Append( PDI_MODATA_POS );
1244 pdiMDat.Append( PDI_MODATA_QTRN );
1245 pdiDev.SetSDataList( -1, pdiMDat );
1246 pdiDev.SetMetric(TRUE);
1250 pdiDev.SetPnoBuffer( pMotionBuf, VRPN_PDI_BUFFER_SIZE );
1254 bCnxReady = pdiDev.CnxReady();
1257 cout <<
"FastrakPDI: Already connected\r\n";
1265VOID vrpn_Tracker_FastrakPDI::Disconnect(VOID)
1268 if (!(pdiDev.CnxReady()))
1270 cout <<
"FastrakPDI: Already disconnected\r\n";
1274 pdiDev.Disconnect();
1275 cout <<
"FastrakPDI: Disconnected\r\n";
1281BOOL vrpn_Tracker_FastrakPDI::SetupDevice( VOID )
1284 if(strlen(pcmd) > 0){
1285 char * pch = strchr(pcmd,
'\\');
1286 while (pch != NULL){
1289 pcmd = pch +
sizeof(char);
1290 pch = strchr(pcmd,
'\\');
1296 if (isBinary == FALSE)
1297 cout <<
"FastrakPDI: Warning! Tracker is still in ASCII mode!\r\n";
1303VOID vrpn_Tracker_FastrakPDI::UpdateStationMap( VOID )
1305 pdiDev.GetStationMap( dwStationMap );
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;
1316 cout <<
"FastrakPDI: reset command ";
1318 while (scmd[i] !=
'\0'){
1320 if (scmd[i] !=
'\n' && scmd[i] !=
'\r')
1325 for (i = 0; i < strlen(scmd); i++){
1331 strncat(szCmd,
"\x0b", 1);
1334 strncat(szCmd,
"\x11", 1);
1337 strncat(szCmd,
"\x13", 1);
1340 strncat(szCmd,
"\x19", 1);
1349 if (scmd[i] !=
'>'){
1354 strncat(szCmd,
"\r\n", 1);
1363 strncat(szCmd, &scmd[i], 1);
1371 CPDIdev * pDev = (CPDIdev*) (&pdiDev);
1373 if (parseErr ==
true)
1374 cout <<
"\r\n\t<unrecognized command>\r\n";
1376 strncat(szCmd,
"\0", 1);
1380 cout <<
"\r\n\t<command ignored, use P instead>\r\n";
1383 pDev->TxtCmd(szCmd,dwRspSize,szRsp);
1384 cout <<
"\r\n\t<tracker will be set to VRPN defaults on reconnect>\r\n";
1391 pDev->TxtCmd(szCmd,dwRspSize,szRsp);
1392 if (isBinary == TRUE)
1393 pdiDev.GetMetric(isMetric);
1396 pDev->SetBinary(TRUE);
1397 pdiDev.GetMetric(isMetric);
1398 pDev->SetBinary(FALSE);
1400 if (isMetric == FALSE)
1401 cout <<
"\r\n\t<position units set to inches>\r\n";
1403 cout <<
"\r\n\t<position units set to meters>\r\n";
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";
1412 cout <<
"\r\n\t<response frames set to binary>\r\n";
1415 cout <<
"\r\n\t<pno frame format changed (use extreme caution)>";
1417 if (isBinary == TRUE)
1418 cout <<
"\r\n\t<suggestion: use ASCII response frames (reset command F)>";
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";
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')
1432 if (isBinary == TRUE)
1433 cout <<
"\r\n\t<binary response end>\r\n";
1435 cout <<
"\r\n\t<ASCII response end>\r\n";
1438 cout <<
"\r\n\t<command sent>\r\n";
1445BOOL vrpn_Tracker_FastrakPDI::StartCont( VOID )
1447 cout <<
"FastrakPDI: Start Continuous Mode\r\n";
1451 if (!(pdiDev.StartContPno(hwnd)))
1457 dwOverflowCount = 0;
1465BOOL vrpn_Tracker_FastrakPDI::StopCont( VOID )
1467 cout <<
"FastrakPDI: Stop Continuous Mode\r\n";
1470 if (!(pdiDev.StopContPno()))
1480 ::ResetEvent(hContEvent);
1486BOOL vrpn_Tracker_FastrakPDI::DisplayCont( timeval ct )
1493 if (!(pdiDev.LastPnoPtr(pBuf, dwSize)))
1496 else if ((pBuf == 0) || (dwSize == 0))
1499 else if (pLastBuf && (pBuf > pLastBuf))
1501 ParseFastrakFrame( pLastBuf+dwLastSize, dwSize+(pBuf-pLastBuf-dwLastSize), ct );
1503 dwLastSize = dwSize;
1506 else if (pBuf != pLastBuf)
1509 cout <<
"wrapped\r\n";
1512 dwLastSize = dwSize;
1513 ParseFastrakFrame( pBuf, dwSize, ct );
1522VOID vrpn_Tracker_FastrakPDI::DisplaySingle( timeval ct )
1531 if (!(pdiDev.ReadSinglePnoBuf(pBuf, dwSize)))
1535 else if ((pBuf == 0) || (dwSize == 0))
1540 ParseFastrakFrame( pBuf, dwSize, ct );
1546VOID vrpn_Tracker_FastrakPDI::ParseFastrakFrame( PBYTE pBuf, DWORD dwSize, timeval current_time )
1553 while (index < dwSize){
1555 BYTE ucSensor = pBuf[dw+1];
1556 BYTE ucInitCommand = pBuf[dw];
1557 BYTE ucErrorNum = pBuf[dw+2];
1558 d_sensor = atoi((
char*)(&ucSensor));
1561 dw += m_nHeaderSize;
1565 if (ucInitCommand !=
'0'){
1566 printf(
"FastrakPDI: received record type %x while in continuous mode, record error byte was %x \r\n", ucInitCommand, ucErrorNum);
1572 if ((m_nStylusMap & (1 << (d_sensor-1))) != 0)
1574 CHAR StyFlag = pBuf[dw+1];
1576 FTstylusBtns[d_sensor-1]->set_button(0, StyFlag -
'0');
1577 FTstylusBtns[d_sensor-1]->mainloop();
1582 PFLOAT pPno = (PFLOAT)(&pBuf[dw]);
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;
1590 pos[0] = float(pPno[0]);
1591 pos[1] = float(pPno[1]);
1592 pos[2] = float(pPno[2]);
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]);
1603 timestamp.tv_sec = current_time.tv_sec;
1604 timestamp.tv_usec = current_time.tv_usec;
1607 len = encode_to(msgbuf);
1608 d_connection->pack_message(len, timestamp,
1612 index += m_nFrameSize;
1617vrpn_Tracker_LibertyPDI::vrpn_Tracker_LibertyPDI (
const char * name,
vrpn_Connection *cn,
1618 vrpn_float64 Hz,
const char * rcmd,
unsigned int nStylusMap) :
1620 , m_nStylusMap(nStylusMap)
1622 , m_nFrameSize(nStylusMap?40:36)
1624 cmd = (
char*)(rcmd);
1625 register_server_handlers();
1626 if(!(Initialize())){
1629 else if (nStylusMap & !(InitStylusBtns()))
1633 else if(!(Connect())){
1636 else if(!(SetupDevice())){
1639 else if(!(StartCont())){
1641 cout <<
"LibertyPDI: Failed to enter continuous mode\r\n";
1645 cout <<
"LibertyPDI: Initialization Complete\r\n";
1651vrpn_Tracker_LibertyPDI::~vrpn_Tracker_LibertyPDI(
void){
1656 for (
int i=0; i<LIBERTY_MAX_SENSORS; i++)
1658 if (StylusBtns[i]) {
1660 delete StylusBtns[i];
1662 fprintf(stderr,
"vrpn_Tracker_LibertyPDI::~vrpn_Tracker_LibertyPDI(): delete failed\n");
1670VOID vrpn_Tracker_LibertyPDI::mainloop()
1672 struct timeval current_time;
1680 DisplayCont(current_time);
1689int vrpn_Tracker_LibertyPDI::encode_to(
char *buf)
1709 return 1000 - buflen;
1714BOOL vrpn_Tracker_LibertyPDI::Initialize(VOID){
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;
1719 memset( StylusBtns, 0,
sizeof(StylusBtns));
1723 dwOverflowCount = 0;
1727 pdiDev.Trace(TRUE, 5);
1736BOOL vrpn_Tracker_LibertyPDI::InitStylusBtns()
1740 for (
int i=0; i<LIBERTY_MAX_SENSORS; i++)
1742 if (((1<<i) & m_nStylusMap) != 0)
1745 sprintf( btnName,
"%sStylus%d", d_servicename, i+1);
1749 cout <<
"Cannot create button device " << btnName << endl;
1754 cout <<
"Button device " << btnName <<
" created." << endl;
1763BOOL vrpn_Tracker_LibertyPDI::Connect( VOID )
1765 if (!(pdiDev.CnxReady()))
1767 pdiDev.SetSerialIF( &pdiSer );
1771 eType = pdiDev.DiscoverCnx();
1773 while (eType != PI_CNX_USB && eType != PI_CNX_SERIAL && attempts < 10){
1777 cout <<
"LibertyPDI: USB Connection\r\n";
1781 cout <<
"LibertyPDI: Serial Connection\r\n";
1785 printf(
"LibertyPDI: %s\r\n", pdiDev.GetLastResultStr() );
1786 eType = pdiDev.DiscoverCnx();
1794 num_sensors = pdiDev.StationCount();
1797 pdiDev.GetBITErrs( cBE );
1800 cBE.Parse( sz, 100 );
1802 if(!(cBE.IsClear()))
1803 pdiDev.ClearBITErrs();
1809 pdiMDat.Append(PDI_MODATA_STYLUS);
1811 pdiMDat.Append( PDI_MODATA_POS );
1812 pdiMDat.Append( PDI_MODATA_QTRN );
1813 pdiDev.SetSDataList( -1, pdiMDat );
1815 pdiDev.SetMetric(TRUE);
1821 pdiDev.SetPnoBuffer( pMotionBuf, VRPN_PDI_BUFFER_SIZE );
1825 bCnxReady = pdiDev.CnxReady();
1828 cout <<
"LibertyPDI: Already connected\r\n";
1836VOID vrpn_Tracker_LibertyPDI::Disconnect(VOID)
1839 if (!(pdiDev.CnxReady()))
1841 cout <<
"LibertyPDI: Already disconnected\r\n";
1845 pdiDev.Disconnect();
1846 cout <<
"LibertyPDI: Disconnected\r\n";
1852BOOL vrpn_Tracker_LibertyPDI::SetupDevice( VOID )
1855 if(strlen(pcmd) > 0){
1856 char * pch = strchr(pcmd,
'\\');
1857 while (pch != NULL){
1860 pcmd = pch +
sizeof(char);
1861 pch = strchr(pcmd,
'\\');
1867 if (isBinary == FALSE)
1868 cout <<
"LibertyPDI: Warning! Tracker is still in ASCII mode!\r\n";
1874VOID vrpn_Tracker_LibertyPDI::UpdateStationMap( VOID )
1876 pdiDev.GetStationMap( dwStationMap );
1880VOID vrpn_Tracker_LibertyPDI::SendCommand(
char *scmd)
1882 char szCmd[PI_MAX_CMD_BUF_LEN] =
"\0";
1883 DWORD dwRspSize = 5000;
1884 char szRsp[5000] =
"\0";
1885 bool parseErr =
false;
1888 cout <<
"LibertyPDI: reset command ";
1890 while (scmd[i] !=
'\0'){
1892 if (scmd[i] !=
'\n' && scmd[i] !=
'\r')
1898 for (i = 0; i < strlen(scmd); i++){
1904 strncat(szCmd,
"\x01", 1);
1907 strncat(szCmd,
"\x02", 1);
1910 strncat(szCmd,
"\x04", 1);
1913 strncat(szCmd,
"\x05", 1);
1916 strncat(szCmd,
"\x06", 1);
1919 strncat(szCmd,
"\x07", 1);
1922 strncat(szCmd,
"\x0b", 1);
1925 strncat(szCmd,
"\x0c", 1);
1928 strncat(szCmd,
"\x0e", 1);
1931 strncat(szCmd,
"\x0f", 1);
1934 strncat(szCmd,
"\x10", 1);
1937 strncat(szCmd,
"\x12", 1);
1940 strncat(szCmd,
"\x13", 1);
1943 strncat(szCmd,
"\x14", 1);
1946 strncat(szCmd,
"\x15", 1);
1949 strncat(szCmd,
"\x16", 1);
1952 strncat(szCmd,
"\x17", 1);
1955 strncat(szCmd,
"\x18", 1);
1958 strncat(szCmd,
"\x19", 1);
1961 strncat(szCmd,
"\x1a", 1);
1970 if (scmd[i] !=
'>'){
1975 strncat(szCmd,
"\r\n", 1);
1984 strncat(szCmd, &scmd[i], 1);
1988 if (parseErr ==
true)
1989 cout <<
"\r\n\t<unrecognized command>\r\n";
1991 strncat(szCmd,
"\0", 1);
1995 cout <<
"\r\n\t<command ignored, use P instead>\r\n";
1998 pdiDev.TxtCmd(szCmd,dwRspSize,szRsp);
1999 cout <<
"\r\n\t<tracker will be set to VRPN defaults on reconnect>\r\n";
2005 pdiDev.TxtCmd(szCmd,dwRspSize,szRsp);
2006 if (isBinary == TRUE)
2007 pdiDev.GetMetric(isMetric);
2009 pdiDev.SetBinary(TRUE);
2010 pdiDev.GetMetric(isMetric);
2011 pdiDev.SetBinary(FALSE);
2013 if (isMetric == FALSE)
2014 cout <<
"\r\n\t<position units set to inches>\r\n";
2016 cout <<
"\r\n\t<position units set to meters>\r\n";
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";
2024 cout <<
"\r\n\t<response frames set to binary>\r\n";
2027 pdiDev.TxtCmd(szCmd,dwRspSize,szRsp);
2028 cout <<
"\r\n\t<pno frame format changed (use extreme caution)>\r\n";
2031 pdiDev.TxtCmd(szCmd,dwRspSize,szRsp);
2032 if (isBinary == TRUE && dwRspSize != 0 && dwRspSize != 5000){
2033 char * pRsp = szRsp;
2035 if (*pRsp == 0x00 || *pRsp == 0x20)
2036 cout <<
"\r\n\t<binary response contained no error>\r\n";
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')
2046 cout <<
"\r\n\t<binary error response end>\r\n";
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')
2057 cout <<
"\r\n\t<ASCII response end>\r\n";
2060 cout <<
"\r\n\t<command sent>\r\n";
2066BOOL vrpn_Tracker_LibertyPDI::StartCont( VOID )
2068 cout <<
"LibertyPDI: Start Continuous Mode\r\n";
2072 if (!(pdiDev.StartContPno(hwnd)))
2078 dwOverflowCount = 0;
2086BOOL vrpn_Tracker_LibertyPDI::StopCont( VOID )
2088 cout <<
"LibertyPDI: Stop Continuous Mode\r\n";
2091 if (!(pdiDev.StopContPno()))
2101 ::ResetEvent(hContEvent);
2107BOOL vrpn_Tracker_LibertyPDI::DisplayCont( timeval ct )
2114 if (!(pdiDev.LastPnoPtr(pBuf, dwSize)))
2117 else if ((pBuf == 0) || (dwSize == 0))
2120 else if (pLastBuf && (pBuf > pLastBuf))
2122 ParseLibertyFrame( pLastBuf+dwLastSize, dwSize+(pBuf-pLastBuf-dwLastSize), ct );
2124 dwLastSize = dwSize;
2127 else if (pBuf != pLastBuf)
2130 cout <<
"wrapped\n";
2133 dwLastSize = dwSize;
2134 ParseLibertyFrame( pBuf, dwSize, ct );
2143VOID vrpn_Tracker_LibertyPDI::DisplaySingle( timeval ct )
2152 if (!(pdiDev.ReadSinglePnoBuf(pBuf, dwSize)))
2156 else if ((pBuf == 0) || (dwSize == 0))
2161 ParseLibertyFrame( pBuf, dwSize, ct );
2167VOID vrpn_Tracker_LibertyPDI::ParseLibertyFrame( PBYTE pBuf, DWORD dwSize, timeval current_time )
2174 while (index < dwSize){
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);
2183 dw += m_nHeaderSize;
2187 if (ucInitCommand !=
'C' && ucInitCommand !=
'P'){
2188 printf(
"LibertyPDI: received command %x while in continuous mode, tracker response was %x \r\n", ucInitCommand, ucErrorNum);
2193 if ((m_nStylusMap & (1 << (d_sensor-1))) != 0)
2195 DWORD m_dwStylus = *((DWORD*)&pBuf[dw]);
2197 StylusBtns[d_sensor-1]->set_button(0, m_dwStylus);
2198 StylusBtns[d_sensor-1]->mainloop();
2200 dw +=
sizeof(DWORD);;
2204 PFLOAT pPno = (PFLOAT)(&pBuf[dw]);
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;
2212 pos[0] = float(pPno[0]);
2213 pos[1] = float(pPno[1]);
2214 pos[2] = float(pPno[2]);
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]);
2224 timestamp.tv_sec = current_time.tv_sec;
2225 timestamp.tv_usec = current_time.tv_usec;
2228 len = encode_to(msgbuf);
2229 d_connection->pack_message(len, timestamp,
2233 index += m_nFrameSize;
Generic connection class not specific to the transport mechanism.
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
const int vrpn_TRACKER_FAIL
const int vrpn_TRACKER_SYNCING
const int vrpn_TRACKER_REPORT_READY
class VRPN_API vrpn_Button_Server