void CNavUTC::loadData(const PackedNavBits& message33)
throw(InvalidParameter)
{
// First, verify the correct message type is being passed in.
long msgType = message33.asUnsignedLong(14,6,1);
if(msgType!=33)
{
char errStr[80];
sprintf(errStr,"Expected CNAV MsgType 33. Found MsgType %ld",msgType);
std::string tstr(errStr);
InvalidParameter exc(tstr);
GPSTK_THROW(exc);
}
obsID = message33.getobsID();
satID = message33.getsatSys();
ctXmit = message33.getTransmitTime();
A0 = message33.asSignedDouble(127,16,-35);
A1 = message33.asSignedDouble(143,13,-51);
A2 = message33.asSignedDouble(156, 7,-68);
deltaTls = message33.asLong(163, 8, 1);
Tot = message33.asUnsignedLong(171,16,16);
WNot = message33.asUnsignedLong(187,13, 1);
WNlsf = message33.asUnsignedLong(200,13, 1);
DN = message33.asUnsignedLong(213, 4, 1);
deltaTlsf = message33.asLong(217, 8, 1);
ctEpoch = GPSWeekSecond(WNot, Tot, TimeSystem::GPS);
dataLoadedFlag = true;
} // end of loadData()
void CNavGGTO::loadData(const PackedNavBits& message35)
throw(InvalidParameter)
{
// First, verify the correct message type is being passed in.
long msgType = message35.asUnsignedLong(14,6,1);
if(msgType!=35)
{
char errStr[80];
sprintf(errStr,"Expected CNAV MsgType 35. Found MsgType %ld",msgType);
std::string tstr(errStr);
InvalidParameter exc(tstr);
GPSTK_THROW(exc);
}
obsID = message35.getobsID();
satID = message35.getsatSys();
ctXmit = message35.getTransmitTime();
TGGTO = message35.asLong(127,16,16);
WNGGTO = message35.asLong(143,13, 1);
GNSS_ID = message35.asLong(156, 3, 1);
A0GGTO = message35.asSignedDouble(158,16,-35);
A1GGTO = message35.asSignedDouble(175,13,-51);
A2GGTO = message35.asSignedDouble(188, 7,-68);
if (GNSS_ID>0)
ctEpoch = GPSWeekSecond(WNGGTO, TGGTO, TimeSystem::GPS);
dataLoadedFlag = true;
} // end of loadData()
void CNavEOP::loadData(const PackedNavBits& message32)
throw(InvalidParameter)
{
// First, verify the correct message type is being passed in.
long msgType = message32.asUnsignedLong(14,6,1);
if(msgType!=32)
{
char errStr[80];
sprintf(errStr,"Expected CNAV MsgType 32. Found MsgType %ld",msgType);
std::string tstr(errStr);
InvalidParameter exc(tstr);
GPSTK_THROW(exc);
}
obsID = message32.getobsID();
satID = message32.getsatSys();
ctXmit = message32.getTransmitTime();
Teop = message32.asUnsignedLong(127,16, 16);
PM_X = message32.asSignedDouble(143,21,-20);
PM_X_dot = message32.asSignedDouble(164,15,-21);
PM_Y = message32.asSignedDouble(179,21,-20);
PM_Y_dot = message32.asSignedDouble(200,15,-21);
deltaUT1 = message32.asSignedDouble(215,31,-24);
deltaUT1_dot = message32.asSignedDouble(246,19,-25);
// The message does not contain a week counter.
// We'll assume the Teop is to be within 1/2 week
// of the transmit time.
long xmitSOW = (static_cast<GPSWeekSecond>(ctXmit)).sow;
short xmitWeek = (static_cast<GPSWeekSecond>(ctXmit)).week;
double timeDiff = Teop - xmitSOW;
short epochWeek = xmitWeek;
if (timeDiff < -HALFWEEK) epochWeek++;
else if (timeDiff > HALFWEEK) epochWeek--;
ctEpoch = GPSWeekSecond(epochWeek, Teop, TimeSystem::GPS);
dataLoadedFlag = true;
} // end of loadData()
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 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()