Debian package version of polhemus's g4display
view | history | commit | commitdiff
Janis Streib
30.03.22 48e3f3ec6672ba89420338fdfa76018e46750c9e
commit | author | age
69b66f 1 // G4Trk.cpp: implementation of the CG4Trk class.
JS 2 //
3 //////////////////////////////////////////////////////////////////////
4 #include <GL/glu.h>
5 #include "Quaternion.h"
6 #include "struct.h"
7 #include "G4Trk.h"
8 #include <unistd.h>
9 #include <string.h>
10
11
12 //////////////////////////////////////////////////////////////////////
13 // Construction/Destruction
14 //////////////////////////////////////////////////////////////////////
15
16 const int FRAMERATE=60;
17
18
19
20 CG4Trk::CG4Trk()
21 {
22
23   m_bConnected=false;
24   m_numHubs=0;
25   m_hubList=NULL;
26   m_pframeData=NULL;
27   m_srcLoc=NULL;
28
29 }
30
31 CG4Trk::~CG4Trk()
32 {
33
34   if (m_bConnected)
35     g4_close_tracker();
36   if (m_hubList)
37     delete[] m_hubList;
38   if (m_pframeData)
39     delete[] m_pframeData;
40   if (m_srcLoc)
41     delete[] m_srcLoc;
42
43 }
44
45 bool CG4Trk::Connect(char* cfgFile)
46 {
47   int res=g4_init_sys(&m_sysId,cfgFile,NULL);
48   if (res==G4_ERROR_NONE){
49     m_bConnected=true;
50
51     // get a list of hubs
52     G4_CMD_STRUCT cs;
53     cs.cmd=G4_CMD_GET_ACTIVE_HUBS;
54     cs.cds.id=G4_CREATE_ID(m_sysId,0,0);
55     cs.cds.pParam=NULL;
56
57     int count=0;
58     do {                      // may need to spin awhile until system recognizes attached hubs
59       usleep(100000);
60       g4_set_query(&cs);
61     } while ((cs.cds.iParam==0) && (++count<150));
62
63     if (cs.cds.iParam==0){
64       m_bConnected=false;
65       g4_close_tracker();
66       return false;
67     }
68
69     usleep(500000);    // one more time to catch any stragglers
70     g4_set_query(&cs);
71
72     m_numHubs=cs.cds.iParam;
73
74     m_hubList=new int[m_numHubs];
75     cs.cds.pParam=m_hubList;
76     g4_set_query(&cs);
77
78
79     usleep(100000);
80
81     // set units
82     // int u=G4_TYPE_INCH;
83     // cs.cmd=G4_CMD_UNITS;
84     // cs.cds.action=G4_ACTION_SET;
85     // cs.cds.iParam=G4_DATA_POS;
86     // cs.cds.pParam=&u;
87     // int xx=g4_set_query(&cs);
88         
89     // set up struct for data acquisition
90     m_pframeData=new G4_FRAMEDATA[m_numHubs];
91     g4_get_frame_data(m_pframeData,m_sysId,m_hubList,m_numHubs);
92
93     // get source qty and locations
94     cs.cmd=G4_CMD_GET_SOURCE_MAP;
95     cs.cds.id=G4_CREATE_ID(m_sysId,0,0);
96     cs.cds.pParam=NULL;
97     g4_set_query(&cs);
98     m_numSrc=cs.cds.iParam;
99
100     m_srcLoc=new float[m_numSrc][6];
101
102     // build a structure
103     LPG4_SRC_MAP map=new G4_SRC_MAP[m_numSrc];
104     cs.cds.pParam=map;
105     cs.cds.iParam=(G4_TYPE_INCH<<16)|G4_TYPE_EULER_DEGREE;
106     g4_set_query(&cs);
107     for (int i=0;i<m_numSrc;i++)
108       memcpy(m_srcLoc[i],map[i].pos,sizeof(float)*6);
109
110     delete[] map;
111     return true;
112   }
113
114   return false;
115
116 }
117 //#include <stdio.h>
118 int CG4Trk::GetHubsPno(REND_STRUCT* prs)
119 {
120   // get all pno
121   int rv=g4_get_frame_data(m_pframeData,m_sysId,m_hubList,m_numHubs);
122   int num_read=rv&0xffff;
123
124   for (int i=0;i<num_read;i++){
125     for (int a=0;a<m_numHubs;a++){
126       if ((unsigned int)prs->hubList[a].GetId()==m_pframeData[i].hub){
127     prs->hubreadmap|=(0x01<<a);
128     for (int j=0;j<G4_SENSORS_PER_HUB;j++){
129       if (m_pframeData[i].stationMap&(0x01<<j))
130         prs->hubList[a].SetPnoData(m_pframeData[i].sfd[j].pos,j);    // set it
131     }    // end for j
132     break;
133       }    // end if
134     }    // end for a
135   }    // end for i
136
137
138
139
140   // /////////////////////////
141   // for (int i=0;i<numHubs;i++){        // for ea hub asked for
142   //   for (int a=0;a<m_numHubs;a++){        // check against all hubs we're tracking
143   //     if (hubarr[i].GetId()==(int)m_pframeData[a].hub){    // a match
144   //     if (m_pframeData[a].frame!=0xffffffff){            // make sure new data
145   //       for (int j=0;j<G4_SENSORS_PER_HUB;j++){    
146   //         if (m_pframeData[a].stationMap&(0x01<<j))    // for ea active sensor
147   //           hubarr[i].SetPnoData(m_pframeData[a].sfd[j].pos,j);    // set it
148
149   //       }    // end for j
150   //     }// end if
151   //     }// end if
152   //   }// end for a
153   // }// end for i
154
155
156   return rv>>16;    // return total hubs on system
157 }
158
159
160 int CG4Trk::GetActHubs()
161 {
162   return m_numHubs;
163 }
164
165 void CG4Trk::GetHubList(int *list, int numEl)
166 {
167   for (int i=0;i<numEl;i++){
168     if (i<m_numHubs)
169       list[i]=m_hubList[i];
170   }
171
172 }
173
174 void CG4Trk::GetSrcLoc(float (*loc)[6])
175 {
176   memcpy(loc,m_srcLoc,sizeof(float)*m_numSrc*6);
177 }    
178
179 int CG4Trk::GetNumSrc()
180 {
181   return m_numSrc;
182
183 }
184
185 void CG4Trk::Boresight(bool enable)
186 {
187   float angle[3]={0.0f,0.0f,0.0f}    ;
188   G4_CMD_STRUCT cs;
189   cs.cmd=G4_CMD_BORESIGHT;
190   cs.cds.id=G4_CREATE_ID(m_sysId,-1,0);    // all hubs,all sensors
191   cs.cds.action=enable?G4_ACTION_SET:G4_ACTION_RESET;    // boresight or unboresight?
192   cs.cds.iParam=G4_TYPE_EULER_DEGREE;
193   cs.cds.pParam=angle;
194   g4_set_query(&cs);
195 }
196
197 void CG4Trk::GetFilterValues(float *pfilt, float *ofilt)
198 {
199   G4_CMD_STRUCT cs;
200   cs.cmd=G4_CMD_FILTER;
201   cs.cds.id=G4_CREATE_ID(m_sysId,m_hubList[0],0);
202   cs.cds.action=G4_ACTION_GET;
203   cs.cds.iParam=G4_DATA_POS;
204   cs.cds.pParam=pfilt;
205   g4_set_query(&cs);
206
207   cs.cds.iParam=G4_DATA_ORI;
208   cs.cds.pParam=ofilt;
209   g4_set_query(&cs);
210
211 }
212
213 void CG4Trk::SetFilterValues(float *pfilt, float *ofilt)
214 {
215   G4_CMD_STRUCT cs;
216   cs.cmd=G4_CMD_FILTER;
217   cs.cds.id=G4_CREATE_ID(m_sysId,-1,0);
218   cs.cds.action=G4_ACTION_SET;
219   cs.cds.iParam=G4_DATA_POS;
220   cs.cds.pParam=pfilt;
221   g4_set_query(&cs);
222
223   cs.cds.iParam=G4_DATA_ORI;
224   cs.cds.pParam=ofilt;
225   g4_set_query(&cs);
226
227 }
228
229 void CG4Trk::GetSetIncr(float &posIncr, float &oriIncr,bool bSet)
230 {
231   G4_CMD_STRUCT cs;
232   cs.cmd=G4_CMD_INCREMENT;
233   float fIncr[2];
234
235   if (bSet){
236     cs.cds.id=G4_CREATE_ID(m_sysId,-1,0);
237     cs.cds.action=G4_ACTION_SET;
238     fIncr[0]=posIncr;
239     fIncr[1]=oriIncr;
240   }
241   else {
242     cs.cds.id=G4_CREATE_ID(m_sysId,m_hubList[0],0);
243     cs.cds.action=G4_ACTION_GET;
244   }
245   cs.cds.iParam=(G4_TYPE_INCH<<16)|G4_TYPE_EULER_DEGREE;
246   cs.cds.pParam=fIncr;
247   g4_set_query(&cs);
248
249   if (!bSet){
250     posIncr=fIncr[0];
251     oriIncr=fIncr[1];
252   }
253
254
255
256 }
257
258 int CG4Trk::UpdateHubs()
259 {
260   // get a list of hubs
261   G4_CMD_STRUCT cs;
262   cs.cmd=G4_CMD_GET_ACTIVE_HUBS;
263   cs.cds.id=G4_CREATE_ID(m_sysId,0,0);
264   cs.cds.pParam=NULL;
265   g4_set_query(&cs);
266
267   if ((int)cs.cds.iParam!=m_numHubs){
268     m_numHubs=cs.cds.iParam;
269     delete[] m_hubList;
270     delete[] m_pframeData;
271     m_hubList=new int[m_numHubs];
272     m_pframeData=new G4_FRAMEDATA[m_numHubs];
273     cs.cds.pParam=m_hubList;
274     g4_set_query(&cs);
275   }
276
277   return m_numHubs;
278
279
280 }