void CNAVMessageAsm::addMDPNavSubframe( const gpstk::MDPNavSubframe& mdpsf)
// const gpstk::GPSOrbElemStore& store)
{
// Create a new PackedNavBits object from the MDPNavSubframe object.
short msgType = getMessageType(mdpsf);
//std::cout << "In addMDPNavSubframe: msgType:" << msgType << ", wkSet:" << weekSet
// << ", week:" << currentWeek << std::endl;
// Can't do much of anything until we see a msg type 10 and determine
// the current week number.
if (!weekSet && msgType!=10) return;
if (!weekSet && msgType==10)
{
currentWeek = getWeek(mdpsf);
weekSet = true;
}
short PRNID = getPRNID(mdpsf);
SatID satID(PRNID, SatID::systemGPS);
ObsID::CarrierBand cb = ObsID::cbUnknown;
switch (mdpsf.carrier)
{
case ccL1 : cb = ObsID::cbL1; break;
case ccL2 : cb = ObsID::cbL2; break;
case ccL5 : cb = ObsID::cbL5; break;
default: break;
}
ObsID::TrackingCode tc = ObsID::tcUnknown;
switch (mdpsf.range)
{
case rcCM : tc = ObsID::tcC2M; break;
case rcCL : tc = ObsID::tcC2L; break;
case rcCMCL : tc = ObsID::tcC2LM; break;
case rcI5 : tc = ObsID::tcI5; break;
case rcQ5 : tc = ObsID::tcQ5; break;
default: break;
}
ObsID::ObservationType ot = ObsID::otNavMsg;
ObsID obsID(ot, cb, tc);
unsigned long xmitSOW = getTOWCount(mdpsf) - 12;
CommonTime xmitTime = GPSWeekSecond(currentWeek, xmitSOW, TimeSystem::GPS);
std::cout << "SOW, time:" << xmitSOW << ", " << xmitTime << std::endl;
PackedNavBits* pnbp = new PackedNavBits(satID, obsID, xmitTime);
// Now add the data
for (int i=1; i<=10; i++)
{
unsigned long ul = (unsigned long) mdpsf.subframe[i];
pnbp->addUnsignedLong(ul, 30, 1);
}
pnbp->trimsize();
// Debug
std::cout << "MsgType:" << msgType << std::endl;
pnbp->dump();
std::cout << std::endl;
// Add the new PackedNavBits object to the map of current messages.
int ndx = -1;
bool foundA3xMsg = false;
if (msgType==10) ndx = 0;
if (msgType==11) ndx = 1;
if (msgType>=30 && msgType<=37)
{
ndx = 2;
foundA3xMsg = true;
}
if (ndx>=0)
{
std::cout << " Update msgType " << msgType << ", ndx " << ndx << std::endl;
currentMsgMap[ndx] = pnbp->clone();
}
// If this is a msg type 3X, check to see if the map has a contiguous
// set of Msg 10/Msg 11/Msg 30x for this SatId/ObsID. If so, attempt
// to create a CNAVOrbElem object. If that succeeds, add the object
// to the orbital element store.
if (foundA3xMsg &&
currentMsgMap[0] != 0 &&
currentMsgMap[1] != 0 &&
currentMsgMap[2] != 0 )
{
CommonTime time10 = currentMsgMap[0]->getTransmitTime();
time10.setTimeSystem(TimeSystem::GPS);
/*
std::cout << "Time (10): " << time10 << std::endl;
std::cout << "Size, SatID, ObsID: " << currentMsgMap[0]->getNumBits()
<< ", " << currentMsgMap[0]->getsatSys()
<< ", " << currentMsgMap[0]->getobsID() << std::endl;
*/
CommonTime time11 = currentMsgMap[1]->getTransmitTime();
time11.setTimeSystem(TimeSystem::GPS);
CommonTime time3x = currentMsgMap[2]->getTransmitTime();
time3x.setTimeSystem(TimeSystem::GPS);
/*
std::cout << "Time (30): " << time3x << std::endl;
std::cout << "Size, SatID, ObsID: " << currentMsgMap[2]->getNumBits()
<< ", " << currentMsgMap[2]->getsatSys()
<< ", " << currentMsgMap[2]->getobsID() << std::endl;
*/
double diff11minus10 = time11-time10;
std::cout << "diff11minus10: " << diff11minus10 << std::endl;
if (diff11minus10==12.0)
{
double diff3xMinus10 = time3x - time10;
std::cout << "diff3xMinus10: " << diff3xMinus10 << std::endl;
if (diff3xMinus10<=36)
{
std::cout << "Attempt to create a CNAV object" << std::endl;
SatID satId = currentMsgMap[0]->getsatSys();
ObsID obsId = currentMsgMap[0]->getobsID();
try
{
OrbElemCNAV oecn (obsId, satId,
*currentMsgMap[0],
*currentMsgMap[1],
*currentMsgMap[2]);
std::cout << oecn << std::endl;
}
catch(gpstk::Exception e)
{
std::cout << "Caught exception converting to OrbElemCNAV." << std::endl;
std::cout << e << std::endl;
}
}
}
}
}
void xPackedNavBitsgpsNavMsg::firstTest(void)
{
// Set time to Day 153, 2011 (6/2/2011) at noon
CivilTime g( 2011, 6, 2, 12, 14, 44.0, TimeSystem::GPS );
CommonTime TransmitTime = g.convertToCommonTime();
SatID satSys(1, SatID::systemGPS);
ObsID obsID( ObsID::otNavMsg, ObsID::cbL2, ObsID::tcC2LM );
// Test Unsigned Integers
unsigned long u_i1 = 32767;
int u_n1 = 16;
int u_s1 = 1;
unsigned long u_i2 = 1;
int u_n2 = 8;
int u_s2 = 1;
unsigned long u_i3 = 255;
int u_n3 = 8;
int u_s3 = 1;
unsigned long u_i4 = 604500;
int u_n4 = 11;
int u_s4 = 300;
// Test Signed Integers
long s_i1 = 15;
int s_n1 = 5;
int s_s1 = 1;
long s_i2 = -16;
int s_n2 = 5;
int s_s2 = 1;
long s_i3 = -1;
int s_n3 = 5;
int s_s3 = 1;
long s_i4 = 0;
int s_n4 = 6;
int s_s4 = 1;
long s_i5 = 4194304;
int s_n5 = 24;
int s_s5 = 1;
// Test Unsigned Doubles
double d_i1 = 0.490005493;
int d_n1 = 16;
int d_s1 = -16;
double d_i2 = -0.5;
int d_n2 = 16;
int d_s2 = -16;
double d_i3 = 0;
int d_n3 = 16;
int d_s3 = -16;
// Test Signed Doubles
double d_i4 = 32000.0;
int d_n4 = 16;
int d_s4 = 0;
// Test Semi-Circles
double sd_i1 = PI-2*pow(2,-31);
int sd_n1 = 32;
int sd_s1 = -31;
double sd_i2 = -PI;
int sd_n2 = 32;
int sd_s2 = -31;
//Test Data copied from RINEX file for PRN3, week 1638, day 153 2011
double rToe = 388800.0;
int n_rToe = 16;
int s_rToe = 4;
unsigned long riodc = 22;
int n_riodc = 8;
int s_riodc = 1;
unsigned long riode = 22;
int n_riode = 8;
int s_riode = 1;
unsigned long raodo = 10;
int n_raodo = 5;
int s_raodo = 1;
unsigned long rfitInt = 0;
int n_rfitInt = 1;
int s_rfitInt = 1;
double rToc = 388800.0;
int n_rToc = 16;
int s_rToc = 4;
double rCuc = 9.57399606705E-07;
int n_rCuc = 16;
int s_rCuc = -29;
double rCus = 8.35768878460E-06;
int n_rCus = 16;
int s_rCus = -29;
double rCrc = 2.03562500000E+02;
int n_rCrc = 16;
int s_rCrc = -5;
double rCrs = 1.87812500000E+01;
int n_rCrs = 16;
int s_rCrs = -5;
double rCic = -2.30967998505E-07;
int n_rCic = 16;
int s_rCic = -29;
double rCis = 5.02914190292E-08;
int n_rCis = 16;
int s_rCis = -29;
double rM0 = 1.05539162795E+00;
int n_rM0 = 32;
int s_rM0 = -31;
double rdn = 5.39093883996E-09;
int n_rdn = 16;
int s_rdn = -43;
double recc = 1.42575260252E-02;
int n_recc = 32;
int s_recc = -33;
double rAhalf = 5.15365527534E+03;
int n_rAhalf = 32;
int s_rAhalf = -19;
double rOMEGA0 = -2.16947563164E+00;
int n_rOMEGA0 = 32;
int s_rOMEGA0 = -31;
double ri0 = 9.28692497530E-01;
int n_ri0 = 32;
int s_ri0 = -31;
double rw = 1.09154604931E+00;
int n_rw = 32;
int s_rw = -31;
double rOMEGAdot = -8.56285667735E-09;
int n_rOMEGAdot = 24;
int s_rOMEGAdot = -43;
double ridot = 5.52880172536E-10;
int n_ridot = 14;
int s_ridot = -43;
double raf0 = 7.23189674318E-04;
int n_raf0 = 22;
int s_raf0 = -31;
double raf1 = 5.11590769747E-12;
int n_raf1 = 16;
int s_raf1 = -43;
double raf2 = 0.0;
int n_raf2 = 8;
int s_raf2 = -55;
double rTgd = -4.65661287308E-09;
int n_rTgd = 8;
int s_rTgd = -31;
ofstream outf("Logs/PackedNavBits_Output", ios::out);
outf.precision(11);
// First Test Case. Create PNB object.
PackedNavBits pnb;
/* Pack */
pnb.setSatID(satSys);
pnb.setObsID(obsID);
pnb.setTime(TransmitTime);
/* pnb.addUnsignedLong(u_i1, u_n1, u_s1);
pnb.addUnsignedLong(u_i2, u_n2, u_s2);
pnb.addUnsignedLong(u_i3, u_n3, u_s3);
pnb.addUnsignedLong(u_i4, u_n4, u_s4);
pnb.addLong(s_i1, s_n1, s_s1);
pnb.addLong(s_i2, s_n2, s_s2);
pnb.addLong(s_i3, s_n3, s_s3);
pnb.addLong(s_i4, s_n4, s_s4);
pnb.addLong(s_i5, s_n5, s_s5);
pnb.addSignedDouble(d_i1, d_n1, d_s1);
pnb.addSignedDouble(d_i2, d_n2, d_s2);
pnb.addSignedDouble(d_i3, d_n3, d_s3);
pnb.addUnsignedDouble(d_i4, d_n4, d_s4);
pnb.addDoubleSemiCircles(sd_i1, sd_n1, sd_s1);
pnb.addDoubleSemiCircles(sd_i2, sd_n2, sd_s2); */
/* Unpack */
/* outf << endl;
outf << "Unpacked Unsigned Integers:" << endl;
int startbit = 0;
outf << "Number 32767: " << pnb.asUnsignedLong(startbit, u_n1, u_s1) << endl;
startbit += u_n1;
outf << "Number 1: " << pnb.asUnsignedLong(startbit, u_n2, u_s2) << endl;
startbit += u_n2;
outf << "Number 255: " << pnb.asUnsignedLong(startbit, u_n3, u_s3) << endl;
startbit += u_n3;
outf << "Number 604500: " << pnb.asUnsignedLong(startbit, u_n4, u_s4) << endl;
startbit += u_n4;
outf << endl;
outf << "Unpacked Signed Integers: " << endl;
outf << "Number 15: " << pnb.asLong(startbit, s_n1, s_s1) << endl;
startbit += s_n1;
outf << "Number -16: " << pnb.asLong(startbit, s_n2, s_s2) << endl;
startbit += s_n2;
outf << "Number -1: " << pnb.asLong(startbit, s_n3, s_s3) << endl;
startbit += s_n3;
outf << "Number 0: " << pnb.asLong(startbit, s_n4, s_s4) << endl;
startbit += s_n4;
outf << "Number 4194304: " << pnb.asLong(startbit, s_n5, s_s5) << endl;
startbit += s_n5;
outf << endl;
outf << "Unpacked Signed Doubles: " << endl;
outf << "Number 0.490005493: " << pnb.asSignedDouble(startbit, d_n1, d_s1) << endl;
startbit += d_n1;
outf << "Number -0.5: " << pnb.asSignedDouble(startbit, d_n2, d_s2) << endl;
startbit += d_n2;
outf << "Number 0: " << pnb.asSignedDouble(startbit, d_n3, d_s3) << endl;
startbit += d_n3;
outf << endl;
outf << "Unpacked Unsigned Doubles: " << endl;
outf << "Number 32000.0: " << pnb.asUnsignedDouble(startbit, d_n4, d_s4) << endl;
startbit += d_n4;
outf << endl;
outf << "Unpacked Double Semi-Circles: " << endl;
outf << "Number PI: " << pnb.asDoubleSemiCircles(startbit, sd_n1, sd_s1) << endl;
startbit += sd_n1;
outf << "Number -PI: " << pnb.asDoubleSemiCircles(startbit, sd_n2, sd_s2) << endl; */
// Pack legacy nav message data
pnb.addSignedDouble(rTgd, n_rTgd, s_rTgd);
pnb.addUnsignedLong(riodc, n_riodc, s_riodc);
pnb.addUnsignedDouble(rToc, n_rToc, s_rToc);
pnb.addSignedDouble(raf2, n_raf2, s_raf2);
pnb.addSignedDouble(raf1, n_raf1, s_raf1);
pnb.addSignedDouble(raf0, n_raf0, s_raf0);
pnb.addUnsignedLong(riode, n_riode, s_riode);
pnb.addSignedDouble(rCrs, n_rCrs, s_rCrs);
pnb.addDoubleSemiCircles(rdn, n_rdn, s_rdn);
pnb.addDoubleSemiCircles(rM0, n_rM0, s_rM0);
pnb.addSignedDouble(rCuc, n_rCuc, s_rCuc);
pnb.addUnsignedDouble(recc, n_recc, s_recc);
pnb.addSignedDouble(rCus, n_rCus, s_rCus);
pnb.addUnsignedDouble(rAhalf, n_rAhalf, s_rAhalf);
pnb.addUnsignedDouble(rToe, n_rToe, s_rToe);
pnb.addUnsignedLong(rfitInt, n_rfitInt, s_rfitInt);
pnb.addUnsignedLong(raodo, n_raodo, s_raodo);
pnb.addSignedDouble(rCic, n_rCic, s_rCic);
pnb.addDoubleSemiCircles(rOMEGA0, n_rOMEGA0, s_rOMEGA0);
pnb.addSignedDouble(rCis, n_rCis, s_rCis);
pnb.addDoubleSemiCircles(ri0, n_ri0, s_ri0);
pnb.addSignedDouble(rCrc, n_rCrc, s_rCrc);
pnb.addDoubleSemiCircles(rw, n_rw, s_rw);
pnb.addDoubleSemiCircles(rOMEGAdot, n_rOMEGAdot, s_rOMEGAdot);
pnb.addUnsignedLong(riode, n_riode, s_riode);
pnb.addDoubleSemiCircles(ridot, n_ridot, s_ridot);
// Unpack the legacy nav message data and get back the results in engineering terms
// Test Data copied from RINEX file for PRN3, week 1638, day 153 2011
int startbit = 0;
outf << "Tgd: " << pnb.asSignedDouble(startbit, n_rTgd, s_rTgd) << endl;
startbit += n_rTgd;
outf << "IODC: " << pnb.asUnsignedLong(startbit, n_riodc, s_riodc) << endl;
startbit += n_riodc;
outf << "Toc: " << pnb.asUnsignedDouble(startbit, n_rToc, s_rToc) << endl;
startbit += n_rToc;
outf << "af2: " << pnb.asSignedDouble(startbit, n_raf2, s_raf2) << endl;
startbit += n_raf2;
outf << "af1: " << pnb.asSignedDouble(startbit, n_raf1, s_raf1) << endl;
startbit += n_raf1;
outf << "af0: " << pnb.asSignedDouble(startbit, n_raf0, s_raf0) << endl;
startbit += n_raf0;
outf << "IODE: " << pnb.asUnsignedLong(startbit, n_riode, s_riode) << endl;
startbit += n_riode;
outf << "Crs: " << pnb.asSignedDouble(startbit, n_rCrs, s_rCrs) << endl;
startbit += n_rCrs;
outf << "dn: " << pnb.asDoubleSemiCircles(startbit, n_rdn, s_rdn) << endl;
startbit += n_rdn;
outf << "M0: " << pnb.asDoubleSemiCircles(startbit, n_rM0, s_rM0) << endl;
startbit += n_rM0;
outf << "Cuc: " << pnb.asSignedDouble(startbit, n_rCuc, s_rCuc) << endl;
startbit += n_rCuc;
outf << "ecc: " << pnb.asUnsignedDouble(startbit, n_recc, s_recc) << endl;
startbit += n_recc;
outf << "Cus: " << pnb.asSignedDouble(startbit, n_rCus, s_rCus) << endl;
startbit += n_rCus;
outf << "Ahalf: " << pnb.asUnsignedDouble(startbit, n_rAhalf, s_rAhalf) << endl;
startbit += n_rAhalf;
outf << "Toe: " << pnb.asUnsignedDouble(startbit, n_rToe, s_rToe) << endl;
startbit += n_rToe;
outf << "fitInt: " << pnb.asUnsignedLong(startbit, n_rfitInt, s_rfitInt) << endl;
startbit += n_rfitInt;
outf << "AODO: " << pnb.asUnsignedLong(startbit, n_raodo, s_raodo) << endl;
startbit += n_raodo;
outf << "Cic: " << pnb.asSignedDouble(startbit, n_rCic, s_rCic) << endl;
startbit += n_rCic;
outf << "OMEGA0: " << pnb.asDoubleSemiCircles(startbit, n_rOMEGA0, s_rOMEGA0) << endl;
startbit += n_rOMEGA0;
outf << "Cis: " << pnb.asSignedDouble(startbit, n_rCis, s_rCis) << endl;
startbit += n_rCis;
outf << "i0: " << pnb.asDoubleSemiCircles(startbit, n_ri0, s_ri0) << endl;
startbit += n_ri0;
outf << "Crc: " << pnb.asSignedDouble(startbit, n_rCrc, s_rCrc) << endl;
startbit += n_rCrc;
outf << "w: " << pnb.asDoubleSemiCircles(startbit, n_rw, s_rw) << endl;
startbit += n_rw;
outf << "OMEGAdot: " << pnb.asDoubleSemiCircles(startbit, n_rOMEGAdot, s_rOMEGAdot) << endl;
startbit += n_rOMEGAdot;
outf << "IODE: " << pnb.asUnsignedLong(startbit, n_riode, s_riode) << endl;
startbit += n_riode;
outf << "idot: " << pnb.asDoubleSemiCircles(startbit, n_ridot, s_ridot) << endl;
outf << endl;
outf << "Time of Transmission: " << pnb.getTransmitTime() << endl;
outf << "Time of Transmission pnb: " << GPSWeekSecond(pnb.getTransmitTime()).printf("%F, %g") << endl;
/* Resize the vector holding the packed nav message data. */
pnb.trimsize();
outf << endl;
outf << "PNB object dump:" << endl;
outf << pnb << endl;
CPPUNIT_ASSERT(fileEqualTest((char*)"Logs/PackedNavBits_Truth",(char*)"Logs/PackedNavBits_Output"));
}
void xCNAV2PNBgpsNavMsg::firstTest(void)
{
// Set time to Day 153, 2011 (6/2/2011) at noon
CivilTime g( 2011, 6, 2, 12, 14, 44.0, TimeSystem::GPS );
CommonTime TransmitTime = g.convertToCommonTime();
SatID satSys(1, SatID::systemGPS);
ObsID obsID( ObsID::otNavMsg, ObsID::cbL2, ObsID::tcC2LM );
// Test Data copied from RINEX file for PRN3, week 1638, day 153 2011
// Subframe 2 Data
unsigned long TOWWeek = 1638;
int n_TOWWeek = 13;
int s_TOWWeek = 1;
unsigned long ITOW = 53;
int n_ITOW = 8;
int s_ITOW = 1;
unsigned long Top = 378000;
int n_Top = 11;
int s_Top = 300;
unsigned long L1CHealth = 0;
int n_L1CHealth = 1;
int s_L1CHealth = 1;
long URAoe = -16;
int n_URAoe = 5;
int s_URAoe = 1;
unsigned long Toe = 388800;
int n_Toe = 11;
int s_Toe = 300;
double Ahalf = 5.15365527534E+03;
int n_Ahalf = 32;
int s_Ahalf = -19;
double deltaA = Ahalf*Ahalf - A_REF_GPS;
int n_deltaA = 26;
int s_deltaA = -9;
double Adot = 0;
int n_Adot = 25;
int s_Adot = -21;
double dn = 5.39093883996E-09;
int n_dn = 17;
int s_dn = -44;
double dndot = 0;
int n_dndot = 23;
int s_dndot = -57;
double M0 = 1.05539162795E+00;
int n_M0 = 33;
int s_M0 = -32;
double ecc = 1.42575260252E-02;
int n_ecc = 33;
int s_ecc = -34;
double w = 1.09154604931E+00;
int n_w = 33;
int s_w = -32;
double OMEGA0 = -2.16947563164E+00;
int n_OMEGA0 = 33;
int s_OMEGA0 = -32;
double i0 = 9.28692497530E-01;
int n_i0 = 33;
int s_i0 = -32;
double OMEGAdot = -8.56285667735E-09;
int n_OMEGAdot = 24;
int s_OMEGAdot = -43;
double deltaOMEGAdot = OMEGAdot - OMEGADOT_REF_GPS;
int n_deltaOMEGAdot = 17;
int s_deltaOMEGAdot = -44;
double idot = 5.52880172536E-10;
int n_idot = 15;
int s_idot = -44;
double Cis = 5.02914190292E-08;
int n_Cis = 16;
int s_Cis = -30;
double Cic = -2.30967998505E-07;
int n_Cic = 16;
int s_Cic = -30;
double Crs = 1.87812500000E+01;
int n_Crs = 24;
int s_Crs = -8;
double Crc = 2.03562500000E+02;
int n_Crc = 24;
int s_Crc = -8;
double Cus = 8.35768878460E-06;
int n_Cus = 21;
int s_Cus = -30;
double Cuc = 9.57399606705E-07;
int n_Cuc = 21;
int s_Cuc = -30;
long URAoc = 15;
int n_URAoc = 5;
int s_URAoc = 1;
unsigned long URAoc1 = 1;
int n_URAoc1 = 3;
int s_URAoc1 = 1;
unsigned long URAoc2 = 2;
int n_URAoc2 = 3;
int s_URAoc2 = 1;
double af0 = 7.23189674318E-04;
int n_af0 = 26;
int s_af0 = -35;
double af1 = 5.11590769747E-12;
int n_af1 = 20;
int s_af1 = -48;
double af2 = 0.0;
int n_af2 = 10;
int s_af2 = -60;
double Tgd = -4.65661287308E-09;
int n_Tgd = 13;
int s_Tgd = -35;
double ISCL1cp = 1E-8;
int n_ISCL1cp = 13;
int s_ISCL1cp = -35;
double ISCL1cd = -1E-8;
int n_ISCL1cd = 13;
int s_ISCL1cd = -35;
unsigned long sflag = 0;
int n_sflag = 1;
int s_sflag = 1;
unsigned long reservedBits = 0;
int n_reservedBits = 10;
int s_reservedBits = 1;
unsigned long CRC = 0;
int n_CRC = 24;
int s_CRC = 1;
ofstream outf("Logs/CNAV2PNB_Output", ios::out);
// First Test Case. Create PNB object in which to store subframe 2 data
// for the CNAV2 nav message.
PackedNavBits pnb;
/* Pack Subframe 2 data */
pnb.setSatID(satSys);
pnb.setObsID(obsID);
pnb.setTime(TransmitTime);
pnb.addUnsignedLong(TOWWeek, n_TOWWeek, s_TOWWeek);
pnb.addUnsignedLong(ITOW, n_ITOW, s_ITOW);
pnb.addUnsignedLong(Top, n_Top, s_Top);
pnb.addUnsignedLong(L1CHealth, n_L1CHealth, s_L1CHealth);
pnb.addLong(URAoe, n_URAoe, s_URAoe);
pnb.addUnsignedLong(Toe, n_Toe, s_Toe);
pnb.addSignedDouble(deltaA, n_deltaA, s_deltaA);
pnb.addSignedDouble(Adot, n_Adot, s_Adot);
pnb.addDoubleSemiCircles(dn, n_dn, s_dn);
pnb.addDoubleSemiCircles(dndot, n_dndot, s_dndot);
pnb.addDoubleSemiCircles(M0, n_M0, s_M0);
pnb.addUnsignedDouble(ecc, n_ecc, s_ecc);
pnb.addDoubleSemiCircles(w, n_w, s_w);
pnb.addDoubleSemiCircles(OMEGA0, n_OMEGA0, s_OMEGA0);
pnb.addDoubleSemiCircles(i0, n_i0, s_i0);
pnb.addDoubleSemiCircles(deltaOMEGAdot, n_deltaOMEGAdot, s_deltaOMEGAdot);
pnb.addDoubleSemiCircles(idot, n_idot, s_idot);
pnb.addSignedDouble(Cis, n_Cis, s_Cis);
pnb.addSignedDouble(Cic, n_Cic, s_Cic);
pnb.addSignedDouble(Crs, n_Crs, s_Crs);
pnb.addSignedDouble(Crc, n_Crc, s_Crc);
pnb.addSignedDouble(Cus, n_Cus, s_Cus);
pnb.addSignedDouble(Cuc, n_Cuc, s_Cuc);
pnb.addLong(URAoc, n_URAoc, s_URAoc);
pnb.addUnsignedLong(URAoc1, n_URAoc1, s_URAoc1);
pnb.addUnsignedLong(URAoc2, n_URAoc2, s_URAoc2);
pnb.addSignedDouble(af0, n_af0, s_af0);
pnb.addSignedDouble(af1, n_af1, s_af1);
pnb.addSignedDouble(af2, n_af2, s_af2);
pnb.addSignedDouble(Tgd, n_Tgd, s_Tgd);
pnb.addSignedDouble(ISCL1cp, n_ISCL1cp, s_ISCL1cp);
pnb.addSignedDouble(ISCL1cd, n_ISCL1cd, s_ISCL1cd);
pnb.addUnsignedLong(sflag, n_sflag, s_sflag);
pnb.addUnsignedLong(reservedBits, n_reservedBits, s_reservedBits);
pnb.addUnsignedLong(CRC, n_CRC, s_CRC);
outf << endl;
outf << "Time of Transmission: " << pnb.getTransmitTime() << endl;
outf << "Time of Transmission pnb: " << GPSWeekSecond(pnb.getTransmitTime()).printf("%F, %g") << endl;
// Resize the vector holding the packed nav message data
pnb.trimsize();
outf << "PNB Object Dump:" << endl;
outf << pnb << endl;
// Second test case. Create a CEC object with the packed data above, available from RINEX file.
outf << endl << "Test Case 2: Creating CEC object with data from RINEX file." << endl;
outf << "Time = " << g << endl;
CommonTime dt = TransmitTime;
short PRNIDArg = 3;
int TOI = 50; // 9 bit word from subframe 1
CNAV2EphClk cec;
cec.loadData( obsID, PRNIDArg, TOI, pnb);
Xvt xvt = cec.svXvt( dt );
double ClkCorr = cec.svClockBias( dt );
double ClkDrift = cec.svClockDrift( dt );
outf << "Position cec: " << xvt.x<< endl;
outf << "Velocity cec: " << xvt.v << endl;
outf.setf(ios::scientific, ios::floatfield);
outf.precision(11);
outf << "RelCorr cec: " << cec.svRelativity(dt) << endl;
outf << "Clock Bias cec: " << ClkCorr << endl;
outf << "Clock Drift cec: " << ClkDrift << endl;
outf << "Time of Prediction cec:" << GPSWeekSecond(cec.getTimeOfPrediction()).printf("%F, %g") << endl;
outf << endl << "CEC Object Dump:" << endl;
outf << cec << endl;
CPPUNIT_ASSERT(fileEqualTest((char*)"Logs/CNAV2PNB_Truth",(char*)"Logs/CNAV2PNB_Output"));
}
void CNAVEphemeris::loadData( const ObsID obsIDArg,
const short PRNIDArg,
const PackedNavBits message10,
const PackedNavBits message11)
throw( InvalidParameter)
{
obsID = obsIDArg;
PRNID = PRNIDArg;
satSys = "G";
// Message Type 10 data
Alert[0] = message10.asUnsignedLong(37, 1, 1);
TOWCount[0] = message10.asUnsignedLong(20, 17, 300);
TOWWeek = message10.asUnsignedLong(38, 13, 1);
L1Health = message10.asUnsignedLong(51, 1, 1);
L2Health = message10.asUnsignedLong(52, 1, 1);
L5Health = message10.asUnsignedLong(53, 1, 1);
Top = message10.asUnsignedLong(54, 11, 300);
short URAoe = message10.asLong(65, 5, 1);
double Toe = message10.asUnsignedLong(70, 11, 300);
double deltaA = message10.asSignedDouble(81, 26, -9);
double Adot = message10.asSignedDouble(107, 25, -21);
double dn = message10.asDoubleSemiCircles(132, 17, -44);
double dndot = message10.asDoubleSemiCircles(149, 23, -57);
double M0 = message10.asDoubleSemiCircles(172, 33, -32);
double ecc = message10.asUnsignedDouble(205, 33, -34);
double w = message10.asDoubleSemiCircles(238, 33, -32);
// Message Type 11 data
Alert[1] = message11.asUnsignedLong(37, 1, 1);
TOWCount[1] = message11.asUnsignedLong(20, 17, 300);
double OMEGA0 = message11.asDoubleSemiCircles(49, 33, -32);
double i0 = message11.asDoubleSemiCircles(82, 33, -32);
double deltaOMEGAdot = message11.asDoubleSemiCircles(115, 17, -44);
double idot = message11.asDoubleSemiCircles(132, 15, -44);
double Cis = message11.asSignedDouble(147, 16, -30);
double Cic = message11.asSignedDouble(163, 16, -30);
double Crs = message11.asSignedDouble(179, 24, -8);
double Crc = message11.asSignedDouble(203, 24, -8);
double Cus = message11.asSignedDouble(227, 21, -30);
double Cuc = message11.asSignedDouble(248, 21, -30);
double A = A_REF_GPS + deltaA;
double Ahalf = SQRT(A);
double OMEGAdot = OMEGADOT_REF_GPS + deltaOMEGAdot;
bool healthy = false;
if (obsIDArg.band == ObsID::cbL2 && L2Health == 0) healthy = true;
if (obsIDArg.band == ObsID::cbL5 && L5Health == 0) healthy = true;
double timeDiff = Toe - TOWCount[0];
short epochWeek = TOWWeek;
if (timeDiff < -HALFWEEK) epochWeek++;
else if (timeDiff > HALFWEEK) epochWeek--;
long beginFitSOW = ((TOWCount[0])/7200)*7200;
long endFitSOW = beginFitSOW + 10800;
short beginFitWk = TOWWeek;
short endFitWk = TOWWeek;
CommonTime beginFit = GPSWeekSecond(beginFitWk, beginFitSOW, TimeSystem::GPS);
if (endFitSOW >= FULLWEEK)
{
endFitSOW -= FULLWEEK;
endFitWk++;
}
CommonTime endFit = GPSWeekSecond(endFitWk, endFitSOW, TimeSystem::GPS);
CommonTime ToeCT = GPSWeekSecond(epochWeek, Toe, TimeSystem::GPS);
orbit.loadData(satSys, obsID, PRNID, beginFit, endFit, ToeCT,
URAoe, healthy, Cuc, Cus, Crc, Crs, Cic, Cis, M0, dn,
dndot, ecc, A, Ahalf, Adot, OMEGA0, i0, w, OMEGAdot, idot);
dataLoaded = true;
}
void OrbSysGpsC_30::loadData(const PackedNavBits& msg)
throw(InvalidParameter)
{
setUID(msg);
if (UID!=30)
{
char errStr[80];
std::string msgString("Expected GPS CNAV MT 30. Found unique ID ");
msgString += StringUtils::asString(UID);
InvalidParameter exc(msgString);
GPSTK_THROW(exc);
}
obsID = msg.getobsID();
satID = msg.getsatSys();
beginValid = msg.getTransmitTime();
// Message Type 30 data
unsigned long testAvail = 4096; // Pattern in message of 0x1000
// if data quantity not available
unsigned long avail = msg.asUnsignedLong(127,13,1);
avail_Tgd = false;
if (avail!=testAvail)
{
avail_Tgd = true;
Tgd = msg.asSignedDouble(127, 13, -35);
}
avail = msg.asUnsignedLong(140,13,1);
avail_L1CA = false;
if (avail!=testAvail)
{
avail_L1CA = true;
ISC_L1CA = msg.asSignedDouble(140, 13, -35);
}
avail = msg.asUnsignedLong(153,13,1);
avail_L2C = false;
if (avail!=testAvail)
{
avail_L2C = true;
ISC_L2C = msg.asSignedDouble(153, 13, -35);
}
avail = msg.asUnsignedLong(166,13,1);
avail_L5I5 = false;
if (avail!=testAvail)
{
avail_L5I5 = true;
ISC_L5I5 = msg.asSignedDouble(166, 13, -35);
}
avail = msg.asUnsignedLong(179,13,1);
avail_L5Q5 = false;
if (avail!=testAvail)
{
avail_L5Q5 = true;
ISC_L5Q5 = msg.asSignedDouble(179, 13, -35);
}
alpha[0] = msg.asSignedDouble(192, 8, -30);
alpha[1] = msg.asSignedDouble(200, 8, -27);
alpha[2] = msg.asSignedDouble(208, 8, -24);
alpha[3] = msg.asSignedDouble(216, 8, -24);
beta[0] = msg.asSignedDouble(224, 8, 11);
beta[1] = msg.asSignedDouble(232, 8, 14);
beta[2] = msg.asSignedDouble(240, 8, 16);
beta[3] = msg.asSignedDouble(248, 8, 16);
// Need to convert from sec/semi-circle to sec/rad
double conversion = 1.0 / PI;
alpha[1] *= conversion;
beta[1] *= conversion;
alpha[2] *= conversion * conversion;
beta[2] *= conversion * conversion;
alpha[3] *= conversion * conversion * conversion;
beta[3] *= conversion * conversion * conversion;
dataLoadedFlag = true;
} // end of loadData()
// These tests are NOT real data, but are designed to test
// edge cases and limits.
unsigned PackedNavBits_T ::
abstractTest()
{
// Test Unsigned Integers
TUDEF("PackedNavBits", "addUnsigned");
unsigned long u_i1 = 32767;
int u_n1 = 16;
int u_s1 = 1;
unsigned long u_i2 = 1;
int u_n2 = 8;
int u_s2 = 1;
unsigned long u_i3 = 255;
int u_n3 = 8;
int u_s3 = 1;
unsigned long u_i4 = 604500;
int u_n4 = 11;
int u_s4 = 300;
// Pack the data
PackedNavBits pnb;
pnb.addUnsignedLong(u_i1, u_n1, u_s1);
pnb.addUnsignedLong(u_i2, u_n2, u_s2);
pnb.addUnsignedLong(u_i3, u_n3, u_s3);
pnb.addUnsignedLong(u_i4, u_n4, u_s4);
// Unpack the data and see that the round-trip worked.
int startbit = 0;
unsigned long ultest = pnb.asUnsignedLong(startbit, u_n1, u_s1);
TUASSERTE(unsigned long, ultest, u_i1);
startbit += u_n1;
ultest = pnb.asUnsignedLong(startbit, u_n2, u_s2);
TUASSERTE(unsigned long, ultest, u_i2);
startbit += u_n2;
ultest = pnb.asUnsignedLong(startbit, u_n3, u_s3);
TUASSERTE(unsigned long, ultest, u_i3);
startbit += u_n3;
ultest = pnb.asUnsignedLong(startbit, u_n4, u_s4);
TUASSERTE(unsigned long, ultest, u_i4);
startbit += u_n4;
// Test Signed Integers
TUCSM("addSigned");
long s_i1 = 15;
int s_n1 = 5;
int s_s1 = 1;
long s_i2 = -16;
int s_n2 = 5;
int s_s2 = 1;
long s_i3 = -1;
int s_n3 = 5;
int s_s3 = 1;
long s_i4 = 0;
int s_n4 = 6;
int s_s4 = 1;
long s_i5 = 4194304;
int s_n5 = 24;
int s_s5 = 1;
pnb.addLong(s_i1, s_n1, s_s1);
pnb.addLong(s_i2, s_n2, s_s2);
pnb.addLong(s_i3, s_n3, s_s3);
pnb.addLong(s_i4, s_n4, s_s4);
pnb.addLong(s_i5, s_n5, s_s5);
long ltest;
ltest = pnb.asLong(startbit, s_n1, s_s1);
TUASSERTE(long, ltest, s_i1);
startbit += s_n1;
ltest = pnb.asLong(startbit, s_n2, s_s2);
TUASSERTE(long, ltest, s_i2);
startbit += s_n2;
ltest = pnb.asLong(startbit, s_n3, s_s3);
TUASSERTE(long, ltest, s_i3);
startbit += s_n3;
ltest = pnb.asLong(startbit, s_n4, s_s4);
TUASSERTE(long, ltest, s_i4);
startbit += s_n4;
ltest = pnb.asLong(startbit, s_n5, s_s5);
TUASSERTE(long, ltest, s_i5);
startbit += s_n5;
// Test Signed Doubles
TUCSM("addDouble");
double d_i1 = 0.490005493;
int d_n1 = 16;
int d_s1 = -16;
double d_e1 = pow(2.0,d_s1); // value of lsb
double d_i2 = -0.5;
int d_n2 = 16;
int d_s2 = -16;
double d_e2 = pow(2.0,d_s2); // value of lsb
double d_i3 = 0;
int d_n3 = 16;
int d_s3 = -16;
double d_e3 = pow(2.0,d_s3); // value of lsb
// Test Unsigned Doubles
double d_i4 = 32000.0;
int d_n4 = 16;
int d_s4 = 0;
double d_e4 = pow(2.0,d_s4); // value of lsb
pnb.addSignedDouble(d_i1, d_n1, d_s1);
pnb.addSignedDouble(d_i2, d_n2, d_s2);
pnb.addSignedDouble(d_i3, d_n3, d_s3);
pnb.addUnsignedDouble(d_i4, d_n4, d_s4);
double dtest;
dtest = pnb.asSignedDouble(startbit, d_n1, d_s1);
TUASSERTFEPS(dtest, d_i1, d_e1);
startbit += d_n1;
dtest = pnb.asSignedDouble(startbit, d_n2, d_s2);
TUASSERTFEPS(dtest, d_i2, d_e2);
startbit += d_n2;
dtest = pnb.asSignedDouble(startbit, d_n3, d_s3);
TUASSERTFEPS(dtest, d_i3, d_e3);
startbit += d_n3;
dtest = pnb.asUnsignedDouble(startbit, d_n4, d_s4);
TUASSERTFEPS(dtest, d_i4, d_e4);
startbit += d_n4;
// Test Semi-Circles
TUCSM("addSemiCircles");
double sd_i1 = PI-2*pow(2.0,-31);
int sd_n1 = 32;
int sd_s1 = -31;
double sd_e1 = pow(2.0,sd_s1) * 3.0; // value of lsb in semi-circles
double sd_i2 = -PI;
int sd_n2 = 32;
int sd_s2 = -31;
double sd_e2 = pow(2.0,sd_s1) * 3.0; // value of lsb
pnb.addDoubleSemiCircles(sd_i1, sd_n1, sd_s1);
pnb.addDoubleSemiCircles(sd_i2, sd_n2, sd_s2);
dtest = pnb.asDoubleSemiCircles(startbit, sd_n1, sd_s1);
TUASSERTFEPS(dtest, sd_i1, sd_e1);
startbit += sd_n1;
dtest = pnb.asDoubleSemiCircles(startbit, sd_n2, sd_s2);
TUASSERTFEPS(dtest, sd_i2, sd_e2);
return testFramework.countFails();
}
void CNavISC::loadData(const PackedNavBits& message30)
throw( InvalidParameter )
{
// First, verify the correct message type is being passed in.
long msgType = message30.asUnsignedLong(14,6,1);
if(msgType!=30)
{
char errStr[80];
sprintf(errStr,"Expected CNAV MsgType 30. Found MsgType %ld",msgType);
std::string tstr(errStr);
InvalidParameter exc(tstr);
GPSTK_THROW(exc);
}
obsID = message30.getobsID();
satID = message30.getsatSys();
ctXmit = message30.getTransmitTime();
ctEpoch = ctXmit; // For ISC, no explicit epoch time.
// Message Type 30 data
unsigned long testAvail = 4096; // Pattern in message of 0x1000
// if data quantity not available
unsigned long avail = message30.asUnsignedLong(127,13,1);
avail_Tgd = false;
if (avail!=testAvail)
{
avail_Tgd = true;
Tgd = message30.asSignedDouble(127, 13, -35);
}
avail = message30.asUnsignedLong(140,13,1);
avail_L1CA = false;
if (avail!=testAvail)
{
avail_L1CA = true;
ISC_L1CA = message30.asSignedDouble(140, 13, -35);
}
avail = message30.asUnsignedLong(153,13,1);
avail_L2C = false;
if (avail!=testAvail)
{
avail_L2C = true;
ISC_L2C = message30.asSignedDouble(153, 13, -35);
}
avail = message30.asUnsignedLong(166,13,1);
avail_L5I5 = false;
if (avail!=testAvail)
{
avail_L5I5 = true;
ISC_L5I5 = message30.asSignedDouble(166, 13, -35);
}
avail = message30.asUnsignedLong(179,13,1);
avail_L5Q5 = false;
if (avail!=testAvail)
{
avail_L5Q5 = true;
ISC_L5Q5 = message30.asSignedDouble(179, 13, -35);
}
alpha[0] = message30.asSignedDouble(192, 8, -30);
alpha[1] = message30.asSignedDouble(200, 8, -27);
alpha[2] = message30.asSignedDouble(208, 8, -24);
alpha[3] = message30.asSignedDouble(216, 8, -24);
beta[0] = message30.asSignedDouble(224, 8, 11);
beta[1] = message30.asSignedDouble(232, 8, 14);
beta[2] = message30.asSignedDouble(240, 8, 16);
beta[3] = message30.asSignedDouble(248, 8, 16);
// Need to convert from sec/semi-circle to sec/rad
double conversion = 1.0 / PI;
alpha[1] *= conversion;
beta[1] *= conversion;
alpha[2] *= conversion * conversion;
beta[2] *= conversion * conversion;
alpha[3] *= conversion * conversion * conversion;
beta[3] *= conversion * conversion * conversion;
dataLoadedFlag = true;
} // end of loadData()
