00001 #pragma ident "$Id: CNAVEphemeris.cpp 3140 2012-06-18 15:03:02Z susancummins $"
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00043 #include <iomanip>
00044 #include <cmath>
00045 #include <iostream>
00046
00047 #include "StringUtils.hpp"
00048 #include "GNSSconstants.hpp"
00049 #include "MathBase.hpp"
00050 #include "GPSEllipsoid.hpp"
00051 #include "CNAVEphemeris.hpp"
00052 #include "GPS_URA.hpp"
00053
00054 namespace gpstk
00055 {
00056 using namespace std;
00057
00058 CNAVEphemeris::CNAVEphemeris()
00059 throw()
00060 {
00061 dataLoaded = false;
00062
00063 satSys = "";
00064
00065 PRNID = TOWWeek = L1Health = L2Health = L5Health =
00066 TOWCount[0] = TOWCount[1] = Alert[0] = Alert[1] = Top = 0;
00067
00068 }
00069
00070 void CNAVEphemeris::loadData( const std::string satSysArg, const ObsID obsIDArg,
00071 const short PRNIDArg, const short AlertMsg10Arg,
00072 const long TOWMsg10Arg, const short AlertMsg11Arg,
00073 const long TOWMsg11Arg, const short TOWWeekArg,
00074 const long TopArg, const short URAoeArg,
00075 const short L1HealthArg, const short L2HealthArg,
00076 const short L5HealthArg, const double ToeArg,
00077 const double accuracyArg, const double CucArg,
00078 const double CusArg, const double CrcArg,
00079 const double CrsArg, const double CicArg,
00080 const double CisArg, const double M0Arg,
00081 const double dnArg, const double dndotArg,
00082 const double eccArg, const double deltaAArg,
00083 const double AdotArg, const double OMEGA0Arg,
00084 const double i0Arg, const double wArg,
00085 const double deltaOMEGAdotArg, const double idotArg )
00086 {
00087 satSys = satSysArg;
00088 obsID = obsIDArg;
00089 PRNID = PRNIDArg;
00090 Alert[0] = AlertMsg10Arg;
00091 Alert[1] = AlertMsg11Arg;
00092 TOWCount[0] = TOWMsg10Arg;
00093 TOWCount[1] = TOWMsg11Arg;
00094 TOWWeek = TOWWeekArg;
00095 L1Health = L1HealthArg;
00096 L2Health = L2HealthArg;
00097 L5Health = L5HealthArg;
00098 Top = TopArg;
00099 short URAoe = URAoeArg;
00100 double deltaA = deltaAArg;
00101 bool healthy = false;
00102 if (obsIDArg.band == ObsID::cbL2 && L2Health == 0) healthy = true;
00103 if (obsIDArg.band == ObsID::cbL5 && L5Health == 0) healthy = true;
00104
00105 double A = A_REF_GPS + deltaA;
00106 double Ahalf = SQRT(A);
00107 double deltaOMEGAdot = deltaOMEGAdotArg;
00108 double OMEGAdot = OMEGADOT_REF_GPS + deltaOMEGAdot;
00109 satSys = "G";
00110 double timeDiff = ToeArg - TOWCount[0];
00111 short epochWeek = TOWWeek;
00112 if (timeDiff < -HALFWEEK) epochWeek++;
00113 else if (timeDiff > HALFWEEK) epochWeek--;
00114
00115
00116
00117 ObsID obsID(ObsID::otNavMsg, obsIDArg.band, obsIDArg.code);
00118
00119 long beginFitSOW = ((TOWCount[0])/7200)*7200;
00120 long endFitSOW = beginFitSOW + 10800;
00121 short beginFitWk = TOWWeek;
00122 short endFitWk = TOWWeek;
00123
00124 CommonTime beginFit = GPSWeekSecond(beginFitWk, beginFitSOW, TimeSystem::GPS);
00125
00126 if (endFitSOW >= FULLWEEK)
00127 {
00128 endFitSOW -= FULLWEEK;
00129 endFitWk++;
00130 }
00131 CommonTime endFit = GPSWeekSecond(endFitWk, endFitSOW, TimeSystem::GPS);
00132
00133 CommonTime ToeCT = GPSWeekSecond(epochWeek, ToeArg, TimeSystem::GPS);
00134
00135 orbit.loadData(satSys, obsID, PRNID, beginFit, endFit, ToeCT,
00136 URAoe, healthy, CucArg, CusArg, CrcArg, CrsArg, CicArg,
00137 CisArg, M0Arg, dnArg, dndotArg, eccArg, A, Ahalf, AdotArg,
00138 OMEGA0Arg, i0Arg, wArg, OMEGAdot, idotArg);
00139 dataLoaded = true;
00140 }
00141
00142 void CNAVEphemeris::loadData( const ObsID obsIDArg,
00143 const short PRNIDArg,
00144 const PackedNavBits message10,
00145 const PackedNavBits message11)
00146
00147 throw( InvalidParameter)
00148 {
00149 obsID = obsIDArg;
00150 PRNID = PRNIDArg;
00151 satSys = "G";
00152
00153
00154 Alert[0] = message10.asUnsignedLong(37, 1, 1);
00155 TOWCount[0] = message10.asUnsignedLong(20, 17, 300);
00156 TOWWeek = message10.asUnsignedLong(38, 13, 1);
00157 L1Health = message10.asUnsignedLong(51, 1, 1);
00158 L2Health = message10.asUnsignedLong(52, 1, 1);
00159 L5Health = message10.asUnsignedLong(53, 1, 1);
00160 Top = message10.asUnsignedLong(54, 11, 300);
00161 short URAoe = message10.asLong(65, 5, 1);
00162 double Toe = message10.asUnsignedLong(70, 11, 300);
00163 double deltaA = message10.asSignedDouble(81, 26, -9);
00164 double Adot = message10.asSignedDouble(107, 25, -21);
00165 double dn = message10.asDoubleSemiCircles(132, 17, -44);
00166 double dndot = message10.asDoubleSemiCircles(149, 23, -57);
00167 double M0 = message10.asDoubleSemiCircles(172, 33, -32);
00168 double ecc = message10.asUnsignedDouble(205, 33, -34);
00169 double w = message10.asDoubleSemiCircles(238, 33, -32);
00170
00171
00172 Alert[1] = message11.asUnsignedLong(37, 1, 1);
00173 TOWCount[1] = message11.asUnsignedLong(20, 17, 300);
00174 double OMEGA0 = message11.asDoubleSemiCircles(49, 33, -32);
00175 double i0 = message11.asDoubleSemiCircles(82, 33, -32);
00176 double deltaOMEGAdot = message11.asDoubleSemiCircles(115, 17, -44);
00177 double idot = message11.asDoubleSemiCircles(132, 15, -44);
00178 double Cis = message11.asSignedDouble(147, 16, -30);
00179 double Cic = message11.asSignedDouble(163, 16, -30);
00180 double Crs = message11.asSignedDouble(179, 24, -8);
00181 double Crc = message11.asSignedDouble(203, 24, -8);
00182 double Cus = message11.asSignedDouble(227, 21, -30);
00183 double Cuc = message11.asSignedDouble(248, 21, -30);
00184
00185 double A = A_REF_GPS + deltaA;
00186 double Ahalf = SQRT(A);
00187 double OMEGAdot = OMEGADOT_REF_GPS + deltaOMEGAdot;
00188
00189 bool healthy = false;
00190 if (obsIDArg.band == ObsID::cbL2 && L2Health == 0) healthy = true;
00191 if (obsIDArg.band == ObsID::cbL5 && L5Health == 0) healthy = true;
00192
00193 double timeDiff = Toe - TOWCount[0];
00194 short epochWeek = TOWWeek;
00195 if (timeDiff < -HALFWEEK) epochWeek++;
00196 else if (timeDiff > HALFWEEK) epochWeek--;
00197
00198 long beginFitSOW = ((TOWCount[0])/7200)*7200;
00199 long endFitSOW = beginFitSOW + 10800;
00200 short beginFitWk = TOWWeek;
00201 short endFitWk = TOWWeek;
00202
00203 CommonTime beginFit = GPSWeekSecond(beginFitWk, beginFitSOW, TimeSystem::GPS);
00204
00205 if (endFitSOW >= FULLWEEK)
00206 {
00207 endFitSOW -= FULLWEEK;
00208 endFitWk++;
00209 }
00210 CommonTime endFit = GPSWeekSecond(endFitWk, endFitSOW, TimeSystem::GPS);
00211
00212 CommonTime ToeCT = GPSWeekSecond(epochWeek, Toe, TimeSystem::GPS);
00213
00214 orbit.loadData(satSys, obsID, PRNID, beginFit, endFit, ToeCT,
00215 URAoe, healthy, Cuc, Cus, Crc, Crs, Cic, Cis, M0, dn,
00216 dndot, ecc, A, Ahalf, Adot, OMEGA0, i0, w, OMEGAdot, idot);
00217 dataLoaded = true;
00218 }
00219
00220 bool CNAVEphemeris::hasData()
00221 {
00222 return(dataLoaded);
00223 }
00224
00225 Xv CNAVEphemeris::svXv(const CommonTime& t) const
00226 throw( InvalidRequest )
00227 {
00228 if(!dataLoaded)
00229 {
00230 InvalidRequest exc("svXvt: Required data not stored.");
00231 GPSTK_THROW(exc);
00232 }
00233 Xv sv;
00234 Xv xv = orbit.svXv(t);
00235 sv.x = xv.x;
00236 sv.v = xv.v;
00237 return sv;
00238 }
00239
00240 double CNAVEphemeris::svRelativity(const CommonTime& t) const
00241 throw( InvalidRequest )
00242 {
00243 if(!dataLoaded)
00244 {
00245 InvalidRequest exc("svRelativity(): Required data not stored.");
00246 GPSTK_THROW(exc);
00247 }
00248 return orbit.svRelativity(t);
00249 }
00250
00251 CommonTime CNAVEphemeris::getTransmitTime() const
00252 throw(InvalidRequest)
00253 {
00254 if(!dataLoaded)
00255 {
00256 InvalidRequest exc("getTransmitTime: Required data not stored.");
00257 GPSTK_THROW(exc);
00258 }
00259 GPSWeekSecond gws(TOWWeek, TOWCount[0], TimeSystem::GPS);
00260 return (gws.convertToCommonTime());
00261 }
00262
00263 CommonTime CNAVEphemeris::getTimeOfPrediction() const
00264 throw( InvalidRequest )
00265 {
00266 if(!dataLoaded)
00267 {
00268 InvalidRequest exc("getTimeOfPrediction(): Required data not stored.");
00269 GPSTK_THROW(exc);
00270 }
00271 short week(TOWWeek);
00272 if( Top - TOWCount[0] < -HALFWEEK)
00273 week++;
00274 else if ( Top - TOWCount[0] > HALFWEEK)
00275 week--;
00276 CommonTime toReturn;
00277 toReturn = GPSWeekSecond(week, Top, TimeSystem::GPS);
00278 return toReturn;
00279 }
00280
00281 short CNAVEphemeris::getPRNID() const
00282 throw( InvalidRequest )
00283 {
00284 if(!dataLoaded)
00285 {
00286 InvalidRequest exc("getPRNID(): Required data not stored.");
00287 GPSTK_THROW(exc);
00288 }
00289 return PRNID;
00290 }
00291
00292 short CNAVEphemeris::getAlert(short messageNum) const
00293 throw( InvalidRequest, Exception )
00294 {
00295 if (!dataLoaded)
00296 {
00297 InvalidRequest exc("getAlert(): messageNum not stored.");
00298 GPSTK_THROW(exc);
00299 }
00300 if (messageNum == 10) return Alert[0];
00301 if (messageNum == 11) return Alert[1];
00302
00303 Exception e("getAlert(): unrecognized value for messageNum: " + messageNum);
00304 GPSTK_THROW(e);
00305 }
00306
00307 double CNAVEphemeris::getAccuracy() const
00308 throw(InvalidRequest)
00309 {
00310 if (!dataLoaded)
00311 {
00312 InvalidRequest exc("Required data not stored.");
00313 GPSTK_THROW(exc);
00314 }
00315 return orbit.getAccuracy();
00316 }
00317
00318 short CNAVEphemeris::getURAoe() const
00319 throw( InvalidRequest )
00320 {
00321 if(!dataLoaded)
00322 {
00323 InvalidRequest exc("getURAoe(): Required data not stored.");
00324 GPSTK_THROW(exc);
00325 }
00326 return orbit.getURAoe();
00327 }
00328
00329 short CNAVEphemeris::getHealth(const ObsID::CarrierBand cb) const
00330 throw( InvalidRequest )
00331 {
00332 if(!dataLoaded)
00333 {
00334 InvalidRequest exc("getHealth(): Required data not stored.");
00335 GPSTK_THROW(exc);
00336 }
00337 short retVal;
00338 switch (cb)
00339 {
00340 case ObsID::cbL1:
00341 retVal = L1Health;
00342 break;
00343 case ObsID::cbL2:
00344 retVal = L2Health;
00345 break;
00346 case ObsID::cbL5:
00347 retVal = L5Health;
00348 break;
00349 default:
00350 InvalidRequest exc("getHealth(): Invalid carrier selection");
00351 GPSTK_THROW(exc);
00352 }
00353 return retVal;
00354 }
00355
00356 long CNAVEphemeris::getTop() const
00357 throw(InvalidRequest)
00358 {
00359 if (!dataLoaded)
00360 {
00361 InvalidRequest exc("Required data not stored.");
00362 GPSTK_THROW(exc);
00363 }
00364 return Top;
00365 }
00366
00367 BrcKeplerOrbit CNAVEphemeris::getOrbit() const
00368 throw(InvalidRequest )
00369 {
00370 if(!orbit.hasData())
00371 {
00372 InvalidRequest exc("getOrbit(): Required Orbit data not stored.");
00373 GPSTK_THROW(exc);
00374 }
00375 return (orbit);
00376 }
00377
00378 static void timeDisplay( ostream & os, const CommonTime& t )
00379 {
00380
00381 GPSWeekSecond dummyTime;
00382 dummyTime = GPSWeekSecond(t);
00383 os << dec;
00384 os << setw(4) << dummyTime.week << "(";
00385 os << setw(4) << (dummyTime.week & 0x03FF) << ") ";
00386 os << setw(6) << setfill(' ') << dummyTime.sow << " ";
00387
00388 switch (dummyTime.getDayOfWeek())
00389 {
00390 case 0: os << "Sun-0"; break;
00391 case 1: os << "Mon-1"; break;
00392 case 2: os << "Tue-2"; break;
00393 case 3: os << "Wed-3"; break;
00394 case 4: os << "Thu-4"; break;
00395 case 5: os << "Fri-5"; break;
00396 case 6: os << "Sat-6"; break;
00397 default: break;
00398 }
00399 os << " " << (static_cast<YDSTime>(t)).printf("%3j %5.0s ")
00400 << (static_cast<CivilTime>(t)).printf("%02m/%02d/%04Y %02H:%02M:%02S");
00401 }
00402
00403
00404 static void shortcut(ostream & os, const long HOW )
00405 {
00406 short DOW, hour, min, sec;
00407 long SOD, SOW;
00408 short SOH;
00409
00410 SOW = static_cast<long>( HOW );
00411 DOW = static_cast<short>( SOW / SEC_PER_DAY );
00412 SOD = SOW - static_cast<long>( DOW * SEC_PER_DAY );
00413 hour = static_cast<short>( SOD/3600 );
00414
00415 SOH = static_cast<short>( SOD - (hour*3600) );
00416 min = SOH/60;
00417
00418 sec = SOH - min * 60;
00419 switch (DOW)
00420 {
00421 case 0: os << "Sun-0"; break;
00422 case 1: os << "Mon-1"; break;
00423 case 2: os << "Tue-2"; break;
00424 case 3: os << "Wed-3"; break;
00425 case 4: os << "Thu-4"; break;
00426 case 5: os << "Fri-5"; break;
00427 case 6: os << "Sat-6"; break;
00428 default: break;
00429 }
00430
00431 os << ":" << setfill('0')
00432 << setw(2) << hour
00433 << ":" << setw(2) << min
00434 << ":" << setw(2) << sec
00435 << setfill(' ');
00436 }
00437
00438 void CNAVEphemeris :: dump(ostream& s) const
00439 throw()
00440 {
00441 s.setf(ios::fixed, ios::floatfield);
00442 s.setf(ios::right, ios::adjustfield);
00443 s.setf(ios::uppercase);
00444 s.precision(0);
00445 s.fill(' ');
00446
00447 s << "****************************************************************"
00448 << "************" << endl
00449 << "CNAV Message Types 10 and 11" << endl
00450 << endl
00451 << "PRN : " << setw(2) << PRNID << " "
00452 << "System : " << satSys << " "
00453 << "Carrier: " << ObsID::cbDesc[obsID.band] << " "
00454 << "Code: " << ObsID::tcDesc[obsID.code] << endl << endl;
00455
00456
00457 s << " Week SOW DOW UTD SOD"
00458 << " MM/DD/YYYY HH:MM:SS\n";
00459 s << "Transmit Time: ";
00460
00461 timeDisplay(s, getTransmitTime());
00462 s << endl;
00463 s << "Time of Predict:";
00464 timeDisplay(s, getTimeOfPrediction());
00465 s << endl;
00466
00467 s << endl
00468 << " ACCURACY PARAMETERS"
00469 << endl
00470 << endl
00471 << "URAoe index: " << setw(4) << orbit.getURAoe() << endl;
00472
00473 s.setf(ios::scientific, ios::floatfield);
00474 s.precision(11);
00475
00476 s << endl
00477 << " SIGNAL PARAMETERS"
00478 << endl
00479 << endl
00480 << "L1 Health bit: " << setw(2) << L1Health << endl
00481 << "L2 Health bit: " << setw(2) << L2Health << endl
00482 << "L5 Health bit: " << setw(2) << L5Health << endl
00483 << setfill(' ') << endl;
00484
00485 s << endl
00486 << " ORBIT PARAMETERS"
00487 << endl
00488 << endl
00489 << "Semi-major axis: " << setw(18) << orbit.getAhalf() << " m**.5" << endl
00490 << "Motion correction: " << setw(18) << orbit.getDn() << " rad/sec"
00491 << endl
00492 << "Eccentricity: " << setw(18) << orbit.getEcc() << endl
00493 << "Arg of perigee: " << setw(18) << orbit.getW() << " rad" << endl
00494 << "Mean anomaly at epoch: " << setw(18) << orbit.getM0() << " rad" << endl
00495 << "Right ascension: " << setw(18) << orbit.getOmega0() << " rad "
00496 << setw(18) << orbit.getOmegaDot() << " rad/sec" << endl
00497 << "Inclination: " << setw(18) << orbit.getI0() << " rad "
00498 << setw(18) << orbit.getIDot() << " rad/sec" << endl;
00499
00500 s << endl
00501 << " HARMONIC CORRECTIONS"
00502 << endl
00503 << endl
00504 << "Radial Sine: " << setw(18) << orbit.getCrs() << " m Cosine: "
00505 << setw(18) << orbit.getCrc() << " m" << endl
00506 << "Inclination Sine: " << setw(18) << orbit.getCis() << " rad Cosine: "
00507 << setw(18) << orbit.getCic() << " rad" << endl
00508 << "In-track Sine: " << setw(18) << orbit.getCus() << " rad Cosine: "
00509 << setw(18) << orbit.getCuc() << " rad" << endl;
00510
00511 s << "****************************************************************"
00512 << "************" << endl;
00513
00514 }
00515
00516 ostream& operator<<(ostream& s, const CNAVEphemeris& eph)
00517 {
00518 eph.dump(s);
00519 return s;
00520 }
00521
00522 }
