#include "cgps_transform.h" #define MAXSTREAM 10000 //////////////////////////////////////////////////////////////////////////// void SwitchBytes( char *Start, int Size ) { char Tmp; char *End = Start + Size - 1; for( Tmp = *Start; Start < End; Tmp = *Start ){ *Start++ = *End; *End-- = Tmp; } } #ifdef CGPS_TRANSFORM_MAIN int main() { unsigned char data_stream[MAXSTREAM]; unsigned short numbytes; RTIGSS_T rtigs_sta; RTIGSO_T rtigs_obs; RTIGSM_T rtigs_met; RTIGSE_T rtigs_eph; short PRN; short retval; unsigned short statID; unsigned short messType; CGPS_Transform GPSTrans; memset(data_stream , 0, sizeof(data_stream)); // use something like recvfrom FILE* inpFile = fopen("RTIGS.txt", "rb"); while (true) { size_t nr = 0; if (inpFile) { nr = fread(data_stream, sizeof(unsigned char), MAXSTREAM, inpFile); if (nr == 0) exit(0); cout << "Number of bytes read: " << nr << endl; } else { exit(1); } // Find the beginning of the message // --------------------------------- size_t sz = sizeof(unsigned short); bool found = false; size_t ii; for (ii = 0; ii < nr - sz; ii += sz) { unsigned short xx; memcpy( (void*) &xx, &data_stream[ii], sz); SwitchBytes( (char*) &xx, sz); if (xx == 200) { found = true; break; } } if (! found) { cout << "Message not found\n"; exit(0); } else { cout << "Message found at " << ii << endl; } messType = GPSTrans.GetRTIGSHdrRecType(&data_stream[ii]); numbytes = GPSTrans.GetRTIGSHdrRecBytes(&data_stream[ii]); statID = GPSTrans.GetRTIGSHdrStaID(&data_stream[ii]); cout << "messType " << messType << endl; cout << "numbytes " << numbytes << endl; cout << "statID " << statID << endl; switch (messType) { case 100: GPSTrans.Decode_RTIGS_Sta(&data_stream[ii], numbytes , rtigs_sta); break; case 200: retval = GPSTrans.Decode_RTIGS_Obs(&data_stream[ii], numbytes , rtigs_obs); if (retval >= 1) { GPSTrans.print_CMEAS(); } break; case 300: retval = GPSTrans.Decode_RTIGS_Eph(&data_stream[ii], numbytes , rtigs_eph, PRN); break; case 400: retval = GPSTrans.Decode_RTIGS_Met(&data_stream[ii], numbytes , &rtigs_met); break; } } return 0; } #endif // Constructor //////////////////////////////////////////////////////////////////////////// CGPS_Transform::CGPS_Transform() { // Decoded Obs Array of 12 CMeas Observation Records memset((void *)&DecObs, 0, sizeof(ARR_OBS_T )); // Keplarian Broadcast Eph memset((void *)&TNAV_Eph,0, sizeof(ARR_TNAV_T )); NumObsRead = -1; CAFlag = -1; ASFlag = -1; P2Flag = -1; P1Flag = -1; InitEndianFlag(); memset (PhaseArcStartTime, 0, sizeof(PhaseArcStartTime)); } // Destructor //////////////////////////////////////////////////////////////////////////// CGPS_Transform::~CGPS_Transform() { } // //////////////////////////////////////////////////////////////////////////// unsigned short CGPS_Transform::GetRTIGSHdrRecType(unsigned char *RTIGS_Str) { unsigned short recordID; memcpy ((void *)&recordID,RTIGS_Str, sizeof(recordID)); if (f_IsLittleEndian) { SwitchBytes( (char *)&recordID, sizeof(recordID) ); } return recordID; } // //////////////////////////////////////////////////////////////////////////// unsigned short CGPS_Transform::GetRTIGSHdrRecBytes(unsigned char *RTIGS_Str) { unsigned short bytes; memcpy ((void *)&bytes,&RTIGS_Str[8], sizeof(bytes)); if (f_IsLittleEndian) { SwitchBytes( (char *)&bytes, sizeof(bytes) ); } return bytes; } // //////////////////////////////////////////////////////////////////////////// unsigned short CGPS_Transform::GetRTIGSHdrStaID(unsigned char *RTIGS_Str) { unsigned short StaID = 0; memcpy ((void *)&StaID, &RTIGS_Str[2], sizeof(StaID)); if (f_IsLittleEndian) { SwitchBytes( (char *)&StaID, sizeof(StaID) ); } return StaID; } // //////////////////////////////////////////////////////////////////////////// void CGPS_Transform::InitEndianFlag() { short one = 1; char *cp = (char *)&one; if (*cp== 0) { f_IsLittleEndian = false; } else { f_IsLittleEndian = true; } } // //////////////////////////////////////////////////////////////////////////// unsigned long CGPS_Transform::JPL_xtractLongVal (unsigned startBitNbr, unsigned xtractNbrBits, const char *msg) { unsigned long retValue=0, i=0; unsigned short posBit = xtractNbrBits - 1; for(i=0; i> numShift) & 0x0001) << posBit--); } return retValue; } // //////////////////////////////////////////////////////////////////////////// inline void CGPS_Transform::SwitchIGS_Sta_HdrBytes( RTIGSS_T *StaHdr) { SwitchBytes( (char *)&StaHdr->GPSTime, sizeof(unsigned long) ); SwitchBytes( (char *)&StaHdr->num_bytes, sizeof(unsigned short) ); SwitchBytes( (char *)&StaHdr->rec_id, sizeof(unsigned short) ); SwitchBytes( (char *)&StaHdr->sta_id, sizeof(unsigned short) ); } // //////////////////////////////////////////////////////////////////////////// inline void CGPS_Transform::SwitchIGS_Obs_HdrBytes( RTIGSO_T *ObsHdr) { SwitchBytes( (char *)&ObsHdr->GPSTime, sizeof(unsigned long) ); SwitchBytes( (char *)&ObsHdr->num_bytes, sizeof(unsigned short) ); SwitchBytes( (char *)&ObsHdr->rec_id, sizeof(unsigned short) ); SwitchBytes( (char *)&ObsHdr->sta_id, sizeof(unsigned short) ); } // //////////////////////////////////////////////////////////////////////////// inline void CGPS_Transform::SwitchIGS_Eph_HdrBytes( RTIGSE_T *EphHdr) { SwitchBytes( (char *)&EphHdr->CollectedGPSTime, sizeof(unsigned long) ); SwitchBytes( (char *)&EphHdr->num_bytes, sizeof(unsigned short) ); SwitchBytes( (char *)&EphHdr->rec_id, sizeof(unsigned short) ); SwitchBytes( (char *)&EphHdr->sta_id, sizeof(unsigned short) ); } // //////////////////////////////////////////////////////////////////////////// inline short CGPS_Transform::SwitchIGS_Met_RecBytes( RTIGSM_T *MetHdr) { short retval = 1; short i, num_items; num_items = (short)MetHdr->numobs; SwitchBytes( (char *)&MetHdr->GPSTime, sizeof(unsigned long) ); SwitchBytes( (char *)&MetHdr->num_bytes, sizeof(unsigned short) ); SwitchBytes( (char *)&MetHdr->rec_id, sizeof(unsigned short) ); SwitchBytes( (char *)&MetHdr->sta_id, sizeof(unsigned short) ); /*switch met data bytes*/ for (i=0; i < num_items; i++) { if (&MetHdr->mets[i] != NULL) { SwitchBytes( (char *)&MetHdr->mets[i], sizeof(long) ); } else { retval = -1; } } return retval; } // //////////////////////////////////////////////////////////////////////////// short CGPS_Transform::Save_TNAV_T_To_Container(TNAV_T *rtcurrent_eph, short &prn) { short retval = 1;//, i; long PRN; PRN = rtcurrent_eph->Satellite; if (f_IsLittleEndian) { SwitchBytes( (char *)&PRN, sizeof(PRN)); } if ((PRN > 0) && (PRN <= 32)) { memcpy( (void *)&TNAV_Eph.Eph[(PRN-1)] ,rtcurrent_eph,sizeof(TNAV_T)); prn = (short)PRN; } else { retval = -1; } return retval; } // //////////////////////////////////////////////////////////////////////////// short CGPS_Transform::CA_Extract(char * CAStr, double &CA_Rng) { unsigned long CARng2, CARng1; short retval = 0; double dtemp; CGPS_Transform::CAFlag = JPL_xtractLongVal(0, 1, CAStr); CGPS_Transform::ASFlag = JPL_xtractLongVal(1, 1, CAStr); CGPS_Transform::P2Flag = JPL_xtractLongVal(2, 1, CAStr); CGPS_Transform::P1Flag = JPL_xtractLongVal(3, 1, CAStr); if (CAFlag) { //Read most significant bits CARng2 = JPL_xtractLongVal(4, 4, CAStr); //Read an int's worth of data CARng1 = JPL_xtractLongVal (8,32,CAStr); // if (f_IsLittleEndian == false) // { //KML June 8/2004 //Added this code to deal with Big Endian architectures // SwitchBytes( (char *) &CARng2, sizeof(CARng2) ); // SwitchBytes( (char *) &CARng1, sizeof(CARng1) ); // } dtemp = 0.0; dtemp = CARng2; CA_Rng = dtemp*pow ((double)2,32); CA_Rng += CARng1; CA_Rng /= 1000; //CA in metres } else { retval = -1; } return retval; } // //////////////////////////////////////////////////////////////////////////// short CGPS_Transform::P1_P2_Block_Extract(char * P1P2Str, double CA, double &Rng , double &Phase, double &RngF2Delta,short decode_F1orF2Flag ) { short retval =0; short PhaseOverFlowFlag = -1; long SignFlag,temp; double RngDelta, PhaseDelta; if (decode_F1orF2Flag == 1) { PhaseOverFlowFlag = CGPS_Transform::P1Flag; } else if (decode_F1orF2Flag == 2) { PhaseOverFlowFlag = CGPS_Transform::P2Flag; } //***************************** // Decode Pseudo Range //***************************** SignFlag = JPL_xtractLongVal (0,1,P1P2Str); temp = JPL_xtractLongVal (1,17,P1P2Str); //KML June 8/2004 // if (f_IsLittleEndian == false) // { //Added this code to deal with Big Endian architectures // SwitchBytes( (char *) &temp, sizeof(temp) ); // } RngDelta = temp; RngDelta /= 1000.0; if (SignFlag) { RngDelta *= -1; } if (decode_F1orF2Flag == 2) { RngF2Delta = RngDelta; } Rng = CA + RngDelta; //*************************** // Decode Phase //*************************** SignFlag = JPL_xtractLongVal (18,1, P1P2Str); temp = JPL_xtractLongVal (19,21, P1P2Str); // if (f_IsLittleEndian == false) // { //KML June 8th 2004 //Added this code to deal with Big Endian architectures // SwitchBytes( (char *) &temp, sizeof(temp) ); // } PhaseDelta = temp; PhaseDelta = PhaseDelta * 2 / 100000; //Phase overflow add to phase if(PhaseOverFlowFlag) { PhaseDelta += MAXL1L2; } if (SignFlag) { PhaseDelta *= -1; } if (decode_F1orF2Flag == 1) { // frequency 1 Phase = (CA - (ScaleFactor2*RngF2Delta)+ PhaseDelta) / L1; } else if (decode_F1orF2Flag == 2) { // frequency 2 Phase = (CA - (ScaleFactor1*RngF2Delta)+ PhaseDelta) / L2; } else { retval =-1; } return retval; } // //////////////////////////////////////////////////////////////////////////// short CGPS_Transform::Decode_RTIGS_Sta(unsigned char *RTIGS_Str, unsigned short RTIGS_Bytes, RTIGSS_T &rtigs_sta) { short retval = 1; memcpy ((void *)&rtigs_sta.rec_id, &RTIGS_Str[0], (sizeof(RTIGSS_T) - sizeof(rtigs_sta.data))); if (f_IsLittleEndian) { SwitchIGS_Sta_HdrBytes( &rtigs_sta); } if (rtigs_sta.rec_id == 100) { if (rtigs_sta.sta_rec_type ==0 ) { rtigs_sta.data = NULL; } else { retval = -2; //no other type supported at this time } } else { retval = -1; } return retval ; } // //////////////////////////////////////////////////////////////////////////// short CGPS_Transform::Decode_RTIGS_Soc_Obs(unsigned char *SocStr, short &StrPos, short CMeasIndex, short SocBytes, unsigned long GPSTime) { short retval =1; double CA, RngF2Delta, PhaseL1,PhaseL2, P1, P2; //static unsigned short PhaseArcStartTime[MAXSV]; moved to class header JPL_COMP_OBS_T GPSObs; //printf("String pos %hd Total Bytes %hd\n", StrPos,SocBytes); if ((StrPos <= SocBytes) && ((CMeasIndex >= 0 ) && (CMeasIndex < MAXSV ))) { memcpy((void *)&GPSObs.prn, (void *)&SocStr[StrPos], 1); if ((GPSObs.prn > 0 ) && (GPSObs.prn <= 32)) { StrPos+=1; memcpy((void *)&GPSObs.epoch_sequence, (void *)&SocStr[StrPos],2); if (f_IsLittleEndian == false) { //KML June 8/2003 //Added this code to deal with Big Endian architectures SwitchBytes( (char *) &GPSObs.epoch_sequence, sizeof(GPSObs.epoch_sequence) ); } //****************************** // Read and decode CA //****************************** StrPos+=2; memcpy((void *)&GPSObs.ca_range, (void *)&SocStr[StrPos],5); CA_Extract(GPSObs.ca_range, CA); if (CGPS_Transform::CAFlag) //Defined in the Class by CA_Extract { //************************************ // Read CA SNR //************************************ StrPos+=5; memcpy((void *)&GPSObs.CA_snr, (void *)&SocStr[StrPos],1); //************************************ // Read and decode P2 L2 //************************************ StrPos+=1; memcpy((void *)&GPSObs.L2_range_phase, (void *)&SocStr[StrPos],5); P1_P2_Block_Extract(GPSObs.L2_range_phase, CA, P2 , PhaseL2, RngF2Delta, 2 ); StrPos+=5; memcpy((void *)&GPSObs.L2_snr, (void *)&SocStr[StrPos],1); //************************************ // Read and decode P1 L1 //************************************ StrPos+=1; memcpy((void *)&GPSObs.L1_range_phase, (void *)&SocStr[StrPos],5); P1_P2_Block_Extract(GPSObs.L1_range_phase, CA, P1, PhaseL1, RngF2Delta, 1); StrPos+=5; memcpy((void *)&GPSObs.L1_snr, (void *)&SocStr[StrPos],1); StrPos+=1; DecObs.Obs[CMeasIndex].GPSTime = GPSTime; /* broadcast time sec.*/ DecObs.Obs[CMeasIndex].chn = CMeasIndex + 1; /* Channel not real*/ DecObs.Obs[CMeasIndex].sat_prn = GPSObs.prn; /* satellite ID*/ DecObs.Obs[CMeasIndex].ntrvl = 1; /* number of seconds Changed from 0 to 1 Nov. 25/2003*/ DecObs.Obs[CMeasIndex].flag[0] = 4; /*observation quality flags*/ //KML Changed Nov. 25/2000 to 4 to indicate Benchmark if (PhaseArcStartTime[(short)(GPSObs.prn-1)] != GPSObs.epoch_sequence) { PhaseArcStartTime[(short)(GPSObs.prn-1)] = GPSObs.epoch_sequence; DecObs.Obs[CMeasIndex].flag[0] |= 0x20; } DecObs.Obs[CMeasIndex].l1_pseudo_range = CA; /* frequency-1 CA pseudorange */ DecObs.Obs[CMeasIndex].l1_phase = PhaseL1; /* frequency-1 CA carrier phase */ //**************************************************** // Changed SNR Ashtech to DBHz Nov 15/2002 //**************************************************** DecObs.Obs[CMeasIndex].l1_sn = GPSObs.CA_snr; DecObs.Obs[CMeasIndex].p1_pseudo_range = P1; /* frequency-1 P1 carrier phase */ DecObs.Obs[CMeasIndex].p1_phase = PhaseL1; /* frequency-1 P1 pseudorange */ DecObs.Obs[CMeasIndex].p1_sn = GPSObs.L1_snr; DecObs.Obs[CMeasIndex].l2_pseudo_range = P2; /* frequency-2 pseudorange (XCorr) */ DecObs.Obs[CMeasIndex].l2_phase = PhaseL2; /* frequency-2 carrier phase (XCorr) */ DecObs.Obs[CMeasIndex].l2_sn = GPSObs.L2_snr; DecObs.Obs[CMeasIndex].p2_pseudo_range = P2; /* frequency-2 pseudorange */ DecObs.Obs[CMeasIndex].p2_phase = PhaseL2; /* frequency-2 carrier phase */ } else { //skip this obs DecObs.Obs[CMeasIndex].sat_prn = 0; } } else { retval = -2; } } else { retval = -1; } return retval; } // //////////////////////////////////////////////////////////////////////////// short CGPS_Transform::RTIGSO_Str_To_CMEAS(unsigned char *RTIGSO_Str, short RTIGS_Bytes, RTIGSO_T &rtigs_obs) { short retval =1,i, StrPos;//, HdrRetval; //short NumObs= 0; short decoded_cnt = 0; short IGSObsMinusPtr; //************************************ // Zero out CMEAS_T container //************************************ memset((void *)&DecObs.Obs[0], 0 , sizeof(ARR_OBS_T) ); //*********************************************** // Decode Header store in class container //*********************************************** StrPos = IGSObsMinusPtr = sizeof(RTIGSO_T) - sizeof (rtigs_obs.data); //// cout << "StrPos " << StrPos << endl; memcpy ((void *)&rtigs_obs.rec_id, RTIGSO_Str, IGSObsMinusPtr); if (f_IsLittleEndian) { SwitchIGS_Obs_HdrBytes( &rtigs_obs); } // 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); if((rtigs_obs.rec_id == 200) && (rtigs_obs.num_obs <= MAXCHANNELS_FOR_SOCKETS_TYPE1)) { for (i = 0 ; i < rtigs_obs.num_obs;i++) { //********************************************* // the following function decodes the soc // structure and writes the obs to the // class's CMEAS container //********************************************* if (Decode_RTIGS_Soc_Obs( RTIGSO_Str, StrPos, decoded_cnt, RTIGS_Bytes, rtigs_obs.GPSTime) < 0) { retval = -2; } else { decoded_cnt ++; } }//end of for retval = NumObsRead = decoded_cnt; //NumObsRead class member } else { retval = -1; } //ObsSeqNum++; return retval; } // //////////////////////////////////////////////////////////////////////////// short CGPS_Transform::Decode_RTIGS_Obs(unsigned char *RTIGSO_Str, unsigned short RTIGS_Bytes,RTIGSO_T &rtigs_obs) { short retval = 1;//, i; if ((retval = RTIGSO_Str_To_CMEAS(RTIGSO_Str, RTIGS_Bytes, rtigs_obs)) < 0) { retval = -1; } return retval; } // //////////////////////////////////////////////////////////////////////////// short CGPS_Transform::Decode_RTIGS_Met(unsigned char *RTIGS_Str, unsigned short RTIGS_Bytes, RTIGSM_T *rtigs_met) { short retval = 1; short numbytes = 0; numbytes = sizeof(RTIGSM_T) - sizeof(rtigs_met->mets); memcpy ((void *)rtigs_met, RTIGS_Str, numbytes); if ((short)rtigs_met->numobs <= 3) { if (rtigs_met->mets != NULL) { memcpy ((void *)&rtigs_met->mets[0], &RTIGS_Str[numbytes], ((short)rtigs_met->numobs * sizeof(long)) ); if (f_IsLittleEndian) { SwitchIGS_Met_RecBytes( rtigs_met); } if (rtigs_met->rec_id != 400) { retval = -1; } } else { retval = -2; delete [] rtigs_met->mets; } } else { printf("failed number of Obs\n"); } return retval; } // //////////////////////////////////////////////////////////////////////////// short CGPS_Transform::Decode_RTIGS_Eph(unsigned char *RTIGS_Str, unsigned short RTIGS_Bytes, RTIGSE_T &rtigs_eph, short &PRN) { short retval = 1;//, i; short index = 0; short prn; const short SubFrameSize = 24; TNAV_T trans_eph; index = sizeof(RTIGSE_T ) - sizeof (rtigs_eph.data); memcpy ((void *)&rtigs_eph.rec_id, &RTIGS_Str[0], index); //copy header into struct from string if (f_IsLittleEndian) { SwitchIGS_Eph_HdrBytes( &rtigs_eph); } if (rtigs_eph.rec_id == 300) { //********************************************* // the following method saves the eph // in the class's TNAV_T container //********************************************* trans_eph.GPSCollectedTime = rtigs_eph.CollectedGPSTime; trans_eph. Satellite = (long)rtigs_eph.prn; //******************************************** // Container class is in network byte order //******************************************** if (f_IsLittleEndian) { SwitchBytes( (char *)&trans_eph.GPSCollectedTime, sizeof(trans_eph.GPSCollectedTime) ); SwitchBytes( (char *)&trans_eph. Satellite, sizeof(trans_eph. Satellite) ); } memcpy((void *)&trans_eph.SubFrame1, (const void *)&RTIGS_Str[index], SubFrameSize); memcpy((void *)&trans_eph.SubFrame2, (const void *)&RTIGS_Str[(index + SubFrameSize)], SubFrameSize); memcpy((void *)&trans_eph.SubFrame3, (const void *)&RTIGS_Str[(index + (SubFrameSize * 2)) ], SubFrameSize); if (Save_TNAV_T_To_Container(&trans_eph, prn) >= 1) //function saves eph in container and returns prn { PRN = prn; } else { retval = -1; } } else { retval = -1; } return retval; } void CGPS_Transform::print_CMEAS() { short i; printf("\nGPSTime SV CA SNR P1 SNR P2 SNR\n"); printf("Seconds (m) DBHz (m) DBHz (m) DBHz\n"); for (i=0; i < NumObsRead ; i++) { 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, DecObs.Obs[i].l1_pseudo_range,DecObs.Obs[i].l1_sn, DecObs.Obs[i].p1_pseudo_range, DecObs.Obs[i].p1_sn, DecObs.Obs[i].l2_pseudo_range,DecObs.Obs[i].l2_sn); } } // 2/1/2008 SPG Start //***************************************************************************************** // // Function/Method : CGPS_Transform::SwitchEphBytes() // // Purpose : // // Returns : // // Author : Created By KML 2002/06 // // Description: // // // // Parameters: // // // // // // // Revision : //***************************************************************************************** void CGPS_Transform::SwitchEphBytes( TNAV_T *rnav ) { unsigned long *word; int i, j; SwitchBytes( (char *)&rnav->GPSCollectedTime, sizeof(long) ); SwitchBytes( (char *)&rnav->Satellite, sizeof(long) ); word = (unsigned long *)rnav->SubFrame1; for( i = 0; i < 3; i++ ) { for( j = 1; j <= 6; j++, word++ ) { SwitchBytes( (char *)word, sizeof(long) ); } } } //***************************************************************************************** // // Function/Method : CGPS_Transform::TNAV_To_BEPH // // Purpose : // // Returns : // // Author : Created By Mark Caissy Modified and put in class by KML 2002/06 // // Description: // // // // Parameters: // // // // // // // Revision : KML June 9/2005 added check for PRN num, switch bytes and return //***************************************************************************************** short CGPS_Transform::TNAV_To_BEPH( TNAV_T *rtcurrent_eph, BEPH_T *new_eph) { TNAV_T temp_eph, *tnav_t_ptr; long word, tmp_word1, tmp_word2; double issue_of_data_clock, issue_of_data_eph1, issue_of_data_eph2, clock_ref_time; double svacrcy[] = { 2.0, 2.8, 4.0, 5.7, 8.0, 11.3, 1.6e01, 3.2e01, 6.4e01, 1.28e02, 2.56e02, 5.12e02, 1.024e03, 2.048e03, 4.096e03, -1.0 }; short retval = 1; //copy into local variable KML memcpy( &temp_eph, rtcurrent_eph, sizeof(TNAV_T) ); tnav_t_ptr = &temp_eph; if (f_IsLittleEndian) //KML Added June 9/2005 { //KML SwitchEphBytes( tnav_t_ptr ); //KML } //KML //**************************************** // Verify that prn of in expected range //**************************************** if (((short)tnav_t_ptr->Satellite > 0) && ((short)tnav_t_ptr->Satellite <= 32)) //KML June 9/2005 { new_eph->transmit_time = GPSEC_UNROLL((tnav_t_ptr->GPSCollectedTime - 18L)); /* 18L is used since each subframe takes 6 * seconds and there are 3 of them. */ /* c process subframe 1 c c new_eph[1] = satellite prn number c new_eph[2] = gps week of navigation mesage c new_eph[3] = l2 codes, bits 11-12, + l2pflag*256 c new_eph[4] = user range accuracy b13-16 (m) c new_eph[5] = navigation health, bit 1 c new_eph[6] = l1, l2 correction term (s), scale 2^-31 c new_eph->clock_ref_time = aodc (age of data clock) c new_eph[8] = clock reference time c new_eph[9] = clock acceleration (s^-1), scale 2^-55 c new_eph[10]= clock rate, (s/s), scale 2^-43 c new_eph[11]= clock offset (s) scale 2^-31 */ tnav_t_ptr->SubFrame1[5] >>= 2; /* shift off 2 lsbs of 6th word */ word = tnav_t_ptr->SubFrame1[5] & 0x3fffff; /* 22 bits for Af0 */ if( word & 0x200000 ) word -= 0x400000; /* 2's complement */ new_eph->a0 = (double)word / 2.147483648e9; /* apply scale factor */ tnav_t_ptr->SubFrame1[5] >>= 22; /* shift off Af0 bits */ tmp_word1 = tnav_t_ptr->SubFrame1[5] & 0xff;/* Af1's 8 LSB's */ word = tnav_t_ptr->SubFrame1[4] & 0xff; /* Af1's 8 MSB's */ word <<= 8; /* shift for proper alignment of bits */ word += tmp_word1; if( word & 0x8000 ) word -= 0x010000; /* 2's complement */ new_eph->a1 = (double)word/8.796093022208e12; /* apply scale factor */ tnav_t_ptr->SubFrame1[4] >>= 8; /* shift off Af1's MSB's */ word = tnav_t_ptr->SubFrame1[4] & 0xff; /* 8 bits for Af2 */ if (word & 0x80) word -= 0x0100; /* 2's compliment */ new_eph->a2 = (double)word/3.6028797018963968e16; /* apply scale factor */ tnav_t_ptr->SubFrame1[4]>>= 8; /* shift off Af2 bits */ word = tnav_t_ptr->SubFrame1[4] & 0xffff; /* Toc bits */ clock_ref_time = (double)word*16; /* apply scale factor */ new_eph->clock_ref_time = clock_ref_time; tmp_word1 = tnav_t_ptr->SubFrame1[3] & 0xff; /* LSB's for IODC */ tnav_t_ptr->SubFrame1[3] >>= 8; /* shift off IODC LSB's */ word = tnav_t_ptr->SubFrame1[3] & 0xff; /* next 8 are TGD */ if (word & 0x80) word -= 0x0100; /* 2's compliment */ new_eph->group_delay = (double)word/2.147483648e9; /* apply scale factor */ tnav_t_ptr->SubFrame1[0] >>= 7; /* shift off spare bits */ tmp_word2 = tnav_t_ptr->SubFrame1[0] & 0x01; /* L2PFlag bit */ new_eph->l2pflag = (double)tmp_word2; tnav_t_ptr->SubFrame1[0] >>= 1; /* shift off L2PFlag bit */ word = tnav_t_ptr->SubFrame1[0] & 0x03; /* 2 MSB's for IODC */ word <<= 8; /* shift MSB's for proper alignment */ issue_of_data_clock = (double)(word + tmp_word1); /* combine 2 + 8 bits */ new_eph->issue_of_clock = issue_of_data_clock; tnav_t_ptr->SubFrame1[0] >>= 2; /* shift off IODC MSB's */ word = tnav_t_ptr->SubFrame1[0] & 0x3f; /* 6 health bits */ /* set only the MSB of the 6 health bits */ new_eph->sat_health = (double) ( (word & 0x20) >> 5 ); tnav_t_ptr->SubFrame1[0] >>= 6; /* shift off the health bits */ word = tnav_t_ptr->SubFrame1[0] & 0x0f; /* next 4 are URA bits */ new_eph->user_range_acc = (double)svacrcy[ (int)word ]; tnav_t_ptr->SubFrame1[0] >>= 4; /* shift off URA bits */ word = tnav_t_ptr->SubFrame1[0] & 0x03; /* 2 bits for L2code */ new_eph->l2code = (double)word; /* L2code */ tmp_word2 = tmp_word1; /* LSB's of IODC for comparison with IODE */ tnav_t_ptr->SubFrame1[0] >>= 2; /* shift off L2code bits */ /* 10 bits for week number */ new_eph->gps_week = GPSWK_UNROLL((double)(tnav_t_ptr->SubFrame1[0] & 0x3ff)); new_eph->satellite = (double)(tnav_t_ptr->Satellite & 0xff); /* c process subframe 2 c c new_eph[12) = issue of new_ephemeris data c new_eph[13) = crs (meters), scale 2^-5 c new_eph[14) = offset rate (rad/s), scale 2^-43 c new_eph[15) = mean anomaly at ref. time (rad), scale 2^-31 c new_eph[16) = cuc (rad), scale 2^-29 c new_eph[17) = eccentricity, scale 2^-33 c new_eph[18) = cus (rad), scale 2^-29 c new_eph[19) = sqrt of sma (m^0.5), scale 2^-19 c new_eph[20) = new_eph. ref. time ~ start gps week (s), scale 2^4 c new_eph[21) = curve fit interval (in hrs) */ tnav_t_ptr->SubFrame2[5] >>= 7; /* shift off 7 lsbs of 6th word */ word = tnav_t_ptr->SubFrame2[5] & 0x01; /*fit interval 1 bit */ new_eph->fit_interval = (double)word; /* see below for further processing */ tnav_t_ptr->SubFrame2[5] >>= 1; /* shift off fit bit */ word = tnav_t_ptr->SubFrame2[5] & 0xffff; /* toe 16 bits */ new_eph->eph_ref_time = (double)word * 16.0; /* scale toe */ /* if ( new_eph->eph_ref_time != clock_ref_time ) return(-1); */ tnav_t_ptr->SubFrame2[5] >>= 16; /* shift off toe bits */ tmp_word1 = tnav_t_ptr->SubFrame2[5] & 0xff; /* 8 LSB's for semi-axis */ word = tnav_t_ptr->SubFrame2[4] & 0xffffff; /* 24 MSB's for semi-axis*/ word <<= 8; /* shift left 8 for proper alignment */ word += tmp_word1; /* assemble the 32 bits */ new_eph->orbit_semimaj = (double)word/5.24288e5; /* scale the semimajor axis */ if (new_eph->orbit_semimaj < 0.0e0) new_eph->orbit_semimaj += 8192.0e0; tnav_t_ptr->SubFrame2[4] >>= 24; /* shift off semi axis MSB's */ tmp_word1 = tnav_t_ptr->SubFrame2[4] & 0xff; /* 8 LsB's for Cus */ word = tnav_t_ptr->SubFrame2[3] & 0xff; /* 8 MSB's for Cus */ word <<= 8; /* shift left 8 for proper alignment */ word += tmp_word1; /* assemble the 16 bits */ if( word & 0x8000 ) word -= 0x010000; /* apply 2's complement */ new_eph->lat_sin_corr = (double)word/5.36870912e8; tnav_t_ptr->SubFrame2[3] >>= 8; /* shift off Cus MSB's */ tmp_word1 = tnav_t_ptr->SubFrame2[3] & 0xffffff; /* 24 LsB's for Ecc */ word = tnav_t_ptr->SubFrame2[2] & 0xff; /* 8 MSB's for Ecc */ word <<= 24; /* shift left 24 for proper alignment */ word += tmp_word1; /* assemble the 32 bits */ new_eph->orbit_ecc = (double)word/8.589934592e9; if(new_eph->orbit_ecc < 0.0) new_eph->orbit_ecc += 0.5; tnav_t_ptr->SubFrame2[2] >>= 8; /* shift off Ecc MSB's */ word = tnav_t_ptr->SubFrame2[2] & 0xffff; /* 16 Cuc bits */ if( word & 0x8000 ) word -= 0x010000; /* apply 2's complement */ new_eph->lat_cos_corr = (double)word/5.36870912e8; tnav_t_ptr->SubFrame2[2] >>= 16; /* shift off Cuc bits*/ tmp_word1 = tnav_t_ptr->SubFrame2[2] & 0xff; /* 8 LsB's for MO */ word = tnav_t_ptr->SubFrame2[1] & 0xffffff; /* 24 MSB's for MO */ word <<= 8; /* shift left 8 for proper alignment */ word += tmp_word1; /* assemble the 32 bits */ new_eph->ref_mean_anmly = (double)word*(PI/2.147483648e9); tnav_t_ptr->SubFrame2[1] >>= 24; /* shift off MO MSB's */ tmp_word1 = tnav_t_ptr->SubFrame2[1] & 0xff; /* 8 LsB's for dN */ word = tnav_t_ptr->SubFrame2[0] & 0xff; /* 8 MSB's for dN */ word <<= 8; /* shift left 8 for proper alignment */ word += tmp_word1; /* assemble the 16 bits */ if( word & 0x8000 ) word -= 0x010000; /* apply 2's complement */ new_eph->mean_mot_diff = (double)word*(PI/8.796093022208e12); tnav_t_ptr->SubFrame2[0] >>= 8; /* shift off dN MSB's */ word = tnav_t_ptr->SubFrame2[0] & 0xffff; /* 16 bit for Crs */ if( word & 0x8000 ) word -= 0x010000; /* apply 2's complement */ new_eph->orbit_sin_corr = (double)word / 32.0; tnav_t_ptr->SubFrame2[0] >>= 16; /* shift off Crs bits */ issue_of_data_eph1 = tnav_t_ptr->SubFrame2[0] & 0xff;/*8 bits for IODE1*/ /* compare IODC with IODE1 */ /* if( issue_of_data_eph1 != tmp_word2 ) return(-2); */ if (issue_of_data_eph1 < 240.0) { if (new_eph->fit_interval == 0.0) new_eph->fit_interval = 4.e0; if (new_eph->fit_interval == 1.0) new_eph->fit_interval = 6.e0; } else if (issue_of_data_clock < 248.e0) { new_eph->fit_interval = 8.e0; } else if (issue_of_data_clock < 497.e0) { new_eph->fit_interval = 14.e0; } else if (issue_of_data_clock < 504.e0) { new_eph->fit_interval = 26.e0; } else if (issue_of_data_clock < 511.e0) { new_eph->fit_interval = 50.e0; } else if (issue_of_data_clock < 757.e0) { new_eph->fit_interval = 74.e0; } else if (issue_of_data_clock < 764.e0) { new_eph->fit_interval = 98.e0; } else if (issue_of_data_clock < 1011.e0) { new_eph->fit_interval = 122.e0; } else if (issue_of_data_clock < 1021.e0) { new_eph->fit_interval = 146.e0; } /* c process subframe 3 c c new_eph[22) = cic (rad), scale 2^-29 c new_eph[23) = right ascension at ref. time (rad), 2^-31 c new_eph[24) = cis (rad), scale 2^-29 c new_eph[25) = inclination (rad), scale 2^-31 c new_eph[26) = crc (m), scale 2^-5 c new_eph[27) = argument of perigee (rad), scale 2^-31 c new_eph[28) = rate of right ascension (rad/s) scale 2^-43 c new_eph[29) = issue of new_ephemeris data c new_eph[30) = inclination rate (rad/s) scale 2^-43 */ tnav_t_ptr->SubFrame3[5] >>= 2; /* shift off 2 lsbs of 6th word */ word = tnav_t_ptr->SubFrame3[5] & 0x3fff; /*IDOT 14 bits */ if( word & 0x2000 ) word -= 0x4000; /* apply 2's complement */ new_eph->incl_rate = (double)word*(PI/8.796093022208e12); tnav_t_ptr->SubFrame3[5] >>= 14; /* shift off 14 IDOT bits */ word = tnav_t_ptr->SubFrame3[5] & 0xff; /*IODE2 bits */ issue_of_data_eph2 = (double)word; tnav_t_ptr->SubFrame3[5] >>= 8; /* shift off IODE2 bits */ tmp_word1 = tnav_t_ptr->SubFrame3[5] & 0xff; /* 8 LsB's for DOmega */ word = tnav_t_ptr->SubFrame3[4] & 0xffff; /* 16 MSB's for DOmega */ word <<= 8; /* shift left 8 for proper alignment */ word += tmp_word1; /* assemble the 24 bits */ if( word & 0x800000 ) word -= 0x01000000; /* apply 2's complement */ new_eph->right_asc_rate = (double)word*(PI/8.796093022208e12); tnav_t_ptr->SubFrame3[4] >>= 16; /* shift off DOmega MSB's */ tmp_word1 = tnav_t_ptr->SubFrame3[4] & 0xffff; /* 16 LsB's for w */ word = tnav_t_ptr->SubFrame3[3] & 0xffff; /* 16 MSB's for w */ word <<= 16; /* shift left 16 for proper alignment */ word += tmp_word1; /* assemble the 32 bits */ new_eph->arg_of_perigee = (double)word*(PI/2.147483648e9); tnav_t_ptr->SubFrame3[3] >>= 16; /* shift off w MSB's */ word = tnav_t_ptr->SubFrame3[3] & 0xffff; /* 16 Crc bits */ if( word & 0x8000 ) word -= 0x010000; /* apply 2's complement */ new_eph->orbit_cos_corr = (double)word/32.0e0; /* IO 32 bits */ new_eph->orbit_incl = (double)tnav_t_ptr->SubFrame3[2] * (PI/2.147483648e9); word = tnav_t_ptr->SubFrame3[1] & 0xffff; /* 16 Cis bits */ if( word & 0x8000 ) word -= 0x010000; /* apply 2's complement */ new_eph->incl_sin_corr = (double)word/5.36870912e8; tnav_t_ptr->SubFrame3[1] >>= 16; /* shift off Cis bits */ tmp_word1 = tnav_t_ptr->SubFrame3[1] & 0xffff; /* 16 LsB's for OmegaO */ word = tnav_t_ptr->SubFrame3[0] & 0xffff; /* 16 MSB's for OmegaO*/ word <<= 16; /* shift left 16 for proper alignment */ word += tmp_word1; /* assemble the 32 bits */ new_eph->right_asc = (double)word*(PI/2.147483648e9); tnav_t_ptr->SubFrame3[0] >>= 16; /* shift off OmegaO bits */ word = tnav_t_ptr->SubFrame3[0] & 0xffff; /* 16 Cic bits */ if( word & 0x8000 ) word -= 0x010000; /* apply 2's complement */ new_eph->incl_cos_corr = (double)word/5.36870912e8; /* c issue of clock data (subframe 1) & the 2 versions c of the issue of new_ephemeris data (subframes 2 & 3) c are not consistent: return error code -2 */ /* if( issue_of_data_eph1 != issue_of_data_eph2 ) return( -3 ); */ new_eph->issue_of_eph = issue_of_data_eph2; /* c correct GPS week value when validity interval crosses end of week c in this case the week decoded is the week of transmission and the c reference time could be in the subsequent week c the condition tested is: c sec_of_week(transmit) - sec_of week(eph_reference) > +302400 */ if( ((long)new_eph->transmit_time)%604800 - new_eph->eph_ref_time > 302400. ) new_eph->gps_week += 1.0; } //KML June 9/2005 else //KML June 9/2005 { //KML June 9/2005 retval = -1; //KML June 9/2005 } //KML June 9/2005 return retval; //KML June 9/2005 } // 2/1/2008 SPG End