source: ntrip/trunk/BNC/RTIGS/cgps_transform.cpp@ 670

Last change on this file since 670 was 670, checked in by weber, 16 years ago

* empty log message *
File size: 34.8 KB
Line 
1#include "cgps_transform.h"
2
3#define MAXSTREAM 10000
4
5////////////////////////////////////////////////////////////////////////////
6void SwitchBytes( char *Start, int Size ) {
7 char Tmp;
8 char *End = Start + Size - 1;
9 for( Tmp = *Start; Start < End; Tmp = *Start ){
10 *Start++ = *End;
11 *End-- = Tmp;
12 }
13}
14
15#ifdef CGPS_TRANSFORM_MAIN
16int main() {
17
18 unsigned char data_stream[MAXSTREAM];
19 unsigned short numbytes;
20 RTIGSS_T rtigs_sta;
21 RTIGSO_T rtigs_obs;
22 RTIGSM_T rtigs_met;
23 RTIGSE_T rtigs_eph;
24 short PRN;
25 short retval;
26 unsigned short statID;
27 unsigned short messType;
28 CGPS_Transform GPSTrans;
29
30 memset(data_stream , 0, sizeof(data_stream));
31
32 // use something like recvfrom
33 FILE* inpFile = fopen("RTIGS.txt", "rb");
34
35 while (true) {
36 size_t nr = 0;
37 if (inpFile) {
38 nr = fread(data_stream, sizeof(unsigned char), MAXSTREAM, inpFile);
39 if (nr == 0) exit(0);
40 cout << "Number of bytes read: " << nr << endl;
41 }
42 else {
43 exit(1);
44 }
45
46 // Find the beginning of the message
47 // ---------------------------------
48 size_t sz = sizeof(unsigned short);
49 bool found = false;
50 size_t ii;
51 for (ii = 0; ii < nr - sz; ii += sz) {
52 unsigned short xx;
53 memcpy( (void*) &xx, &data_stream[ii], sz);
54 SwitchBytes( (char*) &xx, sz);
55 if (xx == 200) {
56 found = true;
57 break;
58 }
59 }
60 if (! found) {
61 cout << "Message not found\n";
62 exit(0);
63 }
64 else {
65 cout << "Message found at " << ii << endl;
66 }
67
68
69 messType = GPSTrans.GetRTIGSHdrRecType(&data_stream[ii]);
70 numbytes = GPSTrans.GetRTIGSHdrRecBytes(&data_stream[ii]);
71 statID = GPSTrans.GetRTIGSHdrStaID(&data_stream[ii]);
72
73 cout << "messType " << messType << endl;
74 cout << "numbytes " << numbytes << endl;
75 cout << "statID " << statID << endl;
76
77 switch (messType) {
78 case 100:
79 GPSTrans.Decode_RTIGS_Sta(&data_stream[ii], numbytes , rtigs_sta);
80 break;
81 case 200:
82 retval = GPSTrans.Decode_RTIGS_Obs(&data_stream[ii], numbytes , rtigs_obs);
83 if (retval >= 1) {
84 GPSTrans.print_CMEAS();
85 }
86 break;
87 case 300:
88 retval = GPSTrans.Decode_RTIGS_Eph(&data_stream[ii], numbytes , rtigs_eph, PRN);
89 break;
90 case 400:
91 retval = GPSTrans.Decode_RTIGS_Met(&data_stream[ii], numbytes , &rtigs_met);
92 break;
93 }
94 }
95
96 return 0;
97}
98#endif
99
100// Constructor
101////////////////////////////////////////////////////////////////////////////
102CGPS_Transform::CGPS_Transform() {
103
104 // Decoded Obs Array of 12 CMeas Observation Records
105 memset((void *)&DecObs, 0, sizeof(ARR_OBS_T ));
106
107 // Keplarian Broadcast Eph
108 memset((void *)&TNAV_Eph,0, sizeof(ARR_TNAV_T ));
109
110 NumObsRead = -1;
111 CAFlag = -1;
112 ASFlag = -1;
113 P2Flag = -1;
114 P1Flag = -1;
115 InitEndianFlag();
116
117 memset (PhaseArcStartTime, 0, sizeof(PhaseArcStartTime));
118}
119
120// Destructor
121////////////////////////////////////////////////////////////////////////////
122CGPS_Transform::~CGPS_Transform() {
123}
124
125//
126////////////////////////////////////////////////////////////////////////////
127unsigned short CGPS_Transform::GetRTIGSHdrRecType(unsigned char *RTIGS_Str)
128{
129 unsigned short recordID;
130 memcpy ((void *)&recordID,RTIGS_Str, sizeof(recordID));
131 if (f_IsLittleEndian)
132 {
133 SwitchBytes( (char *)&recordID, sizeof(recordID) );
134 }
135 return recordID;
136}
137
138//
139////////////////////////////////////////////////////////////////////////////
140unsigned short CGPS_Transform::GetRTIGSHdrRecBytes(unsigned char *RTIGS_Str)
141{
142 unsigned short bytes;
143 memcpy ((void *)&bytes,&RTIGS_Str[8], sizeof(bytes));
144 if (f_IsLittleEndian)
145 {
146 SwitchBytes( (char *)&bytes, sizeof(bytes) );
147 }
148 return bytes;
149
150}
151
152//
153////////////////////////////////////////////////////////////////////////////
154unsigned short CGPS_Transform::GetRTIGSHdrStaID(unsigned char *RTIGS_Str)
155{
156 unsigned short StaID = 0;
157 memcpy ((void *)&StaID, &RTIGS_Str[2], sizeof(StaID));
158 if (f_IsLittleEndian)
159 {
160 SwitchBytes( (char *)&StaID, sizeof(StaID) );
161 }
162 return StaID;
163}
164
165
166//
167////////////////////////////////////////////////////////////////////////////
168void CGPS_Transform::InitEndianFlag() {
169 short one = 1;
170 char *cp = (char *)&one;
171 if (*cp== 0) {
172 f_IsLittleEndian = false;
173 }
174 else {
175 f_IsLittleEndian = true;
176 }
177}
178
179
180//
181////////////////////////////////////////////////////////////////////////////
182unsigned long CGPS_Transform::JPL_xtractLongVal (unsigned startBitNbr, unsigned xtractNbrBits, const char *msg)
183{
184 unsigned long retValue=0, i=0;
185
186 unsigned short posBit = xtractNbrBits - 1;
187
188 for(i=0; i<xtractNbrBits; i++, startBitNbr++)
189 {
190 unsigned short numShift = 7 - (startBitNbr % 8);
191 unsigned short byteNbr = startBitNbr / 8;
192 retValue |= (((msg[byteNbr] >> numShift) & 0x0001) << posBit--);
193
194 }
195
196 return retValue;
197}
198
199//
200////////////////////////////////////////////////////////////////////////////
201inline void CGPS_Transform::SwitchIGS_Sta_HdrBytes( RTIGSS_T *StaHdr)
202{
203 SwitchBytes( (char *)&StaHdr->GPSTime, sizeof(unsigned long) );
204 SwitchBytes( (char *)&StaHdr->num_bytes, sizeof(unsigned short) );
205 SwitchBytes( (char *)&StaHdr->rec_id, sizeof(unsigned short) );
206 SwitchBytes( (char *)&StaHdr->sta_id, sizeof(unsigned short) );
207}
208
209//
210////////////////////////////////////////////////////////////////////////////
211inline void CGPS_Transform::SwitchIGS_Obs_HdrBytes( RTIGSO_T *ObsHdr)
212{
213 SwitchBytes( (char *)&ObsHdr->GPSTime, sizeof(unsigned long) );
214 SwitchBytes( (char *)&ObsHdr->num_bytes, sizeof(unsigned short) );
215 SwitchBytes( (char *)&ObsHdr->rec_id, sizeof(unsigned short) );
216 SwitchBytes( (char *)&ObsHdr->sta_id, sizeof(unsigned short) );
217}
218
219//
220////////////////////////////////////////////////////////////////////////////
221inline void CGPS_Transform::SwitchIGS_Eph_HdrBytes( RTIGSE_T *EphHdr)
222
223{
224 SwitchBytes( (char *)&EphHdr->CollectedGPSTime, sizeof(unsigned long) );
225 SwitchBytes( (char *)&EphHdr->num_bytes, sizeof(unsigned short) );
226 SwitchBytes( (char *)&EphHdr->rec_id, sizeof(unsigned short) );
227 SwitchBytes( (char *)&EphHdr->sta_id, sizeof(unsigned short) );
228}
229
230//
231////////////////////////////////////////////////////////////////////////////
232inline short CGPS_Transform::SwitchIGS_Met_RecBytes( RTIGSM_T *MetHdr)
233{
234short retval = 1;
235short i, num_items;
236 num_items = (short)MetHdr->numobs;
237 SwitchBytes( (char *)&MetHdr->GPSTime, sizeof(unsigned long) );
238 SwitchBytes( (char *)&MetHdr->num_bytes, sizeof(unsigned short) );
239 SwitchBytes( (char *)&MetHdr->rec_id, sizeof(unsigned short) );
240 SwitchBytes( (char *)&MetHdr->sta_id, sizeof(unsigned short) );
241
242 /*switch met data bytes*/
243 for (i=0; i < num_items; i++)
244 {
245 if (&MetHdr->mets[i] != NULL)
246 {
247 SwitchBytes( (char *)&MetHdr->mets[i], sizeof(long) );
248 }
249 else
250 {
251 retval = -1;
252 }
253
254 }
255return retval;
256}
257//
258////////////////////////////////////////////////////////////////////////////
259
260short CGPS_Transform::Save_TNAV_T_To_Container(TNAV_T *rtcurrent_eph, short &prn)
261{
262short retval = 1;//, i;
263long PRN;
264
265 PRN = rtcurrent_eph->Satellite;
266
267 if (f_IsLittleEndian)
268 {
269 SwitchBytes( (char *)&PRN, sizeof(PRN));
270 }
271
272 if ((PRN > 0) && (PRN <= 32))
273 {
274 memcpy( (void *)&TNAV_Eph.Eph[(PRN-1)] ,rtcurrent_eph,sizeof(TNAV_T));
275 prn = (short)PRN;
276 }
277 else
278 {
279 retval = -1;
280 }
281return retval;
282}
283
284//
285////////////////////////////////////////////////////////////////////////////
286short CGPS_Transform::CA_Extract(char * CAStr, double &CA_Rng)
287{
288
289
290unsigned long CARng2, CARng1;
291short retval = 0;
292double dtemp;
293
294
295 CGPS_Transform::CAFlag = JPL_xtractLongVal(0, 1, CAStr);
296 CGPS_Transform::ASFlag = JPL_xtractLongVal(1, 1, CAStr);
297 CGPS_Transform::P2Flag = JPL_xtractLongVal(2, 1, CAStr);
298 CGPS_Transform::P1Flag = JPL_xtractLongVal(3, 1, CAStr);
299
300 if (CAFlag)
301 {
302 //Read most significant bits
303 CARng2 = JPL_xtractLongVal(4, 4, CAStr);
304 //Read an int's worth of data
305 CARng1 = JPL_xtractLongVal (8,32,CAStr);
306// if (f_IsLittleEndian == false)
307// {
308 //KML June 8/2004
309 //Added this code to deal with Big Endian architectures
310// SwitchBytes( (char *) &CARng2, sizeof(CARng2) );
311// SwitchBytes( (char *) &CARng1, sizeof(CARng1) );
312// }
313
314 dtemp = 0.0;
315 dtemp = CARng2;
316
317 CA_Rng = dtemp*pow ((double)2,32);
318 CA_Rng += CARng1;
319
320 CA_Rng /= 1000; //CA in metres
321 }
322 else
323 {
324 retval = -1;
325 }
326return retval;
327}
328
329//
330////////////////////////////////////////////////////////////////////////////
331short CGPS_Transform::P1_P2_Block_Extract(char * P1P2Str, double CA, double &Rng , double &Phase, double &RngF2Delta,short decode_F1orF2Flag )
332{
333short retval =0;
334short PhaseOverFlowFlag = -1;
335long SignFlag,temp;
336
337double RngDelta, PhaseDelta;
338
339 if (decode_F1orF2Flag == 1)
340 {
341 PhaseOverFlowFlag = CGPS_Transform::P1Flag;
342 }
343 else if (decode_F1orF2Flag == 2)
344 {
345 PhaseOverFlowFlag = CGPS_Transform::P2Flag;
346 }
347 //*****************************
348 // Decode Pseudo Range
349 //*****************************
350 SignFlag = JPL_xtractLongVal (0,1,P1P2Str);
351 temp = JPL_xtractLongVal (1,17,P1P2Str);
352
353//KML June 8/2004
354// if (f_IsLittleEndian == false)
355// {
356 //Added this code to deal with Big Endian architectures
357// SwitchBytes( (char *) &temp, sizeof(temp) );
358// }
359
360
361 RngDelta = temp;
362 RngDelta /= 1000.0;
363
364
365 if (SignFlag)
366 {
367 RngDelta *= -1;
368 }
369
370
371 if (decode_F1orF2Flag == 2)
372 {
373 RngF2Delta = RngDelta;
374 }
375
376 Rng = CA + RngDelta;
377
378 //***************************
379 // Decode Phase
380 //***************************
381
382 SignFlag = JPL_xtractLongVal (18,1, P1P2Str);
383 temp = JPL_xtractLongVal (19,21, P1P2Str);
384// if (f_IsLittleEndian == false)
385// {
386
387 //KML June 8th 2004
388 //Added this code to deal with Big Endian architectures
389// SwitchBytes( (char *) &temp, sizeof(temp) );
390// }
391
392 PhaseDelta = temp;
393
394 PhaseDelta = PhaseDelta * 2 / 100000;
395
396
397
398
399 //Phase overflow add to phase
400 if(PhaseOverFlowFlag)
401 {
402 PhaseDelta += MAXL1L2;
403 }
404
405 if (SignFlag)
406 {
407 PhaseDelta *= -1;
408 }
409
410 if (decode_F1orF2Flag == 1)
411 {
412 // frequency 1
413 Phase = (CA - (ScaleFactor2*RngF2Delta)+ PhaseDelta) / L1;
414 }
415 else if (decode_F1orF2Flag == 2)
416 {
417 // frequency 2
418 Phase = (CA - (ScaleFactor1*RngF2Delta)+ PhaseDelta) / L2;
419 }
420 else
421 {
422 retval =-1;
423 }
424return retval;
425}
426
427//
428////////////////////////////////////////////////////////////////////////////
429short CGPS_Transform::Decode_RTIGS_Sta(unsigned char *RTIGS_Str, unsigned short RTIGS_Bytes, RTIGSS_T &rtigs_sta)
430{
431short retval = 1;
432 memcpy ((void *)&rtigs_sta.rec_id, &RTIGS_Str[0], (sizeof(RTIGSS_T) - sizeof(rtigs_sta.data)));
433 if (f_IsLittleEndian)
434 {
435 SwitchIGS_Sta_HdrBytes( &rtigs_sta);
436 }
437 if (rtigs_sta.rec_id == 100)
438 {
439 if (rtigs_sta.sta_rec_type ==0 )
440 {
441 rtigs_sta.data = NULL;
442 }
443 else
444 {
445 retval = -2; //no other type supported at this time
446 }
447 }
448
449
450 else
451 {
452
453
454 retval = -1;
455
456 }
457
458return retval ;
459}
460
461//
462////////////////////////////////////////////////////////////////////////////
463short CGPS_Transform::Decode_RTIGS_Soc_Obs(unsigned char *SocStr, short &StrPos, short CMeasIndex, short SocBytes, unsigned long GPSTime)
464{
465short retval =1;
466double CA, RngF2Delta, PhaseL1,PhaseL2, P1, P2;
467//static unsigned short PhaseArcStartTime[MAXSV]; moved to class header
468JPL_COMP_OBS_T GPSObs;
469//printf("String pos %hd Total Bytes %hd\n", StrPos,SocBytes);
470
471 if ((StrPos <= SocBytes) && ((CMeasIndex >= 0 ) && (CMeasIndex < MAXSV )))
472 {
473
474 memcpy((void *)&GPSObs.prn, (void *)&SocStr[StrPos], 1);
475
476 if ((GPSObs.prn > 0 ) && (GPSObs.prn <= 32))
477 {
478 StrPos+=1;
479
480 memcpy((void *)&GPSObs.epoch_sequence, (void *)&SocStr[StrPos],2);
481
482 if (f_IsLittleEndian == false)
483 {
484 //KML June 8/2003
485 //Added this code to deal with Big Endian architectures
486 SwitchBytes( (char *) &GPSObs.epoch_sequence, sizeof(GPSObs.epoch_sequence) );
487 }
488
489 //******************************
490 // Read and decode CA
491 //******************************
492 StrPos+=2;
493 memcpy((void *)&GPSObs.ca_range, (void *)&SocStr[StrPos],5);
494 CA_Extract(GPSObs.ca_range, CA);
495
496 if (CGPS_Transform::CAFlag) //Defined in the Class by CA_Extract
497 {
498 //************************************
499 // Read CA SNR
500 //************************************
501 StrPos+=5;
502 memcpy((void *)&GPSObs.CA_snr, (void *)&SocStr[StrPos],1);
503 //************************************
504 // Read and decode P2 L2
505 //************************************
506 StrPos+=1;
507 memcpy((void *)&GPSObs.L2_range_phase, (void *)&SocStr[StrPos],5);
508
509 P1_P2_Block_Extract(GPSObs.L2_range_phase, CA, P2 , PhaseL2, RngF2Delta, 2 );
510
511
512 StrPos+=5;
513 memcpy((void *)&GPSObs.L2_snr, (void *)&SocStr[StrPos],1);
514 //************************************
515 // Read and decode P1 L1
516 //************************************
517 StrPos+=1;
518
519
520 memcpy((void *)&GPSObs.L1_range_phase, (void *)&SocStr[StrPos],5);
521
522 P1_P2_Block_Extract(GPSObs.L1_range_phase, CA, P1, PhaseL1, RngF2Delta, 1);
523
524 StrPos+=5;
525 memcpy((void *)&GPSObs.L1_snr, (void *)&SocStr[StrPos],1);
526 StrPos+=1;
527
528 DecObs.Obs[CMeasIndex].GPSTime = GPSTime; /* broadcast time sec.*/
529 DecObs.Obs[CMeasIndex].chn = CMeasIndex + 1; /* Channel not real*/
530 DecObs.Obs[CMeasIndex].sat_prn = GPSObs.prn; /* satellite ID*/
531 DecObs.Obs[CMeasIndex].ntrvl = 1; /* number of seconds Changed from 0 to 1 Nov. 25/2003*/
532 DecObs.Obs[CMeasIndex].flag[0] = 4; /*observation quality flags*/ //KML Changed Nov. 25/2000 to 4 to indicate Benchmark
533
534 if (PhaseArcStartTime[(short)(GPSObs.prn-1)] != GPSObs.epoch_sequence)
535 {
536 PhaseArcStartTime[(short)(GPSObs.prn-1)] = GPSObs.epoch_sequence;
537 DecObs.Obs[CMeasIndex].flag[0] |= 0x20;
538 }
539
540 DecObs.Obs[CMeasIndex].l1_pseudo_range = CA; /* frequency-1 CA pseudorange */
541 DecObs.Obs[CMeasIndex].l1_phase = PhaseL1; /* frequency-1 CA carrier phase */
542 //****************************************************
543 // Changed SNR Ashtech to DBHz Nov 15/2002
544 //****************************************************
545 DecObs.Obs[CMeasIndex].l1_sn = GPSObs.CA_snr;
546
547 DecObs.Obs[CMeasIndex].p1_pseudo_range = P1; /* frequency-1 P1 carrier phase */
548 DecObs.Obs[CMeasIndex].p1_phase = PhaseL1; /* frequency-1 P1 pseudorange */
549
550 DecObs.Obs[CMeasIndex].p1_sn = GPSObs.L1_snr;
551
552 DecObs.Obs[CMeasIndex].l2_pseudo_range = P2; /* frequency-2 pseudorange (XCorr) */
553 DecObs.Obs[CMeasIndex].l2_phase = PhaseL2; /* frequency-2 carrier phase (XCorr) */
554
555 DecObs.Obs[CMeasIndex].l2_sn = GPSObs.L2_snr;
556 DecObs.Obs[CMeasIndex].p2_pseudo_range = P2; /* frequency-2 pseudorange */
557 DecObs.Obs[CMeasIndex].p2_phase = PhaseL2; /* frequency-2 carrier phase */
558 }
559 else
560 {
561 //skip this obs
562 DecObs.Obs[CMeasIndex].sat_prn = 0;
563 }
564 }
565 else
566 {
567 retval = -2;
568
569 }
570 }
571 else
572 {
573 retval = -1;
574 }
575return retval;
576}
577
578//
579////////////////////////////////////////////////////////////////////////////
580short CGPS_Transform::RTIGSO_Str_To_CMEAS(unsigned char *RTIGSO_Str, short RTIGS_Bytes, RTIGSO_T &rtigs_obs)
581{
582short retval =1,i, StrPos;//, HdrRetval;
583//short NumObs= 0;
584short decoded_cnt = 0;
585short IGSObsMinusPtr;
586
587
588 //************************************
589 // Zero out CMEAS_T container
590 //************************************
591memset((void *)&DecObs.Obs[0], 0 , sizeof(ARR_OBS_T) );
592
593 //***********************************************
594 // Decode Header store in class container
595 //***********************************************
596
597
598 StrPos = IGSObsMinusPtr = sizeof(RTIGSO_T) - sizeof (rtigs_obs.data);
599
600 //// cout << "StrPos " << StrPos << endl;
601
602 memcpy ((void *)&rtigs_obs.rec_id, RTIGSO_Str, IGSObsMinusPtr);
603
604 if (f_IsLittleEndian)
605 {
606 SwitchIGS_Obs_HdrBytes( &rtigs_obs);
607 }
608
609
610 // 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);
611
612 if((rtigs_obs.rec_id == 200) && (rtigs_obs.num_obs <= MAXCHANNELS_FOR_SOCKETS_TYPE1))
613 {
614 for (i = 0 ; i < rtigs_obs.num_obs;i++)
615 {
616 //*********************************************
617 // the following function decodes the soc
618 // structure and writes the obs to the
619 // class's CMEAS container
620 //*********************************************
621
622 if (Decode_RTIGS_Soc_Obs( RTIGSO_Str, StrPos, decoded_cnt, RTIGS_Bytes, rtigs_obs.GPSTime) < 0)
623 {
624 retval = -2;
625 }
626 else
627 {
628 decoded_cnt ++;
629 }
630 }//end of for
631 retval = NumObsRead = decoded_cnt; //NumObsRead class member
632 }
633 else
634 {
635 retval = -1;
636 }
637//ObsSeqNum++;
638return retval;
639}
640
641
642//
643////////////////////////////////////////////////////////////////////////////
644short CGPS_Transform::Decode_RTIGS_Obs(unsigned char *RTIGSO_Str, unsigned short RTIGS_Bytes,RTIGSO_T &rtigs_obs)
645{
646 short retval = 1;//, i;
647
648
649 if ((retval = RTIGSO_Str_To_CMEAS(RTIGSO_Str, RTIGS_Bytes, rtigs_obs)) < 0)
650 {
651 retval = -1;
652 }
653
654return retval;
655}
656
657//
658////////////////////////////////////////////////////////////////////////////
659short CGPS_Transform::Decode_RTIGS_Met(unsigned char *RTIGS_Str, unsigned short RTIGS_Bytes, RTIGSM_T *rtigs_met)
660{
661short retval = 1;
662short numbytes = 0;
663numbytes = sizeof(RTIGSM_T) - sizeof(rtigs_met->mets);
664memcpy ((void *)rtigs_met, RTIGS_Str, numbytes);
665
666 if ((short)rtigs_met->numobs <= 3)
667 {
668 if (rtigs_met->mets != NULL)
669 {
670 memcpy ((void *)&rtigs_met->mets[0], &RTIGS_Str[numbytes], ((short)rtigs_met->numobs * sizeof(long)) );
671 if (f_IsLittleEndian)
672 {
673 SwitchIGS_Met_RecBytes( rtigs_met);
674 }
675 if (rtigs_met->rec_id != 400)
676 {
677 retval = -1;
678 }
679 }
680 else
681 {
682 retval = -2;
683 delete [] rtigs_met->mets;
684 }
685 }
686 else
687 {
688 printf("failed number of Obs\n");
689 }
690
691return retval;
692}
693
694//
695////////////////////////////////////////////////////////////////////////////
696short CGPS_Transform::Decode_RTIGS_Eph(unsigned char *RTIGS_Str, unsigned short RTIGS_Bytes, RTIGSE_T &rtigs_eph, short &PRN)
697{
698 short retval = 1;//, i;
699 short index = 0;
700 short prn;
701 const short SubFrameSize = 24;
702 TNAV_T trans_eph;
703 index = sizeof(RTIGSE_T ) - sizeof (rtigs_eph.data);
704
705 memcpy ((void *)&rtigs_eph.rec_id, &RTIGS_Str[0], index); //copy header into struct from string
706 if (f_IsLittleEndian)
707 {
708 SwitchIGS_Eph_HdrBytes( &rtigs_eph);
709 }
710
711 if (rtigs_eph.rec_id == 300)
712 {
713 //*********************************************
714 // the following method saves the eph
715 // in the class's TNAV_T container
716 //*********************************************
717 trans_eph.GPSCollectedTime = rtigs_eph.CollectedGPSTime;
718 trans_eph. Satellite = (long)rtigs_eph.prn;
719 //********************************************
720 // Container class is in network byte order
721 //********************************************
722 if (f_IsLittleEndian)
723 {
724 SwitchBytes( (char *)&trans_eph.GPSCollectedTime, sizeof(trans_eph.GPSCollectedTime) );
725 SwitchBytes( (char *)&trans_eph. Satellite, sizeof(trans_eph. Satellite) );
726 }
727
728 memcpy((void *)&trans_eph.SubFrame1, (const void *)&RTIGS_Str[index], SubFrameSize);
729 memcpy((void *)&trans_eph.SubFrame2, (const void *)&RTIGS_Str[(index + SubFrameSize)], SubFrameSize);
730 memcpy((void *)&trans_eph.SubFrame3, (const void *)&RTIGS_Str[(index + (SubFrameSize * 2)) ], SubFrameSize);
731 if (Save_TNAV_T_To_Container(&trans_eph, prn) >= 1) //function saves eph in container and returns prn
732 {
733 PRN = prn;
734 }
735 else
736 {
737 retval = -1;
738 }
739 }
740 else
741 {
742 retval = -1;
743 }
744
745return retval;
746}
747
748void CGPS_Transform::print_CMEAS()
749{
750short i;
751 printf("\nGPSTime SV CA SNR P1 SNR P2 SNR\n");
752 printf("Seconds (m) DBHz (m) DBHz (m) DBHz\n");
753 for (i=0; i < NumObsRead ; i++)
754 {
755 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,
756 DecObs.Obs[i].l1_pseudo_range,DecObs.Obs[i].l1_sn,
757 DecObs.Obs[i].p1_pseudo_range, DecObs.Obs[i].p1_sn,
758 DecObs.Obs[i].l2_pseudo_range,DecObs.Obs[i].l2_sn);
759 }
760}
761
762// 2/1/2008 SPG Start
763
764//*****************************************************************************************
765//
766
767// Function/Method : CGPS_Transform::SwitchEphBytes()
768//
769// Purpose :
770//
771// Returns :
772//
773// Author : Created By KML 2002/06
774//
775// Description:
776//
777//
778
779//
780// Parameters:
781
782//
783//
784//
785//
786//
787//
788// Revision :
789
790//*****************************************************************************************
791
792void CGPS_Transform::SwitchEphBytes( TNAV_T *rnav )
793
794{
795 unsigned long *word;
796 int i, j;
797 SwitchBytes( (char *)&rnav->GPSCollectedTime, sizeof(long) );
798 SwitchBytes( (char *)&rnav->Satellite, sizeof(long) );
799
800 word = (unsigned long *)rnav->SubFrame1;
801
802 for( i = 0; i < 3; i++ ) {
803 for( j = 1; j <= 6; j++, word++ ) {
804
805 SwitchBytes( (char *)word, sizeof(long) );
806 }
807 }
808}
809//*****************************************************************************************
810//
811
812// Function/Method : CGPS_Transform::TNAV_To_BEPH
813//
814// Purpose :
815//
816// Returns :
817//
818// Author : Created By Mark Caissy Modified and put in class by KML 2002/06
819//
820// Description:
821//
822//
823//
824// Parameters:
825
826//
827//
828//
829
830//
831//
832//
833// Revision : KML June 9/2005 added check for PRN num, switch bytes and return
834//*****************************************************************************************
835
836
837short CGPS_Transform::TNAV_To_BEPH( TNAV_T *rtcurrent_eph, BEPH_T *new_eph)
838{
839 TNAV_T temp_eph,
840 *tnav_t_ptr;
841
842 long word,
843 tmp_word1,
844 tmp_word2;
845
846
847 double issue_of_data_clock,
848 issue_of_data_eph1,
849 issue_of_data_eph2,
850 clock_ref_time;
851
852 double svacrcy[] = { 2.0, 2.8, 4.0, 5.7, 8.0, 11.3, 1.6e01, 3.2e01,
853 6.4e01, 1.28e02, 2.56e02, 5.12e02, 1.024e03, 2.048e03, 4.096e03, -1.0 };
854
855 short retval = 1;
856
857 //copy into local variable KML
858 memcpy( &temp_eph, rtcurrent_eph, sizeof(TNAV_T) );
859
860
861 tnav_t_ptr = &temp_eph;
862
863 if (f_IsLittleEndian) //KML Added June 9/2005
864 { //KML
865 SwitchEphBytes( tnav_t_ptr ); //KML
866 } //KML
867
868 //****************************************
869 // Verify that prn of in expected range
870 //****************************************
871 if (((short)tnav_t_ptr->Satellite > 0) && ((short)tnav_t_ptr->Satellite <= 32)) //KML June 9/2005
872 {
873
874 new_eph->transmit_time = GPSEC_UNROLL((tnav_t_ptr->GPSCollectedTime - 18L));
875 /* 18L is used since each subframe takes 6
876 * seconds and there are 3 of them.
877 */
878 /*
879 c process subframe 1
880 c
881 c new_eph[1] = satellite prn number
882 c new_eph[2] = gps week of navigation mesage
883 c new_eph[3] = l2 codes, bits 11-12, + l2pflag*256
884 c new_eph[4] = user range accuracy b13-16 (m)
885
886 c new_eph[5] = navigation health, bit 1
887 c new_eph[6] = l1, l2 correction term (s), scale 2^-31
888 c new_eph->clock_ref_time = aodc (age of data clock)
889 c new_eph[8] = clock reference time
890 c new_eph[9] = clock acceleration (s^-1), scale 2^-55
891 c new_eph[10]= clock rate, (s/s), scale 2^-43
892 c new_eph[11]= clock offset (s) scale 2^-31
893 */
894 tnav_t_ptr->SubFrame1[5] >>= 2; /* shift off 2 lsbs of 6th word */
895 word = tnav_t_ptr->SubFrame1[5] & 0x3fffff; /* 22 bits for Af0 */
896 if( word & 0x200000 ) word -= 0x400000; /* 2's complement */
897 new_eph->a0 = (double)word / 2.147483648e9; /* apply scale factor */
898
899 tnav_t_ptr->SubFrame1[5] >>= 22; /* shift off Af0 bits */
900 tmp_word1 = tnav_t_ptr->SubFrame1[5] & 0xff;/* Af1's 8 LSB's */
901 word = tnav_t_ptr->SubFrame1[4] & 0xff; /* Af1's 8 MSB's */
902 word <<= 8; /* shift for proper alignment of bits */
903 word += tmp_word1;
904 if( word & 0x8000 ) word -= 0x010000; /* 2's complement */
905 new_eph->a1 = (double)word/8.796093022208e12; /* apply scale factor */
906
907
908 tnav_t_ptr->SubFrame1[4] >>= 8; /* shift off Af1's MSB's */
909 word = tnav_t_ptr->SubFrame1[4] & 0xff; /* 8 bits for Af2 */
910
911 if (word & 0x80) word -= 0x0100; /* 2's compliment */
912 new_eph->a2 = (double)word/3.6028797018963968e16; /* apply scale factor */
913
914 tnav_t_ptr->SubFrame1[4]>>= 8; /* shift off Af2 bits */
915
916
917 word = tnav_t_ptr->SubFrame1[4] & 0xffff; /* Toc bits */
918 clock_ref_time = (double)word*16; /* apply scale factor */
919 new_eph->clock_ref_time = clock_ref_time;
920
921 tmp_word1 = tnav_t_ptr->SubFrame1[3] & 0xff; /* LSB's for IODC */
922 tnav_t_ptr->SubFrame1[3] >>= 8; /* shift off IODC LSB's */
923
924 word = tnav_t_ptr->SubFrame1[3] & 0xff; /* next 8 are TGD */
925 if (word & 0x80) word -= 0x0100; /* 2's compliment */
926 new_eph->group_delay = (double)word/2.147483648e9; /* apply scale factor */
927
928 tnav_t_ptr->SubFrame1[0] >>= 7; /* shift off spare bits */
929 tmp_word2 = tnav_t_ptr->SubFrame1[0] & 0x01; /* L2PFlag bit */
930
931
932 new_eph->l2pflag = (double)tmp_word2;
933 tnav_t_ptr->SubFrame1[0] >>= 1; /* shift off L2PFlag bit */
934 word = tnav_t_ptr->SubFrame1[0] & 0x03; /* 2 MSB's for IODC */
935 word <<= 8; /* shift MSB's for proper alignment */
936 issue_of_data_clock = (double)(word + tmp_word1); /* combine 2 + 8 bits */
937 new_eph->issue_of_clock = issue_of_data_clock;
938
939 tnav_t_ptr->SubFrame1[0] >>= 2; /* shift off IODC MSB's */
940 word = tnav_t_ptr->SubFrame1[0] & 0x3f; /* 6 health bits */
941
942
943 /* set only the MSB of the 6 health bits */
944 new_eph->sat_health = (double) ( (word & 0x20) >> 5 );
945
946 tnav_t_ptr->SubFrame1[0] >>= 6; /* shift off the health bits */
947 word = tnav_t_ptr->SubFrame1[0] & 0x0f; /* next 4 are URA bits */
948 new_eph->user_range_acc = (double)svacrcy[ (int)word ];
949
950 tnav_t_ptr->SubFrame1[0] >>= 4; /* shift off URA bits */
951 word = tnav_t_ptr->SubFrame1[0] & 0x03; /* 2 bits for L2code */
952 new_eph->l2code = (double)word; /* L2code */
953
954 tmp_word2 = tmp_word1; /* LSB's of IODC for comparison with IODE */
955
956 tnav_t_ptr->SubFrame1[0] >>= 2; /* shift off L2code bits */
957 /* 10 bits for week number */
958 new_eph->gps_week = GPSWK_UNROLL((double)(tnav_t_ptr->SubFrame1[0] & 0x3ff));
959
960 new_eph->satellite = (double)(tnav_t_ptr->Satellite & 0xff);
961
962 /*
963 c process subframe 2
964 c
965 c new_eph[12) = issue of new_ephemeris data
966 c new_eph[13) = crs (meters), scale 2^-5
967 c new_eph[14) = offset rate (rad/s), scale 2^-43
968 c new_eph[15) = mean anomaly at ref. time (rad), scale 2^-31
969 c new_eph[16) = cuc (rad), scale 2^-29
970 c new_eph[17) = eccentricity, scale 2^-33
971 c new_eph[18) = cus (rad), scale 2^-29
972 c new_eph[19) = sqrt of sma (m^0.5), scale 2^-19
973 c new_eph[20) = new_eph. ref. time ~ start gps week (s), scale 2^4
974
975 c new_eph[21) = curve fit interval (in hrs)
976 */
977
978 tnav_t_ptr->SubFrame2[5] >>= 7; /* shift off 7 lsbs of 6th word */
979 word = tnav_t_ptr->SubFrame2[5] & 0x01; /*fit interval 1 bit */
980 new_eph->fit_interval = (double)word; /* see below for further processing */
981
982
983
984 tnav_t_ptr->SubFrame2[5] >>= 1; /* shift off fit bit */
985 word = tnav_t_ptr->SubFrame2[5] & 0xffff; /* toe 16 bits */
986 new_eph->eph_ref_time = (double)word * 16.0; /* scale toe */
987
988 /* if ( new_eph->eph_ref_time != clock_ref_time ) return(-1); */
989
990 tnav_t_ptr->SubFrame2[5] >>= 16; /* shift off toe bits */
991
992 tmp_word1 = tnav_t_ptr->SubFrame2[5] & 0xff; /* 8 LSB's for semi-axis */
993 word = tnav_t_ptr->SubFrame2[4] & 0xffffff; /* 24 MSB's for semi-axis*/
994 word <<= 8; /* shift left 8 for proper alignment */
995 word += tmp_word1; /* assemble the 32 bits */
996
997 new_eph->orbit_semimaj = (double)word/5.24288e5; /* scale the semimajor axis */
998 if (new_eph->orbit_semimaj < 0.0e0) new_eph->orbit_semimaj += 8192.0e0;
999
1000
1001 tnav_t_ptr->SubFrame2[4] >>= 24; /* shift off semi axis MSB's */
1002 tmp_word1 = tnav_t_ptr->SubFrame2[4] & 0xff; /* 8 LsB's for Cus */
1003 word = tnav_t_ptr->SubFrame2[3] & 0xff; /* 8 MSB's for Cus */
1004 word <<= 8; /* shift left 8 for proper alignment */
1005 word += tmp_word1; /* assemble the 16 bits */
1006 if( word & 0x8000 ) word -= 0x010000; /* apply 2's complement */
1007 new_eph->lat_sin_corr = (double)word/5.36870912e8;
1008
1009 tnav_t_ptr->SubFrame2[3] >>= 8; /* shift off Cus MSB's */
1010 tmp_word1 = tnav_t_ptr->SubFrame2[3] & 0xffffff; /* 24 LsB's for Ecc */
1011 word = tnav_t_ptr->SubFrame2[2] & 0xff; /* 8 MSB's for Ecc */
1012 word <<= 24; /* shift left 24 for proper alignment */
1013 word += tmp_word1; /* assemble the 32 bits */
1014 new_eph->orbit_ecc = (double)word/8.589934592e9;
1015 if(new_eph->orbit_ecc < 0.0) new_eph->orbit_ecc += 0.5;
1016
1017 tnav_t_ptr->SubFrame2[2] >>= 8; /* shift off Ecc MSB's */
1018 word = tnav_t_ptr->SubFrame2[2] & 0xffff; /* 16 Cuc bits */
1019 if( word & 0x8000 ) word -= 0x010000; /* apply 2's complement */
1020 new_eph->lat_cos_corr = (double)word/5.36870912e8;
1021
1022 tnav_t_ptr->SubFrame2[2] >>= 16; /* shift off Cuc bits*/
1023 tmp_word1 = tnav_t_ptr->SubFrame2[2] & 0xff; /* 8 LsB's for MO */
1024 word = tnav_t_ptr->SubFrame2[1] & 0xffffff; /* 24 MSB's for MO */
1025 word <<= 8; /* shift left 8 for proper alignment */
1026 word += tmp_word1; /* assemble the 32 bits */
1027 new_eph->ref_mean_anmly = (double)word*(PI/2.147483648e9);
1028
1029 tnav_t_ptr->SubFrame2[1] >>= 24; /* shift off MO MSB's */
1030 tmp_word1 = tnav_t_ptr->SubFrame2[1] & 0xff; /* 8 LsB's for dN */
1031 word = tnav_t_ptr->SubFrame2[0] & 0xff; /* 8 MSB's for dN */
1032
1033 word <<= 8; /* shift left 8 for proper alignment */
1034 word += tmp_word1; /* assemble the 16 bits */
1035
1036 if( word & 0x8000 ) word -= 0x010000; /* apply 2's complement */
1037 new_eph->mean_mot_diff = (double)word*(PI/8.796093022208e12);
1038
1039 tnav_t_ptr->SubFrame2[0] >>= 8; /* shift off dN MSB's */
1040 word = tnav_t_ptr->SubFrame2[0] & 0xffff; /* 16 bit for Crs */
1041 if( word & 0x8000 ) word -= 0x010000; /* apply 2's complement */
1042 new_eph->orbit_sin_corr = (double)word / 32.0;
1043
1044 tnav_t_ptr->SubFrame2[0] >>= 16; /* shift off Crs bits */
1045 issue_of_data_eph1 = tnav_t_ptr->SubFrame2[0] & 0xff;/*8 bits for IODE1*/
1046 /* compare IODC with IODE1 */
1047
1048 /* if( issue_of_data_eph1 != tmp_word2 ) return(-2); */
1049
1050 if (issue_of_data_eph1 < 240.0) {
1051 if (new_eph->fit_interval == 0.0) new_eph->fit_interval = 4.e0;
1052 if (new_eph->fit_interval == 1.0) new_eph->fit_interval = 6.e0;
1053 }
1054 else if (issue_of_data_clock < 248.e0)
1055 {
1056 new_eph->fit_interval = 8.e0;
1057 }
1058 else if (issue_of_data_clock < 497.e0)
1059 {
1060 new_eph->fit_interval = 14.e0;
1061 }
1062 else if (issue_of_data_clock < 504.e0)
1063 {
1064 new_eph->fit_interval = 26.e0;
1065 }
1066 else if (issue_of_data_clock < 511.e0)
1067 {
1068 new_eph->fit_interval = 50.e0;
1069 }
1070 else if (issue_of_data_clock < 757.e0)
1071 {
1072 new_eph->fit_interval = 74.e0;
1073 }
1074 else if (issue_of_data_clock < 764.e0)
1075 {
1076 new_eph->fit_interval = 98.e0;
1077 }
1078 else if (issue_of_data_clock < 1011.e0)
1079 {
1080 new_eph->fit_interval = 122.e0;
1081 }
1082 else if (issue_of_data_clock < 1021.e0)
1083 {
1084 new_eph->fit_interval = 146.e0;
1085 }
1086 /*
1087
1088 c process subframe 3
1089 c
1090 c new_eph[22) = cic (rad), scale 2^-29
1091 c new_eph[23) = right ascension at ref. time (rad), 2^-31
1092 c new_eph[24) = cis (rad), scale 2^-29
1093 c new_eph[25) = inclination (rad), scale 2^-31
1094 c new_eph[26) = crc (m), scale 2^-5
1095 c new_eph[27) = argument of perigee (rad), scale 2^-31
1096 c new_eph[28) = rate of right ascension (rad/s) scale 2^-43
1097 c new_eph[29) = issue of new_ephemeris data
1098 c new_eph[30) = inclination rate (rad/s) scale 2^-43
1099 */
1100 tnav_t_ptr->SubFrame3[5] >>= 2; /* shift off 2 lsbs of 6th word */
1101 word = tnav_t_ptr->SubFrame3[5] & 0x3fff; /*IDOT 14 bits */
1102
1103 if( word & 0x2000 ) word -= 0x4000; /* apply 2's complement */
1104 new_eph->incl_rate = (double)word*(PI/8.796093022208e12);
1105
1106 tnav_t_ptr->SubFrame3[5] >>= 14; /* shift off 14 IDOT bits */
1107 word = tnav_t_ptr->SubFrame3[5] & 0xff; /*IODE2 bits */
1108
1109 issue_of_data_eph2 = (double)word;
1110
1111 tnav_t_ptr->SubFrame3[5] >>= 8; /* shift off IODE2 bits */
1112 tmp_word1 = tnav_t_ptr->SubFrame3[5] & 0xff; /* 8 LsB's for DOmega */
1113 word = tnav_t_ptr->SubFrame3[4] & 0xffff; /* 16 MSB's for DOmega */
1114 word <<= 8; /* shift left 8 for proper alignment */
1115 word += tmp_word1; /* assemble the 24 bits */
1116
1117 if( word & 0x800000 ) word -= 0x01000000; /* apply 2's complement */
1118 new_eph->right_asc_rate = (double)word*(PI/8.796093022208e12);
1119
1120 tnav_t_ptr->SubFrame3[4] >>= 16; /* shift off DOmega MSB's */
1121 tmp_word1 = tnav_t_ptr->SubFrame3[4] & 0xffff; /* 16 LsB's for w */
1122 word = tnav_t_ptr->SubFrame3[3] & 0xffff; /* 16 MSB's for w */
1123 word <<= 16; /* shift left 16 for proper alignment */
1124
1125 word += tmp_word1; /* assemble the 32 bits */
1126 new_eph->arg_of_perigee = (double)word*(PI/2.147483648e9);
1127
1128 tnav_t_ptr->SubFrame3[3] >>= 16; /* shift off w MSB's */
1129 word = tnav_t_ptr->SubFrame3[3] & 0xffff; /* 16 Crc bits */
1130 if( word & 0x8000 ) word -= 0x010000; /* apply 2's complement */
1131
1132 new_eph->orbit_cos_corr = (double)word/32.0e0;
1133 /* IO 32 bits */
1134 new_eph->orbit_incl = (double)tnav_t_ptr->SubFrame3[2] * (PI/2.147483648e9);
1135
1136
1137
1138
1139 word = tnav_t_ptr->SubFrame3[1] & 0xffff; /* 16 Cis bits */
1140 if( word & 0x8000 ) word -= 0x010000; /* apply 2's complement */
1141 new_eph->incl_sin_corr = (double)word/5.36870912e8;
1142
1143 tnav_t_ptr->SubFrame3[1] >>= 16; /* shift off Cis bits */
1144 tmp_word1 = tnav_t_ptr->SubFrame3[1] & 0xffff; /* 16 LsB's for OmegaO */
1145 word = tnav_t_ptr->SubFrame3[0] & 0xffff; /* 16 MSB's for OmegaO*/
1146 word <<= 16; /* shift left 16 for proper alignment */
1147
1148 word += tmp_word1; /* assemble the 32 bits */
1149 new_eph->right_asc = (double)word*(PI/2.147483648e9);
1150
1151 tnav_t_ptr->SubFrame3[0] >>= 16; /* shift off OmegaO bits */
1152 word = tnav_t_ptr->SubFrame3[0] & 0xffff; /* 16 Cic bits */
1153 if( word & 0x8000 ) word -= 0x010000; /* apply 2's complement */
1154
1155
1156 new_eph->incl_cos_corr = (double)word/5.36870912e8;
1157 /*
1158 c issue of clock data (subframe 1) & the 2 versions
1159 c of the issue of new_ephemeris data (subframes 2 & 3)
1160 c are not consistent: return error code -2
1161 */
1162 /* if( issue_of_data_eph1 != issue_of_data_eph2 ) return( -3 ); */
1163 new_eph->issue_of_eph = issue_of_data_eph2;
1164
1165
1166 /*
1167 c correct GPS week value when validity interval crosses end of week
1168 c in this case the week decoded is the week of transmission and the
1169 c reference time could be in the subsequent week
1170 c the condition tested is:
1171 c sec_of_week(transmit) - sec_of week(eph_reference) > +302400
1172 */
1173 if( ((long)new_eph->transmit_time)%604800 - new_eph->eph_ref_time > 302400. )
1174 new_eph->gps_week += 1.0;
1175 } //KML June 9/2005
1176 else //KML June 9/2005
1177 { //KML June 9/2005
1178 retval = -1; //KML June 9/2005
1179 } //KML June 9/2005
1180return retval; //KML June 9/2005
1181}
1182// 2/1/2008 SPG End
1183
Note: See TracBrowser for help on using the repository browser.