Ansicht | Historie | Commit | Commitdiff
Janis Streib
19.05.23 b21ff09151ca7e87348f1ea38944decc1f46f5db
Commit | Autor | Alter
4244cc 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);
7fde24 128     prs->hubList[a].SetDigIOData(m_pframeData[i].dig_io);    // set digio
4244cc 129     for (int j=0;j<G4_SENSORS_PER_HUB;j++){
JS 130       if (m_pframeData[i].stationMap&(0x01<<j))
131         prs->hubList[a].SetPnoData(m_pframeData[i].sfd[j].pos,j);    // set it
132     }    // end for j
133     break;
134       }    // end if
135     }    // end for a
136   }    // end for i
137
138
139
140
141   // /////////////////////////
142   // for (int i=0;i<numHubs;i++){        // for ea hub asked for
143   //   for (int a=0;a<m_numHubs;a++){        // check against all hubs we're tracking
144   //     if (hubarr[i].GetId()==(int)m_pframeData[a].hub){    // a match
145   //     if (m_pframeData[a].frame!=0xffffffff){            // make sure new data
146   //       for (int j=0;j<G4_SENSORS_PER_HUB;j++){    
147   //         if (m_pframeData[a].stationMap&(0x01<<j))    // for ea active sensor
148   //           hubarr[i].SetPnoData(m_pframeData[a].sfd[j].pos,j);    // set it
149
150   //       }    // end for j
151   //     }// end if
152   //     }// end if
153   //   }// end for a
154   // }// end for i
155
156
157   return rv>>16;    // return total hubs on system
158 }
159
160
161 int CG4Trk::GetActHubs()
162 {
163   return m_numHubs;
164 }
165
166 void CG4Trk::GetHubList(int *list, int numEl)
167 {
168   for (int i=0;i<numEl;i++){
169     if (i<m_numHubs)
170       list[i]=m_hubList[i];
171   }
172
173 }
174
175 void CG4Trk::GetSrcLoc(float (*loc)[6])
176 {
177   memcpy(loc,m_srcLoc,sizeof(float)*m_numSrc*6);
178 }    
179
180 int CG4Trk::GetNumSrc()
181 {
182   return m_numSrc;
183
184 }
185
186 void CG4Trk::Boresight(bool enable)
187 {
188   float angle[3]={0.0f,0.0f,0.0f}    ;
189   G4_CMD_STRUCT cs;
190   cs.cmd=G4_CMD_BORESIGHT;
191   cs.cds.id=G4_CREATE_ID(m_sysId,-1,0);    // all hubs,all sensors
192   cs.cds.action=enable?G4_ACTION_SET:G4_ACTION_RESET;    // boresight or unboresight?
193   cs.cds.iParam=G4_TYPE_EULER_DEGREE;
194   cs.cds.pParam=angle;
195   g4_set_query(&cs);
196 }
197
198 void CG4Trk::GetFilterValues(float *pfilt, float *ofilt)
199 {
200   G4_CMD_STRUCT cs;
201   cs.cmd=G4_CMD_FILTER;
202   cs.cds.id=G4_CREATE_ID(m_sysId,m_hubList[0],0);
203   cs.cds.action=G4_ACTION_GET;
204   cs.cds.iParam=G4_DATA_POS;
205   cs.cds.pParam=pfilt;
206   g4_set_query(&cs);
207
208   cs.cds.iParam=G4_DATA_ORI;
209   cs.cds.pParam=ofilt;
210   g4_set_query(&cs);
211
212 }
213
214 void CG4Trk::SetFilterValues(float *pfilt, float *ofilt)
215 {
216   G4_CMD_STRUCT cs;
217   cs.cmd=G4_CMD_FILTER;
218   cs.cds.id=G4_CREATE_ID(m_sysId,-1,0);
219   cs.cds.action=G4_ACTION_SET;
220   cs.cds.iParam=G4_DATA_POS;
221   cs.cds.pParam=pfilt;
222   g4_set_query(&cs);
223
224   cs.cds.iParam=G4_DATA_ORI;
225   cs.cds.pParam=ofilt;
226   g4_set_query(&cs);
227
228 }
229
230 void CG4Trk::GetSetIncr(float &posIncr, float &oriIncr,bool bSet)
231 {
232   G4_CMD_STRUCT cs;
233   cs.cmd=G4_CMD_INCREMENT;
234   float fIncr[2];
235
236   if (bSet){
237     cs.cds.id=G4_CREATE_ID(m_sysId,-1,0);
238     cs.cds.action=G4_ACTION_SET;
239     fIncr[0]=posIncr;
240     fIncr[1]=oriIncr;
241   }
242   else {
243     cs.cds.id=G4_CREATE_ID(m_sysId,m_hubList[0],0);
244     cs.cds.action=G4_ACTION_GET;
245   }
246   cs.cds.iParam=(G4_TYPE_INCH<<16)|G4_TYPE_EULER_DEGREE;
247   cs.cds.pParam=fIncr;
248   g4_set_query(&cs);
249
250   if (!bSet){
251     posIncr=fIncr[0];
252     oriIncr=fIncr[1];
253   }
254
255
256
257 }
258
259 int CG4Trk::UpdateHubs()
260 {
261   // get a list of hubs
262   G4_CMD_STRUCT cs;
263   cs.cmd=G4_CMD_GET_ACTIVE_HUBS;
264   cs.cds.id=G4_CREATE_ID(m_sysId,0,0);
265   cs.cds.pParam=NULL;
266   g4_set_query(&cs);
267
268   if ((int)cs.cds.iParam!=m_numHubs){
269     m_numHubs=cs.cds.iParam;
270     delete[] m_hubList;
271     delete[] m_pframeData;
272     m_hubList=new int[m_numHubs];
273     m_pframeData=new G4_FRAMEDATA[m_numHubs];
274     cs.cds.pParam=m_hubList;
275     g4_set_query(&cs);
276   }
277
278   return m_numHubs;
279
280
281 }