1 | #include "rtigs.h"
|
---|
2 |
|
---|
3 | #define MAXSTREAM 2048
|
---|
4 |
|
---|
5 | int main() {
|
---|
6 |
|
---|
7 | unsigned char data_stream[MAXSTREAM];
|
---|
8 | unsigned short numbytes;
|
---|
9 | RTIGSS_T rtigs_sta;
|
---|
10 | RTIGSO_T rtigs_obs;
|
---|
11 | RTIGSM_T rtigs_met;
|
---|
12 | RTIGSE_T rtigs_eph;
|
---|
13 | short PRN;
|
---|
14 | short retval;
|
---|
15 | unsigned short statID;
|
---|
16 | unsigned short messType;
|
---|
17 | CGPS_Transform GPSTrans;
|
---|
18 |
|
---|
19 | memset(data_stream , 0, sizeof(data_stream));
|
---|
20 |
|
---|
21 | // use something like recvfrom
|
---|
22 |
|
---|
23 | messType = GPSTrans.GetRTIGSHdrRecType(data_stream);
|
---|
24 | numbytes = GPSTrans.GetRTIGSHdrRecBytes(data_stream);
|
---|
25 | statID = GPSTrans.GetRTIGSHdrStaID(data_stream);
|
---|
26 |
|
---|
27 | switch (messType) {
|
---|
28 | case 100:
|
---|
29 | GPSTrans.Decode_RTIGS_Sta(data_stream, numbytes , rtigs_sta);
|
---|
30 | break;
|
---|
31 | case 200:
|
---|
32 | retval = GPSTrans.Decode_RTIGS_Obs(data_stream, numbytes , rtigs_obs);
|
---|
33 | if (retval >= 1) {
|
---|
34 | GPSTrans.print_CMEAS();
|
---|
35 | }
|
---|
36 | break;
|
---|
37 | case 300:
|
---|
38 | retval = GPSTrans.Decode_RTIGS_Eph(data_stream, numbytes , rtigs_eph, PRN);
|
---|
39 | break;
|
---|
40 | case 400:
|
---|
41 | retval = GPSTrans.Decode_RTIGS_Met(data_stream, numbytes , &rtigs_met);
|
---|
42 | break;
|
---|
43 | }
|
---|
44 |
|
---|
45 | return 0;
|
---|
46 | }
|
---|
47 |
|
---|
48 | // Constructor
|
---|
49 | ////////////////////////////////////////////////////////////////////////////
|
---|
50 | CGPS_Transform::CGPS_Transform() {
|
---|
51 |
|
---|
52 | // Decoded Obs Array of 12 CMeas Observation Records
|
---|
53 | memset((void *)&DecObs, 0, sizeof(ARR_OBS_T ));
|
---|
54 |
|
---|
55 | // Keplarian Broadcast Eph
|
---|
56 | memset((void *)&TNAV_Eph,0, sizeof(ARR_TNAV_T ));
|
---|
57 |
|
---|
58 | NumObsRead = -1;
|
---|
59 | CAFlag = -1;
|
---|
60 | ASFlag = -1;
|
---|
61 | P2Flag = -1;
|
---|
62 | P1Flag = -1;
|
---|
63 | InitEndianFlag();
|
---|
64 |
|
---|
65 | memset (PhaseArcStartTime, 0, sizeof(PhaseArcStartTime));
|
---|
66 | }
|
---|
67 |
|
---|
68 | // Destructor
|
---|
69 | ////////////////////////////////////////////////////////////////////////////
|
---|
70 | CGPS_Transform::~CGPS_Transform() {
|
---|
71 | }
|
---|
72 |
|
---|
73 | //
|
---|
74 | ////////////////////////////////////////////////////////////////////////////
|
---|
75 | unsigned short CGPS_Transform::GetRTIGSHdrRecType(unsigned char *RTIGS_Str)
|
---|
76 | {
|
---|
77 | unsigned short recordID;
|
---|
78 | memcpy ((void *)&recordID,RTIGS_Str, sizeof(recordID));
|
---|
79 | if (f_IsLittleEndian)
|
---|
80 | {
|
---|
81 | SwitchBytes( (char *)&recordID, sizeof(recordID) );
|
---|
82 | }
|
---|
83 | return recordID;
|
---|
84 | }
|
---|
85 |
|
---|
86 | //
|
---|
87 | ////////////////////////////////////////////////////////////////////////////
|
---|
88 | unsigned short CGPS_Transform::GetRTIGSHdrRecBytes(unsigned char *RTIGS_Str)
|
---|
89 | {
|
---|
90 | unsigned short bytes;
|
---|
91 | memcpy ((void *)&bytes,&RTIGS_Str[8], sizeof(bytes));
|
---|
92 | if (f_IsLittleEndian)
|
---|
93 | {
|
---|
94 | SwitchBytes( (char *)&bytes, sizeof(bytes) );
|
---|
95 | }
|
---|
96 | return bytes;
|
---|
97 |
|
---|
98 | }
|
---|
99 |
|
---|
100 | //
|
---|
101 | ////////////////////////////////////////////////////////////////////////////
|
---|
102 | unsigned short CGPS_Transform::GetRTIGSHdrStaID(unsigned char *RTIGS_Str)
|
---|
103 | {
|
---|
104 | unsigned short StaID = 0;
|
---|
105 | memcpy ((void *)&StaID, &RTIGS_Str[2], sizeof(StaID));
|
---|
106 | if (f_IsLittleEndian)
|
---|
107 | {
|
---|
108 | SwitchBytes( (char *)&StaID, sizeof(StaID) );
|
---|
109 | }
|
---|
110 | return StaID;
|
---|
111 | }
|
---|
112 |
|
---|
113 |
|
---|
114 | //
|
---|
115 | ////////////////////////////////////////////////////////////////////////////
|
---|
116 | void CGPS_Transform::InitEndianFlag() {
|
---|
117 | short one = 1;
|
---|
118 | char *cp = (char *)&one;
|
---|
119 | if (*cp== 0) {
|
---|
120 | f_IsLittleEndian = false;
|
---|
121 | }
|
---|
122 | else {
|
---|
123 | f_IsLittleEndian = true;
|
---|
124 | }
|
---|
125 | }
|
---|
126 |
|
---|
127 | //
|
---|
128 | ////////////////////////////////////////////////////////////////////////////
|
---|
129 | void CGPS_Transform::SwitchBytes( char *Start, int Size ) {
|
---|
130 | char Tmp;
|
---|
131 | char *End = Start + Size - 1;
|
---|
132 | for( Tmp = *Start; Start < End; Tmp = *Start ){
|
---|
133 | *Start++ = *End;
|
---|
134 | *End-- = Tmp;
|
---|
135 | }
|
---|
136 | }
|
---|
137 |
|
---|
138 | //
|
---|
139 | ////////////////////////////////////////////////////////////////////////////
|
---|
140 | unsigned long CGPS_Transform::JPL_xtractLongVal (unsigned startBitNbr, unsigned xtractNbrBits, const char *msg)
|
---|
141 | {
|
---|
142 | unsigned long retValue=0, i=0;
|
---|
143 |
|
---|
144 | unsigned short posBit = xtractNbrBits - 1;
|
---|
145 |
|
---|
146 | for(i=0; i<xtractNbrBits; i++, startBitNbr++)
|
---|
147 | {
|
---|
148 | unsigned short numShift = 7 - (startBitNbr % 8);
|
---|
149 | unsigned short byteNbr = startBitNbr / 8;
|
---|
150 | retValue |= (((msg[byteNbr] >> numShift) & 0x0001) << posBit--);
|
---|
151 |
|
---|
152 | }
|
---|
153 |
|
---|
154 | return retValue;
|
---|
155 | }
|
---|
156 |
|
---|
157 | //
|
---|
158 | ////////////////////////////////////////////////////////////////////////////
|
---|
159 | inline void CGPS_Transform::SwitchIGS_Sta_HdrBytes( RTIGSS_T *StaHdr)
|
---|
160 | {
|
---|
161 | SwitchBytes( (char *)&StaHdr->GPSTime, sizeof(unsigned long) );
|
---|
162 | SwitchBytes( (char *)&StaHdr->num_bytes, sizeof(unsigned short) );
|
---|
163 | SwitchBytes( (char *)&StaHdr->rec_id, sizeof(unsigned short) );
|
---|
164 | SwitchBytes( (char *)&StaHdr->sta_id, sizeof(unsigned short) );
|
---|
165 | }
|
---|
166 |
|
---|
167 | //
|
---|
168 | ////////////////////////////////////////////////////////////////////////////
|
---|
169 | inline void CGPS_Transform::SwitchIGS_Obs_HdrBytes( RTIGSO_T *ObsHdr)
|
---|
170 | {
|
---|
171 | SwitchBytes( (char *)&ObsHdr->GPSTime, sizeof(unsigned long) );
|
---|
172 | SwitchBytes( (char *)&ObsHdr->num_bytes, sizeof(unsigned short) );
|
---|
173 | SwitchBytes( (char *)&ObsHdr->rec_id, sizeof(unsigned short) );
|
---|
174 | SwitchBytes( (char *)&ObsHdr->sta_id, sizeof(unsigned short) );
|
---|
175 | }
|
---|
176 |
|
---|
177 | //
|
---|
178 | ////////////////////////////////////////////////////////////////////////////
|
---|
179 | inline void CGPS_Transform::SwitchIGS_Eph_HdrBytes( RTIGSE_T *EphHdr)
|
---|
180 |
|
---|
181 | {
|
---|
182 | SwitchBytes( (char *)&EphHdr->CollectedGPSTime, sizeof(unsigned long) );
|
---|
183 | SwitchBytes( (char *)&EphHdr->num_bytes, sizeof(unsigned short) );
|
---|
184 | SwitchBytes( (char *)&EphHdr->rec_id, sizeof(unsigned short) );
|
---|
185 | SwitchBytes( (char *)&EphHdr->sta_id, sizeof(unsigned short) );
|
---|
186 | }
|
---|
187 |
|
---|
188 | //
|
---|
189 | ////////////////////////////////////////////////////////////////////////////
|
---|
190 | inline short CGPS_Transform::SwitchIGS_Met_RecBytes( RTIGSM_T *MetHdr)
|
---|
191 | {
|
---|
192 | short retval = 1;
|
---|
193 | short i, num_items;
|
---|
194 | num_items = (short)MetHdr->numobs;
|
---|
195 | SwitchBytes( (char *)&MetHdr->GPSTime, sizeof(unsigned long) );
|
---|
196 | SwitchBytes( (char *)&MetHdr->num_bytes, sizeof(unsigned short) );
|
---|
197 | SwitchBytes( (char *)&MetHdr->rec_id, sizeof(unsigned short) );
|
---|
198 | SwitchBytes( (char *)&MetHdr->sta_id, sizeof(unsigned short) );
|
---|
199 |
|
---|
200 | /*switch met data bytes*/
|
---|
201 | for (i=0; i < num_items; i++)
|
---|
202 | {
|
---|
203 | if (&MetHdr->mets[i] != NULL)
|
---|
204 | {
|
---|
205 | SwitchBytes( (char *)&MetHdr->mets[i], sizeof(long) );
|
---|
206 | }
|
---|
207 | else
|
---|
208 | {
|
---|
209 | retval = -1;
|
---|
210 | }
|
---|
211 |
|
---|
212 | }
|
---|
213 | return retval;
|
---|
214 | }
|
---|
215 | //
|
---|
216 | ////////////////////////////////////////////////////////////////////////////
|
---|
217 |
|
---|
218 | short CGPS_Transform::Save_TNAV_T_To_Container(TNAV_T *rtcurrent_eph, short &prn)
|
---|
219 | {
|
---|
220 | short retval = 1;//, i;
|
---|
221 | long PRN;
|
---|
222 |
|
---|
223 | PRN = rtcurrent_eph->Satellite;
|
---|
224 |
|
---|
225 | if (f_IsLittleEndian)
|
---|
226 | {
|
---|
227 | SwitchBytes( (char *)&PRN, sizeof(PRN));
|
---|
228 | }
|
---|
229 |
|
---|
230 | if ((PRN > 0) && (PRN <= 32))
|
---|
231 | {
|
---|
232 | memcpy( (void *)&TNAV_Eph.Eph[(PRN-1)] ,rtcurrent_eph,sizeof(TNAV_T));
|
---|
233 | prn = (short)PRN;
|
---|
234 | }
|
---|
235 | else
|
---|
236 | {
|
---|
237 | retval = -1;
|
---|
238 | }
|
---|
239 | return retval;
|
---|
240 | }
|
---|
241 |
|
---|
242 | //
|
---|
243 | ////////////////////////////////////////////////////////////////////////////
|
---|
244 | short CGPS_Transform::CA_Extract(char * CAStr, double &CA_Rng)
|
---|
245 | {
|
---|
246 |
|
---|
247 |
|
---|
248 | unsigned long CARng2, CARng1;
|
---|
249 | short retval = 0;
|
---|
250 | double dtemp;
|
---|
251 |
|
---|
252 |
|
---|
253 | CGPS_Transform::CAFlag = JPL_xtractLongVal(0, 1, CAStr);
|
---|
254 | CGPS_Transform::ASFlag = JPL_xtractLongVal(1, 1, CAStr);
|
---|
255 | CGPS_Transform::P2Flag = JPL_xtractLongVal(2, 1, CAStr);
|
---|
256 | CGPS_Transform::P1Flag = JPL_xtractLongVal(3, 1, CAStr);
|
---|
257 |
|
---|
258 | if (CAFlag)
|
---|
259 | {
|
---|
260 | //Read most significant bits
|
---|
261 | CARng2 = JPL_xtractLongVal(4, 4, CAStr);
|
---|
262 | //Read an int's worth of data
|
---|
263 | CARng1 = JPL_xtractLongVal (8,32,CAStr);
|
---|
264 | // if (f_IsLittleEndian == false)
|
---|
265 | // {
|
---|
266 | //KML June 8/2004
|
---|
267 | //Added this code to deal with Big Endian architectures
|
---|
268 | // SwitchBytes( (char *) &CARng2, sizeof(CARng2) );
|
---|
269 | // SwitchBytes( (char *) &CARng1, sizeof(CARng1) );
|
---|
270 | // }
|
---|
271 |
|
---|
272 | dtemp = 0.0;
|
---|
273 | dtemp = CARng2;
|
---|
274 |
|
---|
275 | CA_Rng = dtemp*pow ((double)2,32);
|
---|
276 | CA_Rng += CARng1;
|
---|
277 |
|
---|
278 | CA_Rng /= 1000; //CA in metres
|
---|
279 | }
|
---|
280 | else
|
---|
281 | {
|
---|
282 | retval = -1;
|
---|
283 | }
|
---|
284 | return retval;
|
---|
285 | }
|
---|
286 |
|
---|
287 | //
|
---|
288 | ////////////////////////////////////////////////////////////////////////////
|
---|
289 | short CGPS_Transform::P1_P2_Block_Extract(char * P1P2Str, double CA, double &Rng , double &Phase, double &RngF2Delta,short decode_F1orF2Flag )
|
---|
290 | {
|
---|
291 | short retval =0;
|
---|
292 | short PhaseOverFlowFlag;
|
---|
293 | long SignFlag,temp;
|
---|
294 |
|
---|
295 | double RngDelta, PhaseDelta;
|
---|
296 |
|
---|
297 | if (decode_F1orF2Flag == 1)
|
---|
298 | {
|
---|
299 | PhaseOverFlowFlag = CGPS_Transform::P1Flag;
|
---|
300 | }
|
---|
301 | else if (decode_F1orF2Flag == 2)
|
---|
302 | {
|
---|
303 | PhaseOverFlowFlag = CGPS_Transform::P2Flag;
|
---|
304 | }
|
---|
305 | //*****************************
|
---|
306 | // Decode Pseudo Range
|
---|
307 | //*****************************
|
---|
308 | SignFlag = JPL_xtractLongVal (0,1,P1P2Str);
|
---|
309 | temp = JPL_xtractLongVal (1,17,P1P2Str);
|
---|
310 |
|
---|
311 | //KML June 8/2004
|
---|
312 | // if (f_IsLittleEndian == false)
|
---|
313 | // {
|
---|
314 | //Added this code to deal with Big Endian architectures
|
---|
315 | // SwitchBytes( (char *) &temp, sizeof(temp) );
|
---|
316 | // }
|
---|
317 |
|
---|
318 |
|
---|
319 | RngDelta = temp;
|
---|
320 | RngDelta /= 1000.0;
|
---|
321 |
|
---|
322 |
|
---|
323 | if (SignFlag)
|
---|
324 | {
|
---|
325 | RngDelta *= -1;
|
---|
326 | }
|
---|
327 |
|
---|
328 |
|
---|
329 | if (decode_F1orF2Flag == 2)
|
---|
330 | {
|
---|
331 | RngF2Delta = RngDelta;
|
---|
332 | }
|
---|
333 |
|
---|
334 | Rng = CA + RngDelta;
|
---|
335 |
|
---|
336 | //***************************
|
---|
337 | // Decode Phase
|
---|
338 | //***************************
|
---|
339 |
|
---|
340 | SignFlag = JPL_xtractLongVal (18,1, P1P2Str);
|
---|
341 | temp = JPL_xtractLongVal (19,21, P1P2Str);
|
---|
342 | // if (f_IsLittleEndian == false)
|
---|
343 | // {
|
---|
344 |
|
---|
345 | //KML June 8th 2004
|
---|
346 | //Added this code to deal with Big Endian architectures
|
---|
347 | // SwitchBytes( (char *) &temp, sizeof(temp) );
|
---|
348 | // }
|
---|
349 |
|
---|
350 | PhaseDelta = temp;
|
---|
351 |
|
---|
352 | PhaseDelta = PhaseDelta * 2 / 100000;
|
---|
353 |
|
---|
354 |
|
---|
355 |
|
---|
356 |
|
---|
357 | //Phase overflow add to phase
|
---|
358 | if(PhaseOverFlowFlag)
|
---|
359 | {
|
---|
360 | PhaseDelta += MAXL1L2;
|
---|
361 | }
|
---|
362 |
|
---|
363 | if (SignFlag)
|
---|
364 | {
|
---|
365 | PhaseDelta *= -1;
|
---|
366 | }
|
---|
367 |
|
---|
368 | if (decode_F1orF2Flag == 1)
|
---|
369 | {
|
---|
370 | // frequency 1
|
---|
371 | Phase = (CA - (ScaleFactor2*RngF2Delta)+ PhaseDelta) / L1;
|
---|
372 | }
|
---|
373 | else if (decode_F1orF2Flag == 2)
|
---|
374 | {
|
---|
375 | // frequency 2
|
---|
376 | Phase = (CA - (ScaleFactor1*RngF2Delta)+ PhaseDelta) / L2;
|
---|
377 | }
|
---|
378 | else
|
---|
379 | {
|
---|
380 | retval =-1;
|
---|
381 | }
|
---|
382 | return retval;
|
---|
383 | }
|
---|
384 |
|
---|
385 | //
|
---|
386 | ////////////////////////////////////////////////////////////////////////////
|
---|
387 | short CGPS_Transform::Decode_RTIGS_Sta(unsigned char *RTIGS_Str, unsigned short RTIGS_Bytes, RTIGSS_T &rtigs_sta)
|
---|
388 | {
|
---|
389 | short retval = 1;
|
---|
390 | memcpy ((void *)&rtigs_sta.rec_id, &RTIGS_Str[0], (sizeof(RTIGSS_T) - sizeof(rtigs_sta.data)));
|
---|
391 | if (f_IsLittleEndian)
|
---|
392 | {
|
---|
393 | SwitchIGS_Sta_HdrBytes( &rtigs_sta);
|
---|
394 | }
|
---|
395 | if (rtigs_sta.rec_id == 100)
|
---|
396 | {
|
---|
397 | if (rtigs_sta.sta_rec_type ==0 )
|
---|
398 | {
|
---|
399 | rtigs_sta.data = NULL;
|
---|
400 | }
|
---|
401 | else
|
---|
402 | {
|
---|
403 | retval = -2; //no other type supported at this time
|
---|
404 | }
|
---|
405 | }
|
---|
406 |
|
---|
407 |
|
---|
408 | else
|
---|
409 | {
|
---|
410 |
|
---|
411 |
|
---|
412 | retval = -1;
|
---|
413 |
|
---|
414 | }
|
---|
415 |
|
---|
416 | return retval ;
|
---|
417 | }
|
---|
418 |
|
---|
419 | //
|
---|
420 | ////////////////////////////////////////////////////////////////////////////
|
---|
421 | short CGPS_Transform::Decode_RTIGS_Soc_Obs(unsigned char *SocStr, short &StrPos, short CMeasIndex, short SocBytes, unsigned long GPSTime)
|
---|
422 | {
|
---|
423 | short retval =1;
|
---|
424 | double CA, RngF2Delta, PhaseL1,PhaseL2, P1, P2;
|
---|
425 | //static unsigned short PhaseArcStartTime[MAXSV]; moved to class header
|
---|
426 | JPL_COMP_OBS_T GPSObs;
|
---|
427 | //printf("String pos %hd Total Bytes %hd\n", StrPos,SocBytes);
|
---|
428 |
|
---|
429 | if ((StrPos <= SocBytes) && ((CMeasIndex >= 0 ) && (CMeasIndex < MAXSV )))
|
---|
430 | {
|
---|
431 |
|
---|
432 | memcpy((void *)&GPSObs.prn, (void *)&SocStr[StrPos], 1);
|
---|
433 |
|
---|
434 | if ((GPSObs.prn > 0 ) && (GPSObs.prn <= 32))
|
---|
435 | {
|
---|
436 | StrPos+=1;
|
---|
437 |
|
---|
438 | memcpy((void *)&GPSObs.epoch_sequence, (void *)&SocStr[StrPos],2);
|
---|
439 |
|
---|
440 | if (f_IsLittleEndian == false)
|
---|
441 | {
|
---|
442 | //KML June 8/2003
|
---|
443 | //Added this code to deal with Big Endian architectures
|
---|
444 | SwitchBytes( (char *) &GPSObs.epoch_sequence, sizeof(GPSObs.epoch_sequence) );
|
---|
445 | }
|
---|
446 |
|
---|
447 | //******************************
|
---|
448 | // Read and decode CA
|
---|
449 | //******************************
|
---|
450 | StrPos+=2;
|
---|
451 | memcpy((void *)&GPSObs.ca_range, (void *)&SocStr[StrPos],5);
|
---|
452 | CA_Extract(GPSObs.ca_range, CA);
|
---|
453 |
|
---|
454 | if (CGPS_Transform::CAFlag) //Defined in the Class by CA_Extract
|
---|
455 | {
|
---|
456 | //************************************
|
---|
457 | // Read CA SNR
|
---|
458 | //************************************
|
---|
459 | StrPos+=5;
|
---|
460 | memcpy((void *)&GPSObs.CA_snr, (void *)&SocStr[StrPos],1);
|
---|
461 | //************************************
|
---|
462 | // Read and decode P2 L2
|
---|
463 | //************************************
|
---|
464 | StrPos+=1;
|
---|
465 | memcpy((void *)&GPSObs.L2_range_phase, (void *)&SocStr[StrPos],5);
|
---|
466 |
|
---|
467 | P1_P2_Block_Extract(GPSObs.L2_range_phase, CA, P2 , PhaseL2, RngF2Delta, 2 );
|
---|
468 |
|
---|
469 |
|
---|
470 | StrPos+=5;
|
---|
471 | memcpy((void *)&GPSObs.L2_snr, (void *)&SocStr[StrPos],1);
|
---|
472 | //************************************
|
---|
473 | // Read and decode P1 L1
|
---|
474 | //************************************
|
---|
475 | StrPos+=1;
|
---|
476 |
|
---|
477 |
|
---|
478 | memcpy((void *)&GPSObs.L1_range_phase, (void *)&SocStr[StrPos],5);
|
---|
479 |
|
---|
480 | P1_P2_Block_Extract(GPSObs.L1_range_phase, CA, P1, PhaseL1, RngF2Delta, 1);
|
---|
481 |
|
---|
482 | StrPos+=5;
|
---|
483 | memcpy((void *)&GPSObs.L1_snr, (void *)&SocStr[StrPos],1);
|
---|
484 | StrPos+=1;
|
---|
485 |
|
---|
486 | DecObs.Obs[CMeasIndex].GPSTime = GPSTime; /* broadcast time sec.*/
|
---|
487 | DecObs.Obs[CMeasIndex].chn = CMeasIndex + 1; /* Channel not real*/
|
---|
488 | DecObs.Obs[CMeasIndex].sat_prn = GPSObs.prn; /* satellite ID*/
|
---|
489 | DecObs.Obs[CMeasIndex].ntrvl = 1; /* number of seconds Changed from 0 to 1 Nov. 25/2003*/
|
---|
490 | DecObs.Obs[CMeasIndex].flag[0] = 4; /*observation quality flags*/ //KML Changed Nov. 25/2000 to 4 to indicate Benchmark
|
---|
491 |
|
---|
492 | if (PhaseArcStartTime[(short)(GPSObs.prn-1)] != GPSObs.epoch_sequence)
|
---|
493 | {
|
---|
494 | PhaseArcStartTime[(short)(GPSObs.prn-1)] = GPSObs.epoch_sequence;
|
---|
495 | DecObs.Obs[CMeasIndex].flag[0] |= 0x20;
|
---|
496 | }
|
---|
497 |
|
---|
498 | DecObs.Obs[CMeasIndex].l1_pseudo_range = CA; /* frequency-1 CA pseudorange */
|
---|
499 | DecObs.Obs[CMeasIndex].l1_phase = PhaseL1; /* frequency-1 CA carrier phase */
|
---|
500 | //****************************************************
|
---|
501 | // Changed SNR Ashtech to DBHz Nov 15/2002
|
---|
502 | //****************************************************
|
---|
503 | DecObs.Obs[CMeasIndex].l1_sn = GPSObs.CA_snr;
|
---|
504 |
|
---|
505 | DecObs.Obs[CMeasIndex].p1_pseudo_range = P1; /* frequency-1 P1 carrier phase */
|
---|
506 | DecObs.Obs[CMeasIndex].p1_phase = PhaseL1; /* frequency-1 P1 pseudorange */
|
---|
507 |
|
---|
508 | DecObs.Obs[CMeasIndex].p1_sn = GPSObs.L1_snr;
|
---|
509 |
|
---|
510 | DecObs.Obs[CMeasIndex].l2_pseudo_range = P2; /* frequency-2 pseudorange (XCorr) */
|
---|
511 | DecObs.Obs[CMeasIndex].l2_phase = PhaseL2; /* frequency-2 carrier phase (XCorr) */
|
---|
512 |
|
---|
513 | DecObs.Obs[CMeasIndex].l2_sn = GPSObs.L2_snr;
|
---|
514 | DecObs.Obs[CMeasIndex].p2_pseudo_range = P2; /* frequency-2 pseudorange */
|
---|
515 | DecObs.Obs[CMeasIndex].p2_phase = PhaseL2; /* frequency-2 carrier phase */
|
---|
516 | }
|
---|
517 | else
|
---|
518 | {
|
---|
519 | //skip this obs
|
---|
520 | DecObs.Obs[CMeasIndex].sat_prn = 0;
|
---|
521 | }
|
---|
522 | }
|
---|
523 | else
|
---|
524 | {
|
---|
525 | retval = -2;
|
---|
526 |
|
---|
527 | }
|
---|
528 | }
|
---|
529 | else
|
---|
530 | {
|
---|
531 | retval = -1;
|
---|
532 | }
|
---|
533 | return retval;
|
---|
534 | }
|
---|
535 |
|
---|
536 | //
|
---|
537 | ////////////////////////////////////////////////////////////////////////////
|
---|
538 | short CGPS_Transform::RTIGSO_Str_To_CMEAS(unsigned char *RTIGSO_Str, short RTIGS_Bytes, RTIGSO_T &rtigs_obs)
|
---|
539 | {
|
---|
540 | short retval =1,i, StrPos;//, HdrRetval;
|
---|
541 | //short NumObs= 0;
|
---|
542 | short decoded_cnt = 0;
|
---|
543 | short IGSObsMinusPtr;
|
---|
544 |
|
---|
545 |
|
---|
546 | //************************************
|
---|
547 | // Zero out CMEAS_T container
|
---|
548 | //************************************
|
---|
549 | memset((void *)&DecObs.Obs[0], 0 , sizeof(ARR_OBS_T) );
|
---|
550 |
|
---|
551 | //***********************************************
|
---|
552 | // Decode Header store in class container
|
---|
553 | //***********************************************
|
---|
554 |
|
---|
555 |
|
---|
556 | StrPos = IGSObsMinusPtr = sizeof(RTIGSO_T) - sizeof (rtigs_obs.data);
|
---|
557 |
|
---|
558 | memcpy ((void *)&rtigs_obs.rec_id, RTIGSO_Str, IGSObsMinusPtr);
|
---|
559 |
|
---|
560 | if (f_IsLittleEndian)
|
---|
561 | {
|
---|
562 | SwitchIGS_Obs_HdrBytes( &rtigs_obs);
|
---|
563 | }
|
---|
564 |
|
---|
565 |
|
---|
566 | // printf("RecNumber : %hd Station ID %hd Num Obs %hd NumBytes %hd\n",rtigs_obs.rec_id, rtigs_obs.sta_id, rtigs_obs.num_obs, rtigs_obs.num_bytes);
|
---|
567 | if((rtigs_obs.rec_id == 200) && (rtigs_obs.num_obs <= MAXCHANNELS_FOR_SOCKETS_TYPE1))
|
---|
568 | {
|
---|
569 | for (i = 0 ; i < rtigs_obs.num_obs;i++)
|
---|
570 | {
|
---|
571 | //*********************************************
|
---|
572 | // the following function decodes the soc
|
---|
573 | // structure and writes the obs to the
|
---|
574 | // class's CMEAS container
|
---|
575 | //*********************************************
|
---|
576 |
|
---|
577 | if (Decode_RTIGS_Soc_Obs( RTIGSO_Str, StrPos, decoded_cnt, RTIGS_Bytes, rtigs_obs.GPSTime) < 0)
|
---|
578 | {
|
---|
579 | retval = -2;
|
---|
580 | }
|
---|
581 | else
|
---|
582 | {
|
---|
583 | decoded_cnt ++;
|
---|
584 | }
|
---|
585 | }//end of for
|
---|
586 | retval = NumObsRead = decoded_cnt; //NumObsRead class member
|
---|
587 | }
|
---|
588 | else
|
---|
589 | {
|
---|
590 | retval = -1;
|
---|
591 | }
|
---|
592 | //ObsSeqNum++;
|
---|
593 | return retval;
|
---|
594 | }
|
---|
595 |
|
---|
596 |
|
---|
597 | //
|
---|
598 | ////////////////////////////////////////////////////////////////////////////
|
---|
599 | short CGPS_Transform::Decode_RTIGS_Obs(unsigned char *RTIGSO_Str, unsigned short RTIGS_Bytes,RTIGSO_T &rtigs_obs)
|
---|
600 | {
|
---|
601 | short retval = 1;//, i;
|
---|
602 |
|
---|
603 |
|
---|
604 | if ((retval = RTIGSO_Str_To_CMEAS(RTIGSO_Str, RTIGS_Bytes, rtigs_obs)) < 0)
|
---|
605 | {
|
---|
606 | retval = -1;
|
---|
607 | }
|
---|
608 |
|
---|
609 | return retval;
|
---|
610 | }
|
---|
611 |
|
---|
612 | //
|
---|
613 | ////////////////////////////////////////////////////////////////////////////
|
---|
614 | short CGPS_Transform::Decode_RTIGS_Met(unsigned char *RTIGS_Str, unsigned short RTIGS_Bytes, RTIGSM_T *rtigs_met)
|
---|
615 | {
|
---|
616 | short retval = 1;
|
---|
617 | short numbytes = 0;
|
---|
618 | numbytes = sizeof(RTIGSM_T) - sizeof(rtigs_met->mets);
|
---|
619 | memcpy ((void *)rtigs_met, RTIGS_Str, numbytes);
|
---|
620 |
|
---|
621 | if ((short)rtigs_met->numobs <= 3)
|
---|
622 | {
|
---|
623 | if (rtigs_met->mets != NULL)
|
---|
624 | {
|
---|
625 | memcpy ((void *)&rtigs_met->mets[0], &RTIGS_Str[numbytes], ((short)rtigs_met->numobs * sizeof(long)) );
|
---|
626 | if (f_IsLittleEndian)
|
---|
627 | {
|
---|
628 | SwitchIGS_Met_RecBytes( rtigs_met);
|
---|
629 | }
|
---|
630 | if (rtigs_met->rec_id != 400)
|
---|
631 | {
|
---|
632 | retval = -1;
|
---|
633 | }
|
---|
634 | }
|
---|
635 | else
|
---|
636 | {
|
---|
637 | retval = -2;
|
---|
638 | delete [] rtigs_met->mets;
|
---|
639 | }
|
---|
640 | }
|
---|
641 | else
|
---|
642 | {
|
---|
643 | printf("failed number of Obs\n");
|
---|
644 | }
|
---|
645 |
|
---|
646 | return retval;
|
---|
647 | }
|
---|
648 |
|
---|
649 | //
|
---|
650 | ////////////////////////////////////////////////////////////////////////////
|
---|
651 | short CGPS_Transform::Decode_RTIGS_Eph(unsigned char *RTIGS_Str, unsigned short RTIGS_Bytes, RTIGSE_T &rtigs_eph, short &PRN)
|
---|
652 | {
|
---|
653 | short retval = 1;//, i;
|
---|
654 | short index = 0;
|
---|
655 | short prn;
|
---|
656 | const short SubFrameSize = 24;
|
---|
657 | TNAV_T trans_eph;
|
---|
658 | index = sizeof(RTIGSE_T ) - sizeof (rtigs_eph.data);
|
---|
659 |
|
---|
660 | memcpy ((void *)&rtigs_eph.rec_id, &RTIGS_Str[0], index); //copy header into struct from string
|
---|
661 | if (f_IsLittleEndian)
|
---|
662 | {
|
---|
663 | SwitchIGS_Eph_HdrBytes( &rtigs_eph);
|
---|
664 | }
|
---|
665 |
|
---|
666 | if (rtigs_eph.rec_id == 300)
|
---|
667 | {
|
---|
668 | //*********************************************
|
---|
669 | // the following method saves the eph
|
---|
670 | // in the class's TNAV_T container
|
---|
671 | //*********************************************
|
---|
672 | trans_eph.GPSCollectedTime = rtigs_eph.CollectedGPSTime;
|
---|
673 | trans_eph. Satellite = (long)rtigs_eph.prn;
|
---|
674 | //********************************************
|
---|
675 | // Container class is in network byte order
|
---|
676 | //********************************************
|
---|
677 | if (f_IsLittleEndian)
|
---|
678 | {
|
---|
679 | SwitchBytes( (char *)&trans_eph.GPSCollectedTime, sizeof(trans_eph.GPSCollectedTime) );
|
---|
680 | SwitchBytes( (char *)&trans_eph. Satellite, sizeof(trans_eph. Satellite) );
|
---|
681 | }
|
---|
682 |
|
---|
683 | memcpy((void *)&trans_eph.SubFrame1, (const void *)&RTIGS_Str[index], SubFrameSize);
|
---|
684 | memcpy((void *)&trans_eph.SubFrame2, (const void *)&RTIGS_Str[(index + SubFrameSize)], SubFrameSize);
|
---|
685 | memcpy((void *)&trans_eph.SubFrame3, (const void *)&RTIGS_Str[(index + (SubFrameSize * 2)) ], SubFrameSize);
|
---|
686 | if (Save_TNAV_T_To_Container(&trans_eph, prn) >= 1) //function saves eph in container and returns prn
|
---|
687 | {
|
---|
688 | PRN = prn;
|
---|
689 | }
|
---|
690 | else
|
---|
691 | {
|
---|
692 | retval = -1;
|
---|
693 | }
|
---|
694 | }
|
---|
695 | else
|
---|
696 | {
|
---|
697 | retval = -1;
|
---|
698 | }
|
---|
699 |
|
---|
700 | return retval;
|
---|
701 | }
|
---|
702 |
|
---|
703 | void CGPS_Transform::print_CMEAS()
|
---|
704 | {
|
---|
705 | short i;
|
---|
706 | printf("\nGPSTime SV CA SNR P1 SNR P2 SNR\n");
|
---|
707 | printf("Seconds (m) DBHz (m) DBHz (m) DBHz\n");
|
---|
708 | for (i=0; i < NumObsRead ; i++)
|
---|
709 | {
|
---|
710 | printf("%ld %2hd %10.1lf %4.1f %10.1lf %4.1f %10.1lf %4.1f \n",DecObs.Obs[i].GPSTime, DecObs.Obs[i].sat_prn,
|
---|
711 | DecObs.Obs[i].l1_pseudo_range,DecObs.Obs[i].l1_sn,
|
---|
712 | DecObs.Obs[i].p1_pseudo_range, DecObs.Obs[i].p1_sn,
|
---|
713 | DecObs.Obs[i].l2_pseudo_range,DecObs.Obs[i].l2_sn);
|
---|
714 | }
|
---|
715 | }
|
---|
716 |
|
---|