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