// G4Trk.cpp: implementation of the CG4Trk class.
|
//
|
//////////////////////////////////////////////////////////////////////
|
#include <GL/glu.h>
|
#include "Quaternion.h"
|
#include "struct.h"
|
#include "G4Trk.h"
|
#include <unistd.h>
|
#include <string.h>
|
|
|
//////////////////////////////////////////////////////////////////////
|
// Construction/Destruction
|
//////////////////////////////////////////////////////////////////////
|
|
const int FRAMERATE=60;
|
|
|
|
CG4Trk::CG4Trk()
|
{
|
|
m_bConnected=false;
|
m_numHubs=0;
|
m_hubList=NULL;
|
m_pframeData=NULL;
|
m_srcLoc=NULL;
|
|
}
|
|
CG4Trk::~CG4Trk()
|
{
|
|
if (m_bConnected)
|
g4_close_tracker();
|
if (m_hubList)
|
delete[] m_hubList;
|
if (m_pframeData)
|
delete[] m_pframeData;
|
if (m_srcLoc)
|
delete[] m_srcLoc;
|
|
}
|
|
bool CG4Trk::Connect(char* cfgFile)
|
{
|
int res=g4_init_sys(&m_sysId,cfgFile,NULL);
|
if (res==G4_ERROR_NONE){
|
m_bConnected=true;
|
|
// get a list of hubs
|
G4_CMD_STRUCT cs;
|
cs.cmd=G4_CMD_GET_ACTIVE_HUBS;
|
cs.cds.id=G4_CREATE_ID(m_sysId,0,0);
|
cs.cds.pParam=NULL;
|
|
int count=0;
|
do { // may need to spin awhile until system recognizes attached hubs
|
usleep(100000);
|
g4_set_query(&cs);
|
} while ((cs.cds.iParam==0) && (++count<150));
|
|
if (cs.cds.iParam==0){
|
m_bConnected=false;
|
g4_close_tracker();
|
return false;
|
}
|
|
usleep(500000); // one more time to catch any stragglers
|
g4_set_query(&cs);
|
|
m_numHubs=cs.cds.iParam;
|
|
m_hubList=new int[m_numHubs];
|
cs.cds.pParam=m_hubList;
|
g4_set_query(&cs);
|
|
|
usleep(100000);
|
|
// set units
|
// int u=G4_TYPE_INCH;
|
// cs.cmd=G4_CMD_UNITS;
|
// cs.cds.action=G4_ACTION_SET;
|
// cs.cds.iParam=G4_DATA_POS;
|
// cs.cds.pParam=&u;
|
// int xx=g4_set_query(&cs);
|
|
// set up struct for data acquisition
|
m_pframeData=new G4_FRAMEDATA[m_numHubs];
|
g4_get_frame_data(m_pframeData,m_sysId,m_hubList,m_numHubs);
|
|
// get source qty and locations
|
cs.cmd=G4_CMD_GET_SOURCE_MAP;
|
cs.cds.id=G4_CREATE_ID(m_sysId,0,0);
|
cs.cds.pParam=NULL;
|
g4_set_query(&cs);
|
m_numSrc=cs.cds.iParam;
|
|
m_srcLoc=new float[m_numSrc][6];
|
|
// build a structure
|
LPG4_SRC_MAP map=new G4_SRC_MAP[m_numSrc];
|
cs.cds.pParam=map;
|
cs.cds.iParam=(G4_TYPE_INCH<<16)|G4_TYPE_EULER_DEGREE;
|
g4_set_query(&cs);
|
for (int i=0;i<m_numSrc;i++)
|
memcpy(m_srcLoc[i],map[i].pos,sizeof(float)*6);
|
|
delete[] map;
|
return true;
|
}
|
|
return false;
|
|
}
|
//#include <stdio.h>
|
int CG4Trk::GetHubsPno(REND_STRUCT* prs)
|
{
|
// get all pno
|
int rv=g4_get_frame_data(m_pframeData,m_sysId,m_hubList,m_numHubs);
|
int num_read=rv&0xffff;
|
|
for (int i=0;i<num_read;i++){
|
for (int a=0;a<m_numHubs;a++){
|
if ((unsigned int)prs->hubList[a].GetId()==m_pframeData[i].hub){
|
prs->hubreadmap|=(0x01<<a);
|
for (int j=0;j<G4_SENSORS_PER_HUB;j++){
|
if (m_pframeData[i].stationMap&(0x01<<j))
|
prs->hubList[a].SetPnoData(m_pframeData[i].sfd[j].pos,j); // set it
|
} // end for j
|
break;
|
} // end if
|
} // end for a
|
} // end for i
|
|
|
|
|
// /////////////////////////
|
// for (int i=0;i<numHubs;i++){ // for ea hub asked for
|
// for (int a=0;a<m_numHubs;a++){ // check against all hubs we're tracking
|
// if (hubarr[i].GetId()==(int)m_pframeData[a].hub){ // a match
|
// if (m_pframeData[a].frame!=0xffffffff){ // make sure new data
|
// for (int j=0;j<G4_SENSORS_PER_HUB;j++){
|
// if (m_pframeData[a].stationMap&(0x01<<j)) // for ea active sensor
|
// hubarr[i].SetPnoData(m_pframeData[a].sfd[j].pos,j); // set it
|
|
// } // end for j
|
// }// end if
|
// }// end if
|
// }// end for a
|
// }// end for i
|
|
|
return rv>>16; // return total hubs on system
|
}
|
|
|
int CG4Trk::GetActHubs()
|
{
|
return m_numHubs;
|
}
|
|
void CG4Trk::GetHubList(int *list, int numEl)
|
{
|
for (int i=0;i<numEl;i++){
|
if (i<m_numHubs)
|
list[i]=m_hubList[i];
|
}
|
|
}
|
|
void CG4Trk::GetSrcLoc(float (*loc)[6])
|
{
|
memcpy(loc,m_srcLoc,sizeof(float)*m_numSrc*6);
|
}
|
|
int CG4Trk::GetNumSrc()
|
{
|
return m_numSrc;
|
|
}
|
|
void CG4Trk::Boresight(bool enable)
|
{
|
float angle[3]={0.0f,0.0f,0.0f} ;
|
G4_CMD_STRUCT cs;
|
cs.cmd=G4_CMD_BORESIGHT;
|
cs.cds.id=G4_CREATE_ID(m_sysId,-1,0); // all hubs,all sensors
|
cs.cds.action=enable?G4_ACTION_SET:G4_ACTION_RESET; // boresight or unboresight?
|
cs.cds.iParam=G4_TYPE_EULER_DEGREE;
|
cs.cds.pParam=angle;
|
g4_set_query(&cs);
|
}
|
|
void CG4Trk::GetFilterValues(float *pfilt, float *ofilt)
|
{
|
G4_CMD_STRUCT cs;
|
cs.cmd=G4_CMD_FILTER;
|
cs.cds.id=G4_CREATE_ID(m_sysId,m_hubList[0],0);
|
cs.cds.action=G4_ACTION_GET;
|
cs.cds.iParam=G4_DATA_POS;
|
cs.cds.pParam=pfilt;
|
g4_set_query(&cs);
|
|
cs.cds.iParam=G4_DATA_ORI;
|
cs.cds.pParam=ofilt;
|
g4_set_query(&cs);
|
|
}
|
|
void CG4Trk::SetFilterValues(float *pfilt, float *ofilt)
|
{
|
G4_CMD_STRUCT cs;
|
cs.cmd=G4_CMD_FILTER;
|
cs.cds.id=G4_CREATE_ID(m_sysId,-1,0);
|
cs.cds.action=G4_ACTION_SET;
|
cs.cds.iParam=G4_DATA_POS;
|
cs.cds.pParam=pfilt;
|
g4_set_query(&cs);
|
|
cs.cds.iParam=G4_DATA_ORI;
|
cs.cds.pParam=ofilt;
|
g4_set_query(&cs);
|
|
}
|
|
void CG4Trk::GetSetIncr(float &posIncr, float &oriIncr,bool bSet)
|
{
|
G4_CMD_STRUCT cs;
|
cs.cmd=G4_CMD_INCREMENT;
|
float fIncr[2];
|
|
if (bSet){
|
cs.cds.id=G4_CREATE_ID(m_sysId,-1,0);
|
cs.cds.action=G4_ACTION_SET;
|
fIncr[0]=posIncr;
|
fIncr[1]=oriIncr;
|
}
|
else {
|
cs.cds.id=G4_CREATE_ID(m_sysId,m_hubList[0],0);
|
cs.cds.action=G4_ACTION_GET;
|
}
|
cs.cds.iParam=(G4_TYPE_INCH<<16)|G4_TYPE_EULER_DEGREE;
|
cs.cds.pParam=fIncr;
|
g4_set_query(&cs);
|
|
if (!bSet){
|
posIncr=fIncr[0];
|
oriIncr=fIncr[1];
|
}
|
|
|
|
}
|
|
int CG4Trk::UpdateHubs()
|
{
|
// get a list of hubs
|
G4_CMD_STRUCT cs;
|
cs.cmd=G4_CMD_GET_ACTIVE_HUBS;
|
cs.cds.id=G4_CREATE_ID(m_sysId,0,0);
|
cs.cds.pParam=NULL;
|
g4_set_query(&cs);
|
|
if ((int)cs.cds.iParam!=m_numHubs){
|
m_numHubs=cs.cds.iParam;
|
delete[] m_hubList;
|
delete[] m_pframeData;
|
m_hubList=new int[m_numHubs];
|
m_pframeData=new G4_FRAMEDATA[m_numHubs];
|
cs.cds.pParam=m_hubList;
|
g4_set_query(&cs);
|
}
|
|
return m_numHubs;
|
|
|
}
|
