/***************************************************************************** * * CANADIAN MARCONI COMPANY PROPRIETARY INFORMATION * * $Workfile: GPS2TXT.C $, $Revision: 6 $ * * $Author: Fbreton $, $Date: 9/29/97 11:37a $ * * $Modtime: 9/29/97 11:32a $ * * $Log: /GPS/Allstar/Code/Utilities/gps2txtc/GPS2TXT.C $ * * 6 9/29/97 11:37a Fbreton * added calculation of orbital parameters C0 and N * * 4 6/05/97 3:17p Fbreton * change the version number * * 3 6/05/97 2:53p Fbreton * Add a field comment in channel assign * * 2 6/05/97 2:39p Fbreton * The Sv number received is (0..31) The display has to be (1..32) * * 1 5/28/97 1:34p Fbreton * initial entry in VSS * ****************************************************************************/ #include #include #include #include #include #include #include #include #define DEBUG 0 #define FALSE 0 #define TRUE 1 #define PI 3.14159265358979323846264338327950288419716939937510 #define SQRT_EGP (19964981.84321739) #define WEEK_IN_SEC 604800 #define HALF_WEEK_IN_SEC (WEEK_IN_SEC >> 2) #define INV_SQUARE_INTERVAL (INV_INTERVAL * INV_INTERVAL) #define CURVE_FIT_INTERVAL_OF_EPHEM ((3*60+15)*60) /* Ephemeris are valid only when use around toe by this constant.*/ #define SPEED_OF_LIGHT (299792458.0) typedef struct { unsigned char SOH; unsigned char Id; unsigned char NId; unsigned char Seq; } HEADER_TYPE; typedef unsigned short CHECKSUM_TYPE; typedef struct { unsigned char SV; unsigned char Bytes[72]; } RAW_EPHEM_TYPE; typedef struct { unsigned long Word3; unsigned long Word4; unsigned long Word5; unsigned long Word6; unsigned long Word7; unsigned long Word8; unsigned long Word9; unsigned long Word10; } SUBFRAME_ARRAY_TYPE; typedef struct { double X; double Y; double Z; } SV_XYZ_POS_TYPE; typedef struct { unsigned char Hours; unsigned char Minutes; double Seconds; unsigned char Day; unsigned char Month; unsigned short Year; double Latitude; double Longitude; float Altitude; float Speed; float Angle; float N_Vel; float E_Vel; float V_Vel; float HFOM; float VFOM; unsigned short HDOP; unsigned short VDOP; unsigned char Nav_Mode; unsigned char Nb_SV; unsigned char Sys_Mode; unsigned char Reserved1; unsigned char Reserved2; } NAV_USER_TYPE; typedef struct { double GPS_Time_Sec; unsigned short Week; double X_Pos; double Y_Pos; double Z_Pos; float X_Vel; float Y_Vel; float Z_Vel; double Clock_Bias; double Clock_Drift; float HFOM; float VFOM; unsigned short HDOP; unsigned short VDOP; unsigned char Nav_Mode; unsigned char Nb_SV; unsigned char Resv1; unsigned char Resv2; unsigned char Resv3; CHECKSUM_TYPE Checksum; } NAV_GPS_TYPE; typedef struct { unsigned char SV; unsigned long Carrier_DCO; float C_No; float Reserved; unsigned char Status; } CHAN_ASSIGN_TYPE; typedef struct { unsigned char Set; CHAN_ASSIGN_TYPE Chan[6]; CHECKSUM_TYPE Checksum; } CHANS_ASSIGN_TYPE; typedef struct { unsigned char SV; unsigned char SNR; unsigned long Code_Phase; unsigned long ICP; unsigned char Locktime; } FAST_MEAS_BLOCK_TYPE; typedef struct { signed char Reserved1; signed char Reserved2; unsigned char Nb_Blocks; double GPS_Time; FAST_MEAS_BLOCK_TYPE Block[12]; CHECKSUM_TYPE Checksum; } FAST_MEAS_BLOCKS_TYPE; typedef struct { unsigned char String[80]; CHECKSUM_TYPE Checksum; } ERROR_LOG_TYPE; void Quit(void); unsigned char Flag_C, Flag_D, Flag_E, Flag_F, Flag_G, Flag_R, Flag_T, Flag_U; unsigned char Flag_V, Flag_P; unsigned char Version; unsigned char Chan_Assign[12]; unsigned char Raw_Chan_1,Raw_Chan_2; int in; FILE *outmeas[32],*outnav,*outraw,*outunav,*outerror,*outassign,*outdgps; FILE *outephem,*Foutmeas[32]; unsigned char Error_Open = 0; unsigned char Assign_Open = 0; unsigned char Nav_GPS_Open = 0; unsigned char Nav_User_Open = 0; unsigned char Raw_Open = 0; unsigned char DGPS_Open = 0; unsigned char Ephem_Open = 0; unsigned char Fast_Meas_Open[32]; unsigned long Index = 0; unsigned long DGPSWords[64]; double GPS_Time =0.0; /*-------------------------------------------------------------------------- Data use to compute satellite position and velocity ---------------------------------------------------------------------------*/ /* -- Factors of the series expansion of the true anomaly V. */ typedef enum { NO_DATA=0, ALMANAC=1, EPHEMERID=2 } SV_DATA_SOURCE_TYPE; typedef struct { double Root_A; double N; double C0; double Sin_W; double Cos_W; double I_Dot; double Omega_Dot; double Crc; double I_0; double Cis; double Omega_0; double Cic; double Toe; double Cus; double E; double Cuc; double M0; double Crs; unsigned char Curve_Interval; SV_DATA_SOURCE_TYPE SV_Data_Source; unsigned long IODE; unsigned char Valid_Ephem_Decoded; } ORBIT_PARAMETERS_TYPE; typedef struct { double Af0; double Af1; double Af2; double Toc; double Tgd; } CLOCK_PARAMETERS_TYPE; CLOCK_PARAMETERS_TYPE Clock_Parameters[32]; ORBIT_PARAMETERS_TYPE Orbital_Parameters[32]; unsigned char Ephem_Avaliable[32]; /*-------------------------------------------------------------------------------- */ /*-- EPHEMERIS DATA */ /*-------------------------------------------------------------------------------- */ /*----------------------------------------------------------------------------*/ /* CONSTANT DEFINITION needed to scale ephemeris value as per ICD_GPS_200 */ /*----------------------------------------------------------------------------*/ /*----------------------------------------------------------------------------*/ /*-- WORD 10 SUBFRAME 1 */ /*----------------------------------------------------------------------------*/ #define SCALE_UP_AF0 (unsigned long)(1ul<<10) #define AF0_DELTA (double)(1/((double)(1ul<<31)* \ (SCALE_UP_AF0))) /* 2.0**(-31) */ /* -- Scaling factor */ /*----------------------------------------------------------------------------*/ /*-- WORD 9 SUBFRAME 1 */ /*----------------------------------------------------------------------------*/ #define AF2_DELTA (double)(1/((double)(1ul<<30)*(double)(1ul<<25))) /* 2.0 ** (-55) */ /* -- Scaling factor */ #define AF1_DELTA (double)(1/((double)(1ul<<20)*(double)(1ul<<23))) /* 2.0 ** (-43) */ /* -- Scaling factor */ /*----------------------------------------------------------------------------*/ /*-- WORD 8 SUBFRAME 1 */ /*----------------------------------------------------------------------------*/ #define TOC_DELTA (double)(16.0) /* -- Scaling factor */ #define IODC_8_LSB_DELTA (double)(1.0) /* -- Scaling factor */ /*----------------------------------------------------------------------------*/ /*-- WORD 7 SUBFRAME 1 */ /*----------------------------------------------------------------------------*/ #define TGD_DELTA (double)(1/((double)(1ul<<31))) /* 2.0 ** ( - 31) */ /* -- Scaling factor */ /*----------------------------------------------------------------------------*/ /*-- WORD 3 SUBFRAME 1 */ /*----------------------------------------------------------------------------*/ #define IODC_2_MSB_DELTA (double)(256.0) /* 2.0 ** ( + 8) */ /* -- Scaling factor */ #define SV_ACCURACY_DELTA (double)(1.0) /* -- Scaling factor */ #define WN_DELTA (double)(1.0) /* -- Scaling factor */ /*----------------------------------------------------------------------------*/ /*-- WORD 10 SUBFRAME 2 */ /*----------------------------------------------------------------------------*/ #define TOE_DELTA (double)(16.0) /* -- Scaling factor */ /*----------------------------------------------------------------------------*/ /*-- WORD 9 SUBFRAME 2 */ /*----------------------------------------------------------------------------*/ #define ROOT_A_24_LSB_DELTA (double)(1/((double)(1ul<<19))) /* 2.0 ** ( - 19) */ /* -- Scaling factor */ /*----------------------------------------------------------------------------*/ /*-- WORD 8 SUBFRAME 2 */ /*----------------------------------------------------------------------------*/ #define ROOT_A_8_MSB_DELTA (double)(32.0) /* 2.0 ** ( + 5) */ /* -- Scaling factor */ #define CUS_DELTA (double)(1/((double)(1ul<<29))) /* 2.0 ** ( - 29) */ /* -- Scaling factor */ /*----------------------------------------------------------------------------*/ /*-- WORD 7 SUBFRAME 2 */ /*----------------------------------------------------------------------------*/ #define E_24_LSB_DELTA (double)(1/((double)(1ul<<30)*(double)(1ul<<3))) /* 2.0 ** ( - 33) */ /* -- Scaling factor */ /*----------------------------------------------------------------------------*/ /*-- WORD 6 SUBFRAME 2 */ /*----------------------------------------------------------------------------*/ #define CUC_DELTA (double)(1/((double)(1ul<<29))) /* 2.0 ** ( - 29) */ /* -- Scaling factor */ /*----------------------------------------------------------------------------*/ /*-- WORD 5 SUBFRAME 2 */ /*----------------------------------------------------------------------------*/ #define M0_24_LSB_DELTA (double)(PI/((double)(1ul<<31))) /* 2.0 ** ( - 31) */ /* -- Scaling factor */ /*----------------------------------------------------------------------------*/ /*-- WORD 4 SUBFRAME 2 */ /*----------------------------------------------------------------------------*/ #define DEL_N_DELTA (double)(PI/((double)(1ul<<20)*(double)(1ul<<23))) /* 2.0 ** ( - 43) */ /* -- Scaling factor */ /*----------------------------------------------------------------------------*/ /*-- WORD 3 SUBFRAME 2 */ /*----------------------------------------------------------------------------*/ #define CRS_DELTA (double)(1/((double)(1ul<<5))) /* 2.0 ** ( - 5) */ /* -- Scaling factor */ /*----------------------------------------------------------------------------*/ /*-- WORD 10 SUBFRAME 3 */ /*----------------------------------------------------------------------------*/ #define I_DOT_DELTA (double)(PI/((double)(1ul<<23)*(double)(1ul<<20))) /* 2.0 ** ( - 43) */ /* -- Scaling factor */ /*----------------------------------------------------------------------------*/ /*-- WORD 9 SUBFRAME 3 */ /*----------------------------------------------------------------------------*/ #define SCALE_UP_OMEGA_DOT (unsigned long)(1ul<<8) #define OMEGA_DOT_DELTA (double)(PI/((double)(1ul<<23)*(double)(1ul<<20)\ *(double)(SCALE_UP_OMEGA_DOT))) /* 2.0 ** ( - 43) */ /* -- Scaling factor */ /*----------------------------------------------------------------------------*/ /*-- WORD 8 SUBFRAME 3 */ /*----------------------------------------------------------------------------*/ #define W_24_LSB_DELTA (double)(PI/((double)(1ul<<31))) /* 2.0 ** ( - 31) */ /* -- Scaling factor */ /*----------------------------------------------------------------------------*/ /*-- WORD 7 SUBFRAME 3 */ /*----------------------------------------------------------------------------*/ #define CRC_DELTA (double)(1/((double)(1ul<<5))) /* 2.0 ** ( - 5) */ /* -- Scaling factor */ #define W_8_MSB_DELTA (double)(1/((double)(1ul<<7))) /* 2.0 ** ( - 7) */ /* -- Scaling factor */ /*----------------------------------------------------------------------------*/ /*-- WORD 6 SUBFRAME 3 */ /*----------------------------------------------------------------------------*/ #define I0_24_LSB_DELTA (double)(PI/((double)(1ul<<31))) /* 2.0 ** ( - 31) */ /* -- Scaling factor */ /*----------------------------------------------------------------------------*/ /*-- WORD 5 SUBFRAME 3 */ /*----------------------------------------------------------------------------*/ #define CIS_DELTA (double)(1/((double)(1ul<<29))) /* 2.0 ** ( - 29) */ /* -- Scaling factor */ #define I0_8_MSB_DELTA (double)(1/((double)(1ul<<7))) /* 2.0 ** ( - 7) */ /* -- Scaling factor */ /*----------------------------------------------------------------------------*/ /*-- WORD 4 SUBFRAME 3 */ /*----------------------------------------------------------------------------*/ #define OMEGA_0_24_LSB_DELTA (double)(PI/((double)(1ul<<31))) /* 2.0 ** ( - 31) */ /* -- Scaling factor */ /*----------------------------------------------------------------------------*/ /*-- WORD 3 SUBFRAME 3 */ /*----------------------------------------------------------------------------*/ #define CIC_DELTA (double)(1/((double)(1ul<<29))) /* 2.0 ** ( - 29) */ /* -- Scaling factor */ #define OMEGA_0_8_MSB_DELTA (double)(1/((double)(1ul<<7))) /* 2.0 ** ( - 7) */ /* -- Scaling factor */ /*----------------------------------------------------------------------------*/ void Calc_Orbital_Parameter( SUBFRAME_ARRAY_TYPE *Sub2, SUBFRAME_ARRAY_TYPE *Sub3, ORBIT_PARAMETERS_TYPE *Orbital_Parameters ){ /* ABSTRACT AND PURPOSE: */ /* Computes various orbital parameter values needed to compute a position */ /* interpolation point for a given SV. */ /* */ unsigned long U_E; union { unsigned long U_M0; signed long S_M0; }M0; union { unsigned long U_W0; signed long S_W0; }W0; union { unsigned long U_I0; signed long S_I0; }I0; union { unsigned long U_W; signed long S_W; }W1; union { unsigned long U; signed long S; }I_Dot; union { unsigned long U; signed long S; }Omega_Dot; double A,Root_A; double Del_N, N0; double W; double E_Square; // SUBFRAME_ARRAY_TYPE *Sub2 = &(Ephemeris_Data->Sub2); // SUBFRAME_ARRAY_TYPE *Sub3 = &(Ephemeris_Data->Sub3); Orbital_Parameters->Toe = (float)(Sub2->Word10 >> 14) * TOE_DELTA; // MD TBD if check is needed U_E = (unsigned long)(Sub2->Word7 >> 6) + ((unsigned long)((Sub2->Word6 & 0x3FC0) << 18) ); Orbital_Parameters->E = (double)(U_E) * E_24_LSB_DELTA; Orbital_Parameters->Crs = (double)((signed long) ((Sub2->Word3 << 10 ) & (signed long)0xFFFF0000) *(CRS_DELTA/(double)(1ul<<16))); M0.U_M0 = (unsigned long)(Sub2->Word5 >> 6) + ((unsigned long)((Sub2->Word4 & 0x3FC0) << 18)); Orbital_Parameters->M0 = (double)(M0.S_M0)*M0_24_LSB_DELTA; Orbital_Parameters->Cuc = (double)((signed long) ((Sub2->Word6 << 2 ) & (signed long)0xFFFF0000)) * (CUC_DELTA/(double)(1ul<<16)); Orbital_Parameters->Cus = (double)((signed long) ((Sub2->Word8 << 2) & (signed long)0xFFFF0000) * (CUS_DELTA/(double)(1ul<<16))); Orbital_Parameters->Cic = (double)((signed long) ((Sub3->Word3 << 2) & ((signed long)0xFFFF0000)) * (CIC_DELTA/(double)(1ul<<16))); W0.U_W0 = ((unsigned long)(Sub3->Word4 >> 6) + ((unsigned long)((Sub3->Word3 & 0x3FC0) << 18)) ); Orbital_Parameters->Omega_0 = (double)(W0.S_W0)*OMEGA_0_24_LSB_DELTA; Orbital_Parameters->Cis = (double)((signed long) ((Sub3->Word5 << 2) & (signed long)0xFFFF0000) *(CIS_DELTA/(double)(1ul<<16))); I0.U_I0 = ((unsigned long) (Sub3->Word6 >> 6) + ((unsigned long)((Sub3->Word5 & 0x3FC0) << 18))); Orbital_Parameters->I_0 = (double)(I0.S_I0)*I0_24_LSB_DELTA; Orbital_Parameters->Crc = (double)((signed long) ((Sub3->Word7 << 2) & (signed long)0xFFFF0000) *(CRC_DELTA/(double)(1ul<<16))); Omega_Dot.U = (unsigned long)((Sub3->Word9 >> 6) *SCALE_UP_OMEGA_DOT); Orbital_Parameters->Omega_Dot = (double)(Omega_Dot.S)*OMEGA_DOT_DELTA; I_Dot.U = (unsigned long)(((Sub3->Word10 << 10 ) & 0xFFFC0000)); Orbital_Parameters->I_Dot = (double)(I_Dot.S) * (I_DOT_DELTA/(double)(1ul<<18)); Root_A = (double)( (unsigned long)(Sub2->Word9 >> 6) + ((unsigned long)((Sub2->Word8 & 0x3FC0) << 18)) ) *ROOT_A_24_LSB_DELTA; Orbital_Parameters->Root_A = Root_A; Del_N = (double)( (signed long)((Sub2->Word4 <<2) & (signed long)0xFFFF0000)) *(DEL_N_DELTA/(double)(1ul<<16)); W1.U_W = ((unsigned long)(Sub3->Word8 >> 6) + ((unsigned long)((Sub3->Word7 & 0x3FC0) << 18))); W = (double)(W1.S_W)*W_24_LSB_DELTA; /* Compute derived data */ A = Root_A*Root_A; /* -- Find the orbit's semi-major axis */ N0 = SQRT_EGP / (Root_A * A); /* -- Find the mean motion */ Orbital_Parameters->N = N0 + Del_N; /* -- Compute corrected mean motion */ Orbital_Parameters->Sin_W = sin(W); Orbital_Parameters->Cos_W = cos(W); /* -- Compute the factors of the true anomaly series expansion */ E_Square = Orbital_Parameters->E * Orbital_Parameters->E; Orbital_Parameters->C0 = A * (1.0 - E_Square); /* -- Compute the C0 term */ Orbital_Parameters->Curve_Interval = (char)(Sub2->Word10 >> 13) & 0x1; Orbital_Parameters->Valid_Ephem_Decoded = TRUE; Orbital_Parameters->SV_Data_Source = EPHEMERID; Orbital_Parameters->IODE = Sub3->Word10 >> 22; } /*---------------------------------------------------------------------------*/ void Decode_Clock_Parameters( SUBFRAME_ARRAY_TYPE *Sub1, CLOCK_PARAMETERS_TYPE *Clock_Parameters ){ union { unsigned long U; signed long S; }AF0; AF0.U = (unsigned long)((Sub1->Word10 >> 8)*SCALE_UP_AF0); Clock_Parameters->Af0 = (double)(AF0.S * AF0_DELTA); Clock_Parameters->Af1 = (double)((signed long) ((Sub1->Word9 << 10 ) & (signed long)0xFFFF0000) *(AF1_DELTA/(double)(1ul<<16))); Clock_Parameters->Af2 = (double)((signed char) (Sub1->Word9 >> 22)*AF2_DELTA); Clock_Parameters->Toc = (double)((unsigned long) ((Sub1->Word8 << 10 ) & (unsigned long)0xFFFF0000) *(TOC_DELTA/(double)(1ul<<16))); Clock_Parameters->Tgd = (double)((signed char) ((Sub1->Word7 >> 6) & (signed char)0xFF)*TGD_DELTA); } /*-------------------------------------------------------------------------*/ void Get_Chan_Assignment(HEADER_TYPE Header) { /* message 6-7 */ CHANS_ASSIGN_TYPE Assign; unsigned char i; if (!Assign_Open) { if ((outassign = fopen("Assign.tm","wt")) == NULL) { printf("Unable to open output file Assign.tm\n"); Quit(); } Assign_Open = 1; if (Flag_T) { fprintf(outassign,"Index SV Reserved1 Reserved2 Carrier_DCO C_No "); fprintf(outassign,"Status1 Status2 Status3 Status4 \n"); } fprintf(outassign,"%5lu ",Index); } if (Assign_Open) { read(in,(char *)&Assign,Header.Seq); //read the complete message for (i=0;i<6;i++) { fprintf(outassign,"%2d " , (Assign.Chan[i].SV & 0x1F) + 1); fprintf(outassign,"%1d " ,(Assign.Chan[i].SV & 0x40) >> 6); fprintf(outassign,"%1d " ,(Assign.Chan[i].SV & 0x80) >> 7); fprintf(outassign,"%10u " , Assign.Chan[i].Carrier_DCO); fprintf(outassign,"%6.2g " , Assign.Chan[i].C_No); fprintf(outassign,"%1u " , Assign.Chan[i].Status & 0x03); fprintf(outassign,"%1u " ,(Assign.Chan[i].Status & 0x0C) >> 2); fprintf(outassign,"%1u " ,(Assign.Chan[i].Status & 0x10) >> 4); fprintf(outassign,"%1u " ,(Assign.Chan[i].Status & 0xE0) >> 5); } } } /*-------------------------------------------------------------------------*/ void Get_Nav_User_Coord(HEADER_TYPE Header){ /* message 20 */ NAV_USER_TYPE Data; if (!Nav_User_Open) { if ((outunav = fopen("NavUser.tm","wt")) == NULL) { printf("Unable to open output file NavUser.tm\n"); Quit(); } if (Flag_T) { fprintf(outunav,"Index Longitude Latitude Altitude "); fprintf(outunav,"Hours Minutes Seconds Speed Angle "); fprintf(outunav,"North_Vel East_Vel Vert_Vel "); fprintf(outunav,"Year Month Day HFOM VFOM HDOP VDOP "); fprintf(outunav,"NAV_Mode Nb_SV Resvd1 Resvd2 "); fprintf(outunav,"Time_Align.\n"); } Nav_User_Open = 1; } read(in,(char *)&Data,Header.Seq); fprintf(outunav,"%5lu\t" ,Index); fprintf(outunav,"%12.6f\t" ,(Data.Longitude / (PI*2)) * 360.0); fprintf(outunav,"%12.6f\t" ,(Data.Latitude / (PI*2)) * 360.0); fprintf(outunav,"%12.6f\t" ,Data.Altitude); fprintf(outunav,"%2u\t%2u\t%2.6lf\t" ,Data.Hours,Data.Minutes,Data.Seconds); fprintf(outunav,"%12.6f\t%12.6f\t" ,Data.Speed,Data.Angle); fprintf(outunav,"%12.6f\t%12.6f\t" ,Data.N_Vel,Data.E_Vel); fprintf(outunav,"%12.6f\t" ,Data.V_Vel); fprintf(outunav,"%4u\t%2u\t%2u\t" ,Data.Year,Data.Month,Data.Day); fprintf(outunav,"%2.3lf\t" ,Data.HFOM); fprintf(outunav,"%12.6f\t" ,Data.VFOM); fprintf(outunav,"%6.3f\t" ,Data.HDOP * 0.1); fprintf(outunav,"%6.3f\t" ,Data.VDOP * 0.1); fprintf(outunav,"%2u\t" ,Data.Nav_Mode & 0x1F); fprintf(outunav,"%2u\t" ,Data.Nb_SV & 0x0F); fprintf(outunav,"%2u\t" ,Data.Reserved1); fprintf(outunav,"%2u\t" ,Data.Reserved2); fprintf(outunav,"%2u\t" ,Data.Nav_Mode >> 7); fprintf(outunav,"\n"); } /*-------------------------------------------------------------------------*/ void Get_Nav_GPS_Coord(HEADER_TYPE Header) { /* message 21 */ NAV_GPS_TYPE Data; if (!Nav_GPS_Open) { if ((outnav = fopen("NavGPS.tm","wt")) == NULL) { printf("Unable to open output file Nav.tm\n"); Quit(); } if (Flag_T) { fprintf(outnav,"Index Sec Week Reserved Pos_X Pos_Y Pos_Z "); fprintf(outnav,"X_Vel Y_Vel Z_Vel Bias Drift HFOM VFOM "); fprintf(outnav,"HDOP VDOP Nav_Mode Conf. Rsvd Rsvd Nb_SV\n"); } Nav_GPS_Open = 1; } read(in,(char *)&Data,Header.Seq); fprintf(outnav,"%5lu ",Index); fprintf(outnav,"%24.19g ",Data.GPS_Time_Sec); fprintf(outnav,"%12u " ,Data.Week); fprintf(outnav,"%12lu " ,Data.Resv1|Data.Resv2<<8|Data.Resv3<<16); //reserved fprintf(outnav,"%24.19g ",Data.X_Pos); fprintf(outnav,"%24.19g ",Data.Y_Pos); fprintf(outnav,"%24.19g ",Data.Z_Pos); fprintf(outnav,"%24.19g ",Data.X_Vel); fprintf(outnav,"%24.19g ",Data.Y_Vel); fprintf(outnav,"%24.19g ",Data.Z_Vel); fprintf(outnav,"%24.19g ",Data.Clock_Bias); fprintf(outnav,"%24.19g ",Data.Clock_Drift); fprintf(outnav,"%24.19g ",Data.HFOM); fprintf(outnav,"%24.19g ",Data.VFOM); fprintf(outnav,"%12u " ,Data.HDOP); fprintf(outnav,"%12u " ,Data.VDOP); fprintf(outnav,"%2d " ,Data.Nav_Mode & 0x1F); //nav mode fprintf(outnav,"%1d " ,(Data.Nav_Mode & 0x20) >> 5); //confidence level fprintf(outnav,"%1d " ,(Data.Nav_Mode & 0x40) >> 6); //reserved bit fprintf(outnav,"%1d " ,(Data.Nav_Mode & 0x80) >> 7); //time alignment fprintf(outnav,"%12lu " ,(Data.Nb_SV & 0x0F)); // number of SV used fprintf(outnav,"\n"); } /*-------------------------------------------------------------------------*/ void Get_Raw_Ephem_Block(HEADER_TYPE Header) { /* message 22 */ RAW_EPHEM_TYPE NBlocks; SUBFRAME_ARRAY_TYPE Sub1, Sub2, Sub3; unsigned char SV; // CLOCK_PARAMETERS_TYPE Clock_Parameters; // ORBIT_PARAMETERS_TYPE Orbital_Parameters; read(in,(char *)&NBlocks,Header.Seq); //read the complete message Sub1.Word3 = ((unsigned long)NBlocks.Bytes[0] <<22)+((unsigned long)NBlocks.Bytes[1] <<14)+((unsigned long)NBlocks.Bytes[2]<<6); Sub1.Word4 = ((unsigned long)NBlocks.Bytes[3] <<22)+((unsigned long)NBlocks.Bytes[4] <<14)+((unsigned long)NBlocks.Bytes[5]<<6); Sub1.Word5 = ((unsigned long)NBlocks.Bytes[6] <<22)+((unsigned long)NBlocks.Bytes[7] <<14)+((unsigned long)NBlocks.Bytes[8]<<6); Sub1.Word6 = ((unsigned long)NBlocks.Bytes[9] <<22)+((unsigned long)NBlocks.Bytes[10]<<14)+((unsigned long)NBlocks.Bytes[11]<<6); Sub1.Word7 = ((unsigned long)NBlocks.Bytes[12]<<22)+((unsigned long)NBlocks.Bytes[13]<<14)+((unsigned long)NBlocks.Bytes[14]<<6); Sub1.Word8 = ((unsigned long)NBlocks.Bytes[15]<<22)+((unsigned long)NBlocks.Bytes[16]<<14)+((unsigned long)NBlocks.Bytes[17]<<6); Sub1.Word9 = ((unsigned long)NBlocks.Bytes[18]<<22)+((unsigned long)NBlocks.Bytes[19]<<14)+((unsigned long)NBlocks.Bytes[20]<<6); Sub1.Word10= ((unsigned long)NBlocks.Bytes[21]<<22)+((unsigned long)NBlocks.Bytes[22]<<14)+((unsigned long)NBlocks.Bytes[23]<<6); Sub2.Word3 = ((unsigned long)NBlocks.Bytes[24+0] <<22)+((unsigned long)NBlocks.Bytes[24+1] <<14)+((unsigned long)NBlocks.Bytes[24+2]<<6); Sub2.Word4 = ((unsigned long)NBlocks.Bytes[24+3] <<22)+((unsigned long)NBlocks.Bytes[24+4] <<14)+((unsigned long)NBlocks.Bytes[24+5]<<6); Sub2.Word5 = ((unsigned long)NBlocks.Bytes[24+6] <<22)+((unsigned long)NBlocks.Bytes[24+7] <<14)+((unsigned long)NBlocks.Bytes[24+8]<<6); Sub2.Word6 = ((unsigned long)NBlocks.Bytes[24+9] <<22)+((unsigned long)NBlocks.Bytes[24+10]<<14)+((unsigned long)NBlocks.Bytes[24+11]<<6); Sub2.Word7 = ((unsigned long)NBlocks.Bytes[24+12]<<22)+((unsigned long)NBlocks.Bytes[24+13]<<14)+((unsigned long)NBlocks.Bytes[24+14]<<6); Sub2.Word8 = ((unsigned long)NBlocks.Bytes[24+15]<<22)+((unsigned long)NBlocks.Bytes[24+16]<<14)+((unsigned long)NBlocks.Bytes[24+17]<<6); Sub2.Word9 = ((unsigned long)NBlocks.Bytes[24+18]<<22)+((unsigned long)NBlocks.Bytes[24+19]<<14)+((unsigned long)NBlocks.Bytes[24+20]<<6); Sub2.Word10= ((unsigned long)NBlocks.Bytes[24+21]<<22)+((unsigned long)NBlocks.Bytes[24+22]<<14)+((unsigned long)NBlocks.Bytes[24+23]<<6); Sub3.Word3 = ((unsigned long)NBlocks.Bytes[48+0] <<22)+((unsigned long)NBlocks.Bytes[48+1] <<14)+((unsigned long)NBlocks.Bytes[48+2]<<6); Sub3.Word4 = ((unsigned long)NBlocks.Bytes[48+3] <<22)+((unsigned long)NBlocks.Bytes[48+4] <<14)+((unsigned long)NBlocks.Bytes[48+5]<<6); Sub3.Word5 = ((unsigned long)NBlocks.Bytes[48+6] <<22)+((unsigned long)NBlocks.Bytes[48+7] <<14)+((unsigned long)NBlocks.Bytes[48+8]<<6); Sub3.Word6 = ((unsigned long)NBlocks.Bytes[48+9] <<22)+((unsigned long)NBlocks.Bytes[48+10]<<14)+((unsigned long)NBlocks.Bytes[48+11]<<6); Sub3.Word7 = ((unsigned long)NBlocks.Bytes[48+12]<<22)+((unsigned long)NBlocks.Bytes[48+13]<<14)+((unsigned long)NBlocks.Bytes[48+14]<<6); Sub3.Word8 = ((unsigned long)NBlocks.Bytes[48+15]<<22)+((unsigned long)NBlocks.Bytes[48+16]<<14)+((unsigned long)NBlocks.Bytes[48+17]<<6); Sub3.Word9 = ((unsigned long)NBlocks.Bytes[48+18]<<22)+((unsigned long)NBlocks.Bytes[48+19]<<14)+((unsigned long)NBlocks.Bytes[48+20]<<6); Sub3.Word10= ((unsigned long)NBlocks.Bytes[48+21]<<22)+((unsigned long)NBlocks.Bytes[48+22]<<14)+((unsigned long)NBlocks.Bytes[48+23]<<6); SV = NBlocks.SV; Calc_Orbital_Parameter(&Sub2,&Sub3,&Orbital_Parameters[SV]); Decode_Clock_Parameters(&Sub1,&Clock_Parameters[SV]); Ephem_Avaliable[SV] = TRUE; if (!Ephem_Open) { if ((outephem = fopen("ephem.tm","wt")) == NULL) { printf("Unable to open output file Ephem.tm\n"); Quit(); } Ephem_Open = 1; } fprintf(outephem,"SV: %5u \n" ,NBlocks.SV+1); fprintf(outephem,"Interval: %5u \n" ,Orbital_Parameters[SV].Curve_Interval); fprintf(outephem,"Source: %5u \n" ,Orbital_Parameters[SV].SV_Data_Source); fprintf(outephem,"IODE : %5u \n\n" ,Orbital_Parameters[SV].IODE); fprintf(outephem,"GPS_Time = %24.19g \n" ,GPS_Time); fprintf(outephem,"Toe = %24.19g \n" ,Orbital_Parameters[SV].Toe); fprintf(outephem,"E = %24.19g \n" ,Orbital_Parameters[SV].E); fprintf(outephem,"Root_A = %24.19g \n" ,Orbital_Parameters[SV].Root_A); fprintf(outephem,"N = %24.19g \n" ,Orbital_Parameters[SV].N); fprintf(outephem,"C0 = %24.19g \n" ,Orbital_Parameters[SV].C0); fprintf(outephem,"M0 = %24.19g \n" ,Orbital_Parameters[SV].M0); fprintf(outephem,"Sin_W = %24.19g \n" ,Orbital_Parameters[SV].Sin_W ); fprintf(outephem,"Cos_W = %24.19g \n" ,Orbital_Parameters[SV].Cos_W ); // fprintf(outephem,"K1 = %24.19g \n" ,Orbital_Parameters[SV].V_Series_Factors.K1); // fprintf(outephem,"K2 = %24.19g \n" ,Orbital_Parameters[SV].V_Series_Factors.K2); // fprintf(outephem,"K3 = %24.19g \n" ,Orbital_Parameters[SV].V_Series_Factors.K3); // fprintf(outephem,"K4 = %24.19g \n" ,Orbital_Parameters[SV].V_Series_Factors.K4); // fprintf(outephem,"K5 = %24.19g \n" ,Orbital_Parameters[SV].V_Series_Factors.K5); fprintf(outephem,"I_0 = %24.19g \n" ,Orbital_Parameters[SV].I_0); fprintf(outephem,"I_Dot = %24.19g \n" ,Orbital_Parameters[SV].I_Dot); fprintf(outephem,"Omega_0 = %24.19g \n" ,Orbital_Parameters[SV].Omega_0); fprintf(outephem,"Omega_Dot = %24.19g \n" ,Orbital_Parameters[SV].Omega_Dot); fprintf(outephem,"Crs = %24.19g \n" ,Orbital_Parameters[SV].Crs); fprintf(outephem,"Crc = %24.19g \n" ,Orbital_Parameters[SV].Crc); fprintf(outephem,"Cis = %24.19g \n" ,Orbital_Parameters[SV].Cis); fprintf(outephem,"Cic = %24.19g \n" ,Orbital_Parameters[SV].Cic); fprintf(outephem,"Cus = %24.19g \n" ,Orbital_Parameters[SV].Cus); fprintf(outephem,"Cuc = %24.19g \n\n\n" ,Orbital_Parameters[SV].Cuc); fprintf(outephem,"Af0 = %24.19g \n" ,Clock_Parameters[SV].Af0); fprintf(outephem,"Af1 = %24.19g \n" ,Clock_Parameters[SV].Af1); fprintf(outephem,"Af2 = %24.19g \n" ,Clock_Parameters[SV].Af2); fprintf(outephem,"Toc = %24.19g \n" ,Clock_Parameters[SV].Toc); fprintf(outephem,"Tgd = %24.19g \n\n\n" ,Clock_Parameters[SV].Tgd); } /*-------------------------------------------------------------------------*/ void Get_NEW_UGPS_Meas_Block(HEADER_TYPE Header){ /* message 23 */ FAST_MEAS_BLOCKS_TYPE Data; unsigned long i; unsigned char SV_NB,Status; double Int_Carr; read(in,(char *)&Data,Header.Seq); //read the complete message /* GPS time is required for the ephemeris data*/ GPS_Time = Data.GPS_Time; for(i=0;i>12) + ((double)((Data.Block[i].ICP & 0xfff)>>2)/1024.0); Status = (unsigned char)(Data.Block[i].ICP & 3); fprintf(Foutmeas[SV_NB],"%24.19g ",Int_Carr); fprintf(Foutmeas[SV_NB],"%3d " ,Status); fprintf(Foutmeas[SV_NB],"%3d\n " ,Data.Block[i].Locktime); } } void Read_Msgs(void) { HEADER_TYPE Header; unsigned short i; unsigned char scrap[200]; unsigned long Cnt; for (i=0;i<32;i++){ Fast_Meas_Open[i] = 0; Ephem_Avaliable[i] = 0; } for (i=0;i<12;i++) Chan_Assign[i] = 0; // /* Skip starting zero's */ // if (!Flag_D) { // for (i=0;i<(1024+14);i++) // read(in,(char *)&Dummy,1); // } // Cnt = 1024 + 14; Cnt = 0; while (read(in,scrap,1) != 0) { // 0 == NULL if (scrap[0] == 1) { if (read(in,((char *)(&Header))+1,sizeof(Header)-1) == 0) { // 0 == NULL printf("EOF\n"); Quit(); } else { Header.SOH = scrap[0]; if ((Header.SOH==1) && ((Header.Id + Header.NId) != 255)) { lseek(in,-(long)(sizeof(Header)-1),SEEK_CUR); printf("\nID != NId\n"); printf("Bug at byte %ul (%X) %ul\n",Cnt,Cnt,Cnt+1); printf("SOH = %d\n",Header.SOH); printf(" Id = %d\n",Header.Id); printf("NId = %d\n",Header.NId); printf("Seq = %d\n",Header.Seq); Cnt = Cnt + 5; } else { switch (Header.Id) { case 6: if (Flag_C) { if (Assign_Open) fprintf(outassign,"%5lu ",Index); Get_Chan_Assignment(Header); read(in,scrap,2); //read the checksum } else read(in,scrap,Header.Seq + 2); break; case 7: if (Flag_C && Assign_Open) { Get_Chan_Assignment(Header); fprintf(outassign,"\n"); read(in,scrap,2); //read the checksum } else read(in,scrap,Header.Seq + 2); break; case 20: if (Flag_U) { Get_Nav_User_Coord(Header); read(in,scrap,2); //read the checksum } else read(in,scrap,Header.Seq + 2); break; case 21: if (Flag_G) { Get_Nav_GPS_Coord(Header); read(in,scrap,2); //read the checksum } else read(in,scrap,Header.Seq + 2); Index++; printf("+"); break; case 22: if(Flag_E) { Get_Raw_Ephem_Block(Header); read(in,scrap,2); //read the checksum } else read(in,scrap,Header.Seq + 2); break; case 23: if(Flag_R) { Get_NEW_UGPS_Meas_Block(Header); read(in,scrap,2); //read the checksum } else read(in,scrap,Header.Seq + 2); break; default: if ((Header.SOH == 1) && (Header.Id+Header.NId==255)) { read(in,scrap, Header.Seq + 2); } else { printf("\nError, unkown message\n"); printf(" Bug at byte %ul (%X)%ul\n",Cnt,Cnt,Cnt+1); printf(" SOH = %d\n",Header.SOH); printf(" Id = %d\n",Header.Id); printf(" NId = %d\n",Header.NId); printf(" Seq = %d\n",Header.Seq); Quit(); } break; } Cnt = Cnt + Header.Seq + 6; } /* end else,i.e. if header found */ } /* end if not eof while reading header */ } /* end if soh found */ else Cnt++; } /* end while loop */ printf("End of file\n"); } void Quit(void) { unsigned char i; close(in); if (Error_Open) fclose(outerror); if (Ephem_Open) fclose(outephem); if (Assign_Open ) fclose(outassign); if (Nav_GPS_Open ) fclose(outnav); if (Nav_User_Open) fclose(outunav); for (i=0;i<32;i++) if (Fast_Meas_Open[i]) fclose(Foutmeas[i]); fcloseall(); exit(0); } void Print_Help(void) { printf("Utility to convert CMC BINARY messages in ASCII format\n"); printf("Version 1.2c\n\n"); printf("Usage: GPS2TXT Infile /[options]\n"); printf(" Options: (Select messaged to be decoded)\n"); printf(" c : Channel Assignment (msg 6-7) \n"); printf(" u : Nav User Coordinates (msg 20)\n"); printf(" g : Nav GPS Coordinates (msg 21)\n"); printf(" e : Ephemeris-ICDGPS200 (msg 22)\n"); printf(" r : Measblock (msg 23)\n"); printf(" t : Titles (On first file line)\n"); printf(" a : All (All messages)\n"); printf(" v : Verbose\n"); printf(" h : Help\n"); printf(" ? : Help\n\n"); printf(" Exemple : GPS2TXT TEST.LOG /a /t"); exit(0); } void main(int argc,char *argv[]) { unsigned char i; Flag_C = FALSE; Flag_D = FALSE; Flag_E = FALSE; Flag_G = FALSE; Flag_R = FALSE; Flag_T = FALSE; Flag_U = FALSE; Flag_V = FALSE; Flag_P = FALSE; Version = 2; if (argc == 1 || argc == 2){ Print_Help(); } else { printf("Openning file %s\n",argv[1]); if ((in = open(argv[1],O_RDONLY|O_BINARY)) < 0) { printf("Unable to open input file %s\n",argv[1]); exit(0); } else { /* Decode options */ for (i=2; i < argc; i++) { if (argv[i][0] == '/') { switch (toupper((argv[i][1] & 0xdf))) { case 'A': Flag_C = TRUE; Flag_U = TRUE; Flag_G = TRUE; Flag_E = TRUE; Flag_R = TRUE; break; case 'C': Flag_C = TRUE; break; case 'D': Flag_D = TRUE; break; case 'E': Flag_E = TRUE; break; case 'G': Flag_G = TRUE; break; case 'H': Print_Help(); break; case 'P': Flag_P = TRUE; break; case 'R': Flag_R = TRUE; break; case 'T': Flag_T = TRUE; break; case 'U': Flag_U = TRUE; break; case 'V': Flag_V = TRUE; break; case 0x1f: //'?'=0x3f Print_Help(); break; default: Print_Help(); break; } /* End switch */ } /* End if */ } /* End for */ } /* End else */ } /* End else */ printf("Reading messages\n"); Read_Msgs(); Quit(); }