10 #define CM_TO_METERS (1/100.0)
13 vrpn_Tracker_G4::vrpn_Tracker_G4 (
const char *name,
vrpn_Connection *c,
const char *filepath, vrpn_float64 Hz,
const char *rcmd, vrpn_Tracker_G4_HubMap * pHMap) :
16 srcCalPath = LPCTSTR(filepath);
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";
51 vrpn_Tracker_G4::~vrpn_Tracker_G4(
void){
64 void vrpn_Tracker_G4::mainloop()
66 struct timeval current_time;
74 DisplayCont(current_time);
83 int vrpn_Tracker_G4::encode_to(
char *buf)
103 return 1000 - buflen;
109 BOOL vrpn_Tracker_G4::Initialize(VOID){
117 pdiG4.Trace(TRUE, 5);
126 BOOL vrpn_Tracker_G4::InitDigIOBtns()
133 HUBMAP_ENTRY * pHub = m_pHMap->Begin();
139 pHub->pBtnSrv =
new vrpn_Button_Server(pHub->BtnName, d_connection, pHub->nBtnCount );
140 if (pHub->pBtnSrv == NULL)
142 cout <<
"Cannot create button device " << pHub->BtnName << endl;
155 BOOL vrpn_Tracker_G4::Connect(VOID)
157 if (!(pdiG4.CnxReady()))
159 if(pdiG4.ConnectG4(srcCalPath)){
160 cout<<
"G4: Connected\r\n";
164 cout<<
"G4: Already Connected\r\n";
167 return pdiG4.CnxReady();
171 VOID vrpn_Tracker_G4::Disconnect(VOID)
174 if (!(pdiG4.CnxReady()))
176 cout <<
"G4: Already disconnected\r\n";
181 cout<<
"G4: Disconnected\r\n";
187 BOOL vrpn_Tracker_G4::SetupDevice( VOID )
190 pdiG4.GetStationMap( dwStationMap );
191 while(dwStationMap == 0 && i<30)
194 pdiG4.GetStationMap( dwStationMap );
198 OriUnits = E_PDI_ORI_QUATERNION;
199 pdiG4.SetPNOOriUnits( OriUnits );
201 PosUnits = E_PDI_POS_METER;
202 pdiG4.SetPNOPosUnits( PosUnits );
204 pdiG4.SetPnoBuffer( pMotionBuf, VRPN_PDI_BUFFER_SIZE );
214 pch = strtok (pcmd,
"\n");
217 pcmd += strlen(pch) + 1;
220 pch = strtok (pcmd,
"\n");
237 if(dwStationMap != 0)
245 VOID vrpn_Tracker_G4::UpdateStationMap( VOID )
247 pdiG4.GetStationMap( dwStationMap );
248 printf(
"Set GetStationMap Result: %s\r\n", pdiG4.GetLastResultStr() );
254 void vrpn_Tracker_G4::sendCommand(
char *scmd)
256 char command = scmd[0];
257 printf(
"G4: Received Command: %s\n",scmd);
261 DoBoresightCmd(&scmd[1]);
272 DoIncrementCmd(scmd);
275 DoTipOffsetCmd(scmd);
289 printf(
"\tIgnoring 'U' command: VRPN Position Units standard is Meters.\r\n");
302 printf(
"\tIgnoring 'O' command: VRPN Orientation standard is Quaternion XYZW.\r\n");
305 printf(
"\tUnrecognized Command: %c\r\n", scmd[1]);
310 #define CMD_ACTION_SET 1
311 #define CMD_ACTION_RESET 2
312 void vrpn_Tracker_G4::DoBoresightCmd(
char *scmd)
315 PDIori threeVec = {0.0,0.0,0.0};
316 PDI4vec fourVec = {1.0,0.0,0.0,0.0};
318 char delims[] =
",\n";
335 pAct = strtok(pArgs,comma);
342 nAction = pAct[0] -
'0';
344 pHub = strtok(NULL,comma);
351 nHub = (pHub[0] ==
'*') ? -1 : atoi(pHub);
353 pSens = strtok(NULL, delims);
360 nSensor = (pSens[0] ==
'*') ? -1 : atoi(pSens);
362 pRef = strtok(NULL, eol);
365 nParams += sscanf( pRef,
"%f,%f,%f,%f", &fP[1],&fP[2],&fP[3],&fP[0] );
373 printf(
"\tERROR: Invalid Boresight Command Syntax : B%s\r\n", scmd);
379 case (CMD_ACTION_SET):
382 pdiG4.SetSBoresight(nHub, nSensor, fP);
383 printf(
"\tSet Boresight Result: %s\r\n", pdiG4.GetLastResultStr() );
385 else if (nParams == 3)
387 pdiG4.SetSBoresight(nHub, nSensor, fourVec);
388 printf(
"\tSet Boresight Result: %s\r\n", pdiG4.GetLastResultStr() );
392 printf(
"\tERROR: Unexpected Boresight Argument count: %d\r\n", nParams);
395 case (CMD_ACTION_RESET):
396 pdiG4.ResetSBoresight(nHub, nSensor);
397 printf(
"\tReset Boresight Result: %s\r\n", pdiG4.GetLastResultStr() );
400 printf(
"\tERROR: Unrecognized Boresight Action: %s\r\n", pAct);
407 void vrpn_Tracker_G4::DoFilterCmd(
char *scmd)
409 char delims[] =
",\n\r\\";
412 char * pArgs = &scmd[1];
425 float F=0, FLow=0, FHigh=0, Factor=0;
427 char * pLevelName = 0;
429 pAct = strtok(pArgs,comma);
436 nAction = pAct[0] -
'0';
439 pHub = strtok(NULL,delims);
446 nHub = (pHub[0] ==
'*') ? -1 : atoi(pHub);
449 if (nAction == CMD_ACTION_SET)
451 pLev = strtok(NULL, delims);
461 if (nLev == E_PDI_FILTER_CUSTOM)
463 pCust = strtok(NULL, eol);
470 nParams += sscanf( pCust,
"%f,%f,%f,%f", &f.m_fSensitivity,
473 &f.m_fMaxTransRate );
476 printf(
"\tERROR: Unexpected Filter Argument count: %d\r\n", nParams);
488 printf(
"\tERROR: Invalid Filter Command Syntax: %s\r\n", scmd);
492 if (nLev == E_PDI_FILTER_NONE)
494 nAction = CMD_ACTION_RESET;
499 case (CMD_ACTION_SET):
500 SetFilterPreset( nLev, f, &pLevelName);
503 pdiG4.SetHPosFilter( nHub, f);
507 pdiG4.SetHAttFilter( nHub, f);
509 printf(
"\tSet Filter Cmd %c %s Result: %s\r\n", cmd, pLevelName, pdiG4.GetLastResultStr() );
512 case (CMD_ACTION_RESET):
515 pdiG4.ResetHPosFilter( nHub );
519 pdiG4.ResetHAttFilter( nHub );
521 printf(
"\tReset Filter Cmd %c Result: %s\r\n", cmd, pdiG4.GetLastResultStr() );
525 printf(
"\tERROR: Unrecognized Filter Action: %c\r\n", pAct);
532 CPDIfilter FilterPresets[4] =
534 CPDIfilter(0.0f, 1.0f, 0.0f, 0.0f),
535 CPDIfilter(0.2f, 0.2f, 0.8f, 0.95f),
536 CPDIfilter(0.05f, 0.05f, 0.8f, 0.95f),
537 CPDIfilter(0.02f, 0.02f, 0.8f, 0.95f)
539 char * PresetNames[5] =
541 "NONE",
"LIGHT",
"MEDIUM",
"HEAVY",
"CUSTOM"
544 void vrpn_Tracker_G4::SetFilterPreset(
int nLev, CPDIfilter & f,
char **pLevName )
548 case E_PDI_FILTER_NONE:
549 case E_PDI_FILTER_LIGHT:
550 case E_PDI_FILTER_MED:
551 case E_PDI_FILTER_HEAVY:
552 f = FilterPresets[nLev];
553 *pLevName = PresetNames[nLev];
555 case E_PDI_FILTER_CUSTOM:
556 *pLevName = PresetNames[nLev];
559 f = FilterPresets[E_PDI_FILTER_HEAVY];
560 *pLevName = PresetNames[E_PDI_FILTER_HEAVY];
565 void vrpn_Tracker_G4::DoFORCmd(
char *scmd )
567 char delims[] =
",\n\r\\";
570 char * pArgs = &scmd[1];
583 pAct = strtok(pArgs,comma);
590 nAction = pAct[0] -
'0';
593 if (nAction == CMD_ACTION_SET)
595 pFOR = strtok( NULL, eol );
602 nParams += sscanf( pFOR,
"%f,%f,%f", &pos[0], &pos[1], &pos[2] );
607 nParams += sscanf( pFOR,
"%f,%f,%f,%f", &qtrn[1], &qtrn[2], &qtrn[3], &qtrn[0] );
611 if (nParams != nParamSpec)
613 printf(
"\tERROR: Unexpected Frame of Reference %c Argument count: %d\r\n", cmd, nParams);
621 printf(
"\tERROR: Invalid Frame of Reference Command Syntax: %s\r\n", scmd);
627 case (CMD_ACTION_SET):
630 pdiG4.SetFrameOfRefTRANS( pos );
634 pdiG4.SetFrameOfRefROT( qtrn );
636 printf(
"\tSet Frame of Ref %c Result: %s\r\n", cmd, pdiG4.GetLastResultStr() );
639 case (CMD_ACTION_RESET):
642 pdiG4.ResetFrameOfRefTRANS();
646 pdiG4.ResetFrameOfRefROT();
648 printf(
"\tReset Frams of Ref Cmd %c Result: %s\r\n", cmd, pdiG4.GetLastResultStr() );
652 printf(
"\tERROR: Unrecognized Frame Of Reference Action: %c\r\n", pAct);
662 void vrpn_Tracker_G4::DoIncrementCmd(
char *scmd)
666 char delims[] =
" ,\r\n\\";
669 char *pArgs = &scmd[1];
683 float fPosIncr, fOriIncr;
686 pAct = strtok(pArgs,comma);
693 nAction = pAct[0] -
'0';
695 pHub = strtok(NULL,comma);
702 nHub = (pHub[0] ==
'*') ? -1 : atoi(pHub);
704 pSens = strtok(NULL, delims);
711 nSensor = (pSens[0] ==
'*') ? -1 : atoi(pSens);
714 if (nAction == CMD_ACTION_SET)
716 pIncrs = strtok(NULL, eol);
723 nParams += sscanf( pIncrs,
"%f,%f", &fPosIncr, &fOriIncr );
726 if (nParams != nParamSpec)
728 printf(
"\tERROR: Unexpected Increment Cmd Argument count: %d\r\n", nParams);
738 printf(
"\tERROR: Invalid Increment Command Syntax : %s\r\n", scmd);
742 pdiG4.SetPNOOriUnits(E_PDI_ORI_EULER_DEGREE);
743 printf(
"\tSet Ori Units DEGREES Result: %s\r\n", pdiG4.GetLastResultStr() );
746 case (CMD_ACTION_SET):
747 pdiG4.SetSIncrement(nHub, nSensor, fPosIncr, fOriIncr);
748 printf(
"\tSet Increment Result: %s\r\n", pdiG4.GetLastResultStr() );
751 case (CMD_ACTION_RESET):
752 pdiG4.ResetSIncrement(nHub, nSensor);
753 printf(
"\tReset Increment Result: %s\r\n", pdiG4.GetLastResultStr() );
757 printf(
"\tERROR: Unrecognized Increment Action: %s\r\n", pAct);
760 pdiG4.SetPNOOriUnits(E_PDI_ORI_QUATERNION);
761 printf(
"\tSet Ori Units QUATERNION Result: %s\r\n", pdiG4.GetLastResultStr() );
767 void vrpn_Tracker_G4::DoTipOffsetCmd(
char *scmd)
771 char delims[] =
" ,\n\r\\";
774 char *pArgs = &scmd[1];
791 pAct = strtok(pArgs,comma);
798 nAction = pAct[0] -
'0';
800 pHub = strtok(NULL,comma);
807 nHub = (pHub[0] ==
'*') ? -1 : atoi(pHub);
809 pSens = strtok(NULL, delims);
816 nSensor = (pSens[0] ==
'*') ? -1 : atoi(pSens);
819 if (nAction == CMD_ACTION_SET)
821 pTO = strtok(NULL, eol);
828 nParams += sscanf( pTO,
"%f,%f,%f", &pos[0], &pos[1], &pos[2] );
831 if (nParams != nParamSpec)
833 printf(
"\tERROR: Unexpected Tip Offset Cmd Argument count: %d\r\n", nParams);
843 printf(
"\tERROR: Invalid Tip Offset Command Syntax : %s\r\n", scmd);
849 case (CMD_ACTION_SET):
850 pdiG4.SetSTipOffset(nHub, nSensor, pos);
851 printf(
"\tSet Tip Offset Result: %s\r\n", pdiG4.GetLastResultStr() );
853 case (CMD_ACTION_RESET):
854 pdiG4.ResetSTipOffset(nHub, nSensor);
855 printf(
"\tReset Tip Offset Result: %s\r\n", pdiG4.GetLastResultStr() );
858 printf(
"\tERROR: Unrecognized Tip Offset Action: %s\r\n", pAct);
868 BOOL vrpn_Tracker_G4::StartCont( VOID )
870 cout<<
"G4: Start Continuous Mode\r\n";
874 if (!(pdiG4.StartContPnoG4(hwnd)))
888 BOOL vrpn_Tracker_G4::StopCont( VOID )
890 cout<<
"G4: Stop Continuous Mode\r\n";
893 if (!(pdiG4.StopContPnoG4()))
903 ::ResetEvent(hContEvent);
909 BOOL vrpn_Tracker_G4::DisplayCont( timeval ct )
917 DWORD dwLastSize = 0;
920 if (!(pdiG4.LastPnoPtr(pBuf, dwSize)))
923 else if ((pBuf == 0) || (dwSize == 0))
926 else if (pLastBuf && (pBuf > pLastBuf))
928 ParseG4NativeFrame( pLastBuf+dwLastSize, dwSize+(pBuf-pLastBuf-dwLastSize), ct );
933 else if (pBuf != pLastBuf)
936 cout <<
"wrapped" << endl;
940 ParseG4NativeFrame( pBuf, dwSize, ct );
950 BOOL vrpn_Tracker_G4::DisplaySingle( timeval ct )
956 if (!(pdiG4.ReadSinglePnoBufG4(pBuf, dwSize)))
960 else if ((pBuf == 0) || (dwSize == 0))
965 ParseG4NativeFrame( pBuf, dwSize, ct );
975 void vrpn_Tracker_G4::ParseG4NativeFrame( PBYTE pBuf, DWORD dwSize, timeval current_time )
980 LPG4_HUBDATA pHubFrame;
984 pHubFrame = (LPG4_HUBDATA)(&pBuf[dw]);
986 dw +=
sizeof(G4_HUBDATA);
988 UINT nHubID = pHubFrame->nHubID;
989 UINT nFrameNum = pHubFrame->nFrameCount;
990 UINT nSensorMap = pHubFrame->dwSensorMap;
991 UINT nDigIO = pHubFrame->dwDigIO;
993 HUBMAP_ENTRY * pH = 0;
996 if (m_pHMap && (pH = m_pHMap->Find( nHubID )) && (nButtons = pH->nBtnCount) )
998 for (
int i=0; i<nButtons; i++)
1000 pH->pBtnSrv->set_button(i, (nDigIO & (1<<i)) >> i);
1001 pH->pBtnSrv->mainloop();
1007 for (
int j=0; j<G4_MAX_SENSORS_PER_HUB; j++)
1009 if (((nSensMask << j) & nSensorMap) != 0)
1011 G4_SENSORDATA * pSD = &(pHubFrame->sd[j]);
1012 d_sensor = (nHubID*10)+j;
1014 pos[0] = pSD->pos[0];
1015 pos[1] = pSD->pos[1];
1016 pos[2] = pSD->pos[2];
1018 quat[0] = pSD->ori[1];
1019 quat[1] = pSD->ori[2];
1020 quat[2] = pSD->ori[3];
1021 quat[3] = pSD->ori[0];
1024 timestamp.tv_sec = current_time.tv_sec;
1025 timestamp.tv_usec = current_time.tv_usec;
1029 len = encode_to(msgbuf);
1030 d_connection->pack_message(len, timestamp,
1031 position_m_id, d_sender_id, msgbuf,
1043 vrpn_Tracker_FastrakPDI::vrpn_Tracker_FastrakPDI (
const char * name,
vrpn_Connection *cn,
1044 vrpn_float64 Hz,
const char * rcmd,
unsigned int nStylusMap) :
1046 , m_nStylusMap(nStylusMap)
1048 , m_nFrameSize(nStylusMap?33:31)
1050 cmd = (
char*)(rcmd);
1052 if(!(Initialize())){
1055 else if (nStylusMap & !(InitStylusBtns()))
1059 else if(!(Connect())){
1062 else if(!(SetupDevice())){
1064 cout <<
"FasTrakPDI: Device setup failed\r\n";
1066 else if(!(StartCont())){
1068 cout <<
"FastrakPDI: Failed to enter continuous mode\r\n";
1072 cout <<
"FastrakPDI: Initialization Complete\r\n";
1078 vrpn_Tracker_FastrakPDI::~vrpn_Tracker_FastrakPDI(
void)
1084 for (
int i=0; i<FT_MAX_SENSORS; i++)
1086 if (FTstylusBtns[i])
1087 delete FTstylusBtns[i];
1092 VOID vrpn_Tracker_FastrakPDI::mainloop()
1094 struct timeval current_time;
1102 DisplayCont(current_time);
1111 int vrpn_Tracker_FastrakPDI::encode_to(
char *buf)
1131 return 1000 - buflen;
1136 BOOL vrpn_Tracker_FastrakPDI::Initialize(VOID)
1140 pos[0]=0; pos[1]=0; pos[2]=0;
1141 d_quat[0]=0; d_quat[1]=0; d_quat[2]=0; d_quat[3]=0;
1143 memset( FTstylusBtns, 0,
sizeof(FTstylusBtns));
1147 dwOverflowCount = 0;
1150 pdiDev.Trace(TRUE, 5);
1158 BOOL vrpn_Tracker_FastrakPDI::InitStylusBtns()
1162 for (
int i=0; i<FT_MAX_SENSORS; i++)
1164 if (((1<<i) & m_nStylusMap) != 0)
1167 sprintf( btnName,
"%sStylus%d", d_servicename, i+1);
1169 if (FTstylusBtns[i] == NULL)
1171 cout <<
"Cannot create button device " << btnName << endl;
1176 cout <<
"Button device " << btnName <<
" created." << endl;
1184 BOOL vrpn_Tracker_FastrakPDI::Connect( VOID )
1186 if (!(pdiDev.CnxReady()))
1188 pdiDev.SetSerialIF( &pdiSer );
1192 eType = pdiDev.DiscoverCnx();
1194 while (eType != PI_CNX_USB && eType != PI_CNX_SERIAL && attempts < 10){
1198 cout <<
"FastrakPDI: USB Connection\r\n";
1202 cout <<
"FastrakPDI: Serial Connection\r\n";
1206 printf(
"FastrakPDI: %s\r\n", pdiDev.GetLastResultStr() );
1207 eType = pdiDev.DiscoverCnx();
1215 num_sensors = pdiDev.StationCount();
1218 pdiDev.GetBITErrs( cBE );
1221 cBE.Parse( sz, 100 );
1223 if(!(cBE.IsClear()))
1224 pdiDev.ClearBITErrs();
1231 pdiMDat.Append(PDI_MODATA_STYLUS);
1233 pdiMDat.Append( PDI_MODATA_POS );
1234 pdiMDat.Append( PDI_MODATA_QTRN );
1235 pdiDev.SetSDataList( -1, pdiMDat );
1236 pdiDev.SetMetric(TRUE);
1240 pdiDev.SetPnoBuffer( pMotionBuf, VRPN_PDI_BUFFER_SIZE );
1242 bCnxReady = pdiDev.CnxReady();
1245 cout <<
"FastrakPDI: Already connected\r\n";
1253 VOID vrpn_Tracker_FastrakPDI::Disconnect(VOID)
1256 if (!(pdiDev.CnxReady()))
1258 cout <<
"FastrakPDI: Already disconnected\r\n";
1262 pdiDev.Disconnect();
1263 cout <<
"FastrakPDI: Disconnected\r\n";
1269 BOOL vrpn_Tracker_FastrakPDI::SetupDevice( VOID )
1272 if(strlen(pcmd) > 0){
1273 char * pch = strchr(pcmd,
'\\');
1274 while (pch != NULL){
1277 pcmd = pch +
sizeof(char);
1278 pch = strchr(pcmd,
'\\');
1284 if (isBinary == FALSE)
1285 cout <<
"FastrakPDI: Warning! Tracker is still in ASCII mode!\r\n";
1291 VOID vrpn_Tracker_FastrakPDI::UpdateStationMap( VOID )
1293 pdiDev.GetStationMap( dwStationMap );
1297 VOID vrpn_Tracker_FastrakPDI::SendCommand(
char *scmd){
1298 char szCmd[PI_MAX_CMD_BUF_LEN] =
"\0";
1299 DWORD dwRspSize = 5000;
1300 char szRsp[5000] =
"\0";
1301 bool parseErr =
false;
1304 cout <<
"FastrakPDI: reset command ";
1306 while (scmd[i] !=
'\0'){
1308 if (scmd[i] !=
'\n' && scmd[i] !=
'\r')
1313 for (i = 0; i < strlen(scmd); i++){
1319 strncat(szCmd,
"\x0b", 1);
1322 strncat(szCmd,
"\x11", 1);
1325 strncat(szCmd,
"\x13", 1);
1328 strncat(szCmd,
"\x19", 1);
1337 if (scmd[i] !=
'>'){
1342 strncat(szCmd,
"\r\n", 1);
1351 strncat(szCmd, &scmd[i], 1);
1359 CPDIdev * pDev = (CPDIdev*) (&pdiDev);
1361 if (parseErr ==
true)
1362 cout <<
"\r\n\t<unrecognized command>\r\n";
1364 strncat(szCmd,
"\0", 1);
1368 cout <<
"\r\n\t<command ignored, use P instead>\r\n";
1371 pDev->TxtCmd(szCmd,dwRspSize,szRsp);
1372 cout <<
"\r\n\t<tracker will be set to VRPN defaults on reconnect>\r\n";
1379 pDev->TxtCmd(szCmd,dwRspSize,szRsp);
1380 if (isBinary == TRUE)
1381 pdiDev.GetMetric(isMetric);
1384 pDev->SetBinary(TRUE);
1385 pdiDev.GetMetric(isMetric);
1386 pDev->SetBinary(FALSE);
1388 if (isMetric == FALSE)
1389 cout <<
"\r\n\t<position units set to inches>\r\n";
1391 cout <<
"\r\n\t<position units set to meters>\r\n";
1395 pDev->TxtCmd(szCmd,dwRspSize,szRsp);
1396 pdiDev.GetBinary(isBinary);
1397 if (isBinary == FALSE)
1398 cout <<
"\r\n\t<response frames set to ASCII>\r\n";
1400 cout <<
"\r\n\t<response frames set to binary>\r\n";
1403 cout <<
"\r\n\t<pno frame format changed (use extreme caution)>";
1405 if (isBinary == TRUE)
1406 cout <<
"\r\n\t<suggestion: use ASCII response frames (reset command F)>";
1408 pDev->TxtCmd(szCmd,dwRspSize,szRsp);
1409 if (dwRspSize != 0 && dwRspSize != 5000){
1410 if (isBinary == TRUE)
1411 cout <<
"\r\n\t<binary response bgn>\r\n\t";
1413 cout <<
"\r\n\t<ASCII response bgn>\r\n\t";
1414 for (i = 0; i < dwRspSize; i++){
1415 if (szRsp[i] !=
'\r')
1416 printf(
"%c",szRsp[i]);
1417 if (szRsp[i] ==
'\n')
1420 if (isBinary == TRUE)
1421 cout <<
"\r\n\t<binary response end>\r\n";
1423 cout <<
"\r\n\t<ASCII response end>\r\n";
1426 cout <<
"\r\n\t<command sent>\r\n";
1433 BOOL vrpn_Tracker_FastrakPDI::StartCont( VOID )
1435 cout <<
"FastrakPDI: Start Continuous Mode\r\n";
1439 if (!(pdiDev.StartContPno(hwnd)))
1445 dwOverflowCount = 0;
1453 BOOL vrpn_Tracker_FastrakPDI::StopCont( VOID )
1455 cout <<
"FastrakPDI: Stop Continuous Mode\r\n";
1458 if (!(pdiDev.StopContPno()))
1468 ::ResetEvent(hContEvent);
1474 BOOL vrpn_Tracker_FastrakPDI::DisplayCont( timeval ct )
1481 DWORD dwLastSize = 0;
1483 if (!(pdiDev.LastPnoPtr(pBuf, dwSize)))
1486 else if ((pBuf == 0) || (dwSize == 0))
1489 else if (pLastBuf && (pBuf > pLastBuf))
1491 ParseFastrakFrame( pLastBuf+dwLastSize, dwSize+(pBuf-pLastBuf-dwLastSize), ct );
1493 dwLastSize = dwSize;
1496 else if (pBuf != pLastBuf)
1499 cout <<
"wrapped\r\n";
1502 dwLastSize = dwSize;
1503 ParseFastrakFrame( pBuf, dwSize, ct );
1512 VOID vrpn_Tracker_FastrakPDI::DisplaySingle( timeval ct )
1521 if (!(pdiDev.ReadSinglePnoBuf(pBuf, dwSize)))
1525 else if ((pBuf == 0) || (dwSize == 0))
1530 ParseFastrakFrame( pBuf, dwSize, ct );
1536 VOID vrpn_Tracker_FastrakPDI::ParseFastrakFrame( PBYTE pBuf, DWORD dwSize, timeval current_time )
1543 while (index < dwSize){
1545 BYTE ucSensor = pBuf[dw+1];
1546 BYTE ucInitCommand = pBuf[dw];
1547 BYTE ucErrorNum = pBuf[dw+2];
1548 d_sensor = atoi((
char*)(&ucSensor));
1551 dw += m_nHeaderSize;
1555 if (ucInitCommand !=
'0'){
1556 printf(
"FastrakPDI: received record type %x while in continuous mode, record error byte was %x \r\n", ucInitCommand, ucErrorNum);
1562 if ((m_nStylusMap & (1 << (d_sensor-1))) != 0)
1564 CHAR StyFlag = pBuf[dw+1];
1566 FTstylusBtns[d_sensor-1]->set_button(0, StyFlag -
'0');
1567 FTstylusBtns[d_sensor-1]->mainloop();
1572 PFLOAT pPno = (PFLOAT)(&pBuf[dw]);
1574 if (isMetric == TRUE){
1575 pos[0] = float(pPno[0])*CM_TO_METERS;
1576 pos[1] = float(pPno[1])*CM_TO_METERS;
1577 pos[2] = float(pPno[2])*CM_TO_METERS;
1580 pos[0] = float(pPno[0]);
1581 pos[1] = float(pPno[1]);
1582 pos[2] = float(pPno[2]);
1586 d_quat[0] = float(pPno[4]);
1587 d_quat[1] = float(pPno[5]);
1588 d_quat[2] = float(pPno[6]);
1589 d_quat[3] = float(pPno[3]);
1593 timestamp.tv_sec = current_time.tv_sec;
1594 timestamp.tv_usec = current_time.tv_usec;
1597 len = encode_to(msgbuf);
1598 d_connection->pack_message(len, timestamp,
1602 index += m_nFrameSize;
1607 vrpn_Tracker_LibertyPDI::vrpn_Tracker_LibertyPDI (
const char * name,
vrpn_Connection *cn,
1608 vrpn_float64 Hz,
const char * rcmd,
unsigned int nStylusMap) :
1610 , m_nStylusMap(nStylusMap)
1612 , m_nFrameSize(nStylusMap?40:36)
1614 cmd = (
char*)(rcmd);
1616 if(!(Initialize())){
1619 else if (nStylusMap & !(InitStylusBtns()))
1623 else if(!(Connect())){
1626 else if(!(SetupDevice())){
1629 else if(!(StartCont())){
1631 cout <<
"LibertyPDI: Failed to enter continuous mode\r\n";
1635 cout <<
"LibertyPDI: Initialization Complete\r\n";
1641 vrpn_Tracker_LibertyPDI::~vrpn_Tracker_LibertyPDI(
void){
1646 for (
int i=0; i<LIBERTY_MAX_SENSORS; i++)
1649 delete StylusBtns[i];
1654 VOID vrpn_Tracker_LibertyPDI::mainloop()
1656 struct timeval current_time;
1664 DisplayCont(current_time);
1673 int vrpn_Tracker_LibertyPDI::encode_to(
char *buf)
1693 return 1000 - buflen;
1698 BOOL vrpn_Tracker_LibertyPDI::Initialize(VOID){
1700 pos[0]=0; pos[1]=0; pos[2]=0;
1701 d_quat[0]=0; d_quat[1]=0; d_quat[2]=0; d_quat[3]=0;
1703 memset( StylusBtns, 0,
sizeof(StylusBtns));
1707 dwOverflowCount = 0;
1711 pdiDev.Trace(TRUE, 5);
1720 BOOL vrpn_Tracker_LibertyPDI::InitStylusBtns()
1724 for (
int i=0; i<LIBERTY_MAX_SENSORS; i++)
1726 if (((1<<i) & m_nStylusMap) != 0)
1729 sprintf( btnName,
"%sStylus%d", d_servicename, i+1);
1731 if (StylusBtns[i] == NULL)
1733 cout <<
"Cannot create button device " << btnName << endl;
1738 cout <<
"Button device " << btnName <<
" created." << endl;
1747 BOOL vrpn_Tracker_LibertyPDI::Connect( VOID )
1749 if (!(pdiDev.CnxReady()))
1751 pdiDev.SetSerialIF( &pdiSer );
1755 eType = pdiDev.DiscoverCnx();
1757 while (eType != PI_CNX_USB && eType != PI_CNX_SERIAL && attempts < 10){
1761 cout <<
"LibertyPDI: USB Connection\r\n";
1765 cout <<
"LibertyPDI: Serial Connection\r\n";
1769 printf(
"LibertyPDI: %s\r\n", pdiDev.GetLastResultStr() );
1770 eType = pdiDev.DiscoverCnx();
1778 num_sensors = pdiDev.StationCount();
1781 pdiDev.GetBITErrs( cBE );
1784 cBE.Parse( sz, 100 );
1786 if(!(cBE.IsClear()))
1787 pdiDev.ClearBITErrs();
1793 pdiMDat.Append(PDI_MODATA_STYLUS);
1795 pdiMDat.Append( PDI_MODATA_POS );
1796 pdiMDat.Append( PDI_MODATA_QTRN );
1797 pdiDev.SetSDataList( -1, pdiMDat );
1799 pdiDev.SetMetric(TRUE);
1805 pdiDev.SetPnoBuffer( pMotionBuf, VRPN_PDI_BUFFER_SIZE );
1807 bCnxReady = pdiDev.CnxReady();
1810 cout <<
"LibertyPDI: Already connected\r\n";
1818 VOID vrpn_Tracker_LibertyPDI::Disconnect(VOID)
1821 if (!(pdiDev.CnxReady()))
1823 cout <<
"LibertyPDI: Already disconnected\r\n";
1827 pdiDev.Disconnect();
1828 cout <<
"LibertyPDI: Disconnected\r\n";
1834 BOOL vrpn_Tracker_LibertyPDI::SetupDevice( VOID )
1837 if(strlen(pcmd) > 0){
1838 char * pch = strchr(pcmd,
'\\');
1839 while (pch != NULL){
1842 pcmd = pch +
sizeof(char);
1843 pch = strchr(pcmd,
'\\');
1849 if (isBinary == FALSE)
1850 cout <<
"LibertyPDI: Warning! Tracker is still in ASCII mode!\r\n";
1856 VOID vrpn_Tracker_LibertyPDI::UpdateStationMap( VOID )
1858 pdiDev.GetStationMap( dwStationMap );
1862 VOID vrpn_Tracker_LibertyPDI::SendCommand(
char *scmd)
1864 char szCmd[PI_MAX_CMD_BUF_LEN] =
"\0";
1865 DWORD dwRspSize = 5000;
1866 char szRsp[5000] =
"\0";
1867 bool parseErr =
false;
1870 cout <<
"LibertyPDI: reset command ";
1872 while (scmd[i] !=
'\0'){
1874 if (scmd[i] !=
'\n' && scmd[i] !=
'\r')
1880 for (i = 0; i < strlen(scmd); i++){
1886 strncat(szCmd,
"\x01", 1);
1889 strncat(szCmd,
"\x02", 1);
1892 strncat(szCmd,
"\x04", 1);
1895 strncat(szCmd,
"\x05", 1);
1898 strncat(szCmd,
"\x06", 1);
1901 strncat(szCmd,
"\x07", 1);
1904 strncat(szCmd,
"\x0b", 1);
1907 strncat(szCmd,
"\x0c", 1);
1910 strncat(szCmd,
"\x0e", 1);
1913 strncat(szCmd,
"\x0f", 1);
1916 strncat(szCmd,
"\x10", 1);
1919 strncat(szCmd,
"\x12", 1);
1922 strncat(szCmd,
"\x13", 1);
1925 strncat(szCmd,
"\x14", 1);
1928 strncat(szCmd,
"\x15", 1);
1931 strncat(szCmd,
"\x16", 1);
1934 strncat(szCmd,
"\x17", 1);
1937 strncat(szCmd,
"\x18", 1);
1940 strncat(szCmd,
"\x19", 1);
1943 strncat(szCmd,
"\x1a", 1);
1952 if (scmd[i] !=
'>'){
1957 strncat(szCmd,
"\r\n", 1);
1966 strncat(szCmd, &scmd[i], 1);
1970 if (parseErr ==
true)
1971 cout <<
"\r\n\t<unrecognized command>\r\n";
1973 strncat(szCmd,
"\0", 1);
1977 cout <<
"\r\n\t<command ignored, use P instead>\r\n";
1980 pdiDev.TxtCmd(szCmd,dwRspSize,szRsp);
1981 cout <<
"\r\n\t<tracker will be set to VRPN defaults on reconnect>\r\n";
1987 pdiDev.TxtCmd(szCmd,dwRspSize,szRsp);
1988 if (isBinary == TRUE)
1989 pdiDev.GetMetric(isMetric);
1991 pdiDev.SetBinary(TRUE);
1992 pdiDev.GetMetric(isMetric);
1993 pdiDev.SetBinary(FALSE);
1995 if (isMetric == FALSE)
1996 cout <<
"\r\n\t<position units set to inches>\r\n";
1998 cout <<
"\r\n\t<position units set to meters>\r\n";
2001 pdiDev.TxtCmd(szCmd,dwRspSize,szRsp);
2002 pdiDev.GetBinary(isBinary);
2003 if (isBinary == FALSE)
2004 cout <<
"\r\n\t<response frames set to ascii>\r\n";
2006 cout <<
"\r\n\t<response frames set to binary>\r\n";
2009 pdiDev.TxtCmd(szCmd,dwRspSize,szRsp);
2010 cout <<
"\r\n\t<pno frame format changed (use extreme caution)>\r\n";
2013 pdiDev.TxtCmd(szCmd,dwRspSize,szRsp);
2014 if (isBinary == TRUE && dwRspSize != 0 && dwRspSize != 5000){
2015 char * pRsp = szRsp;
2017 if (*pRsp == 0x00 || *pRsp == 0x20)
2018 cout <<
"\r\n\t<binary response contained no error>\r\n";
2021 cout <<
"\r\n\t<binary error response bgn>\r\n\t";
2022 for (
int i = 2; i < 2 + short(*pRsp); i++){
2023 if (szRsp[i] !=
'\r')
2024 printf(
"%c",pRsp[i]);
2025 if (szRsp[i] ==
'\n')
2028 cout <<
"\r\n\t<binary error response end>\r\n";
2031 else if (dwRspSize != 0 && dwRspSize != 5000){
2032 cout <<
"\r\n\t<ASCII response bgn>\r\n\t";
2033 for (
unsigned int i = 0; i < dwRspSize; i++){
2034 if (szRsp[i] !=
'\r')
2035 printf(
"%c",szRsp[i]);
2036 if (szRsp[i] ==
'\n')
2039 cout <<
"\r\n\t<ASCII response end>\r\n";
2042 cout <<
"\r\n\t<command sent>\r\n";
2048 BOOL vrpn_Tracker_LibertyPDI::StartCont( VOID )
2050 cout <<
"LibertyPDI: Start Continuous Mode\r\n";
2054 if (!(pdiDev.StartContPno(hwnd)))
2060 dwOverflowCount = 0;
2068 BOOL vrpn_Tracker_LibertyPDI::StopCont( VOID )
2070 cout <<
"LibertyPDI: Stop Continuous Mode\r\n";
2073 if (!(pdiDev.StopContPno()))
2083 ::ResetEvent(hContEvent);
2089 BOOL vrpn_Tracker_LibertyPDI::DisplayCont( timeval ct )
2096 DWORD dwLastSize = 0;
2098 if (!(pdiDev.LastPnoPtr(pBuf, dwSize)))
2101 else if ((pBuf == 0) || (dwSize == 0))
2104 else if (pLastBuf && (pBuf > pLastBuf))
2106 ParseLibertyFrame( pLastBuf+dwLastSize, dwSize+(pBuf-pLastBuf-dwLastSize), ct );
2108 dwLastSize = dwSize;
2111 else if (pBuf != pLastBuf)
2114 cout <<
"wrapped\n";
2117 dwLastSize = dwSize;
2118 ParseLibertyFrame( pBuf, dwSize, ct );
2127 VOID vrpn_Tracker_LibertyPDI::DisplaySingle( timeval ct )
2136 if (!(pdiDev.ReadSinglePnoBuf(pBuf, dwSize)))
2140 else if ((pBuf == 0) || (dwSize == 0))
2145 ParseLibertyFrame( pBuf, dwSize, ct );
2151 VOID vrpn_Tracker_LibertyPDI::ParseLibertyFrame( PBYTE pBuf, DWORD dwSize, timeval current_time )
2158 while (index < dwSize){
2160 BYTE ucSensor = pBuf[dw+2];
2161 BYTE ucInitCommand = pBuf[dw+3];
2162 BYTE ucErrorNum = pBuf[dw+4];
2163 SHORT shSize = pBuf[dw+6];
2164 d_sensor =
unsigned short(ucSensor);
2167 dw += m_nHeaderSize;
2171 if (ucInitCommand !=
'C' && ucInitCommand !=
'P'){
2172 printf(
"LibertyPDI: received command %x while in continuous mode, tracker response was %x \r\n", ucInitCommand, ucErrorNum);
2177 if ((m_nStylusMap & (1 << (d_sensor-1))) != 0)
2179 DWORD m_dwStylus = *((DWORD*)&pBuf[dw]);
2181 StylusBtns[d_sensor-1]->set_button(0, m_dwStylus);
2182 StylusBtns[d_sensor-1]->mainloop();
2184 dw +=
sizeof(DWORD);;
2188 PFLOAT pPno = (PFLOAT)(&pBuf[dw]);
2190 if (isMetric == TRUE){
2191 pos[0] = float(pPno[0])*CM_TO_METERS;
2192 pos[1] = float(pPno[1])*CM_TO_METERS;
2193 pos[2] = float(pPno[2])*CM_TO_METERS;
2196 pos[0] = float(pPno[0]);
2197 pos[1] = float(pPno[1]);
2198 pos[2] = float(pPno[2]);
2202 d_quat[0] = float(pPno[4]);
2203 d_quat[1] = float(pPno[5]);
2204 d_quat[2] = float(pPno[6]);
2205 d_quat[3] = float(pPno[3]);
2208 timestamp.tv_sec = current_time.tv_sec;
2209 timestamp.tv_usec = current_time.tv_usec;
2212 len = encode_to(msgbuf);
2213 d_connection->pack_message(len, timestamp,
2217 index += m_nFrameSize;