www.pudn.com > BncSource.zip > cgps_transform.cpp
#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;
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);
}
}