1 //============================================================================== 2 // 3 // This file is part of GPSTk, the GPS Toolkit. 4 // 5 // The GPSTk is free software; you can redistribute it and/or modify 6 // it under the terms of the GNU Lesser General Public License as published 7 // by the Free Software Foundation; either version 3.0 of the License, or 8 // any later version. 9 // 10 // The GPSTk is distributed in the hope that it will be useful, 11 // but WITHOUT ANY WARRANTY; without even the implied warranty of 12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 // GNU Lesser General Public License for more details. 14 // 15 // You should have received a copy of the GNU Lesser General Public 16 // License along with GPSTk; if not, write to the Free Software Foundation, 17 // Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA 18 // 19 // This software was developed by Applied Research Laboratories at the 20 // University of Texas at Austin. 21 // Copyright 2004-2020, The Board of Regents of The University of Texas System 22 // 23 //============================================================================== 24 25 //============================================================================== 26 // 27 // This software was developed by Applied Research Laboratories at the 28 // University of Texas at Austin, under contract to an agency or agencies 29 // within the U.S. Department of Defense. The U.S. Government retains all 30 // rights to use, duplicate, distribute, disclose, or release this software. 31 // 32 // Pursuant to DoD Directive 523024 33 // 34 // DISTRIBUTION STATEMENT A: This software has been approved for public 35 // release, distribution is unlimited. 36 // 37 //============================================================================== 38 /** 39 * @file OrbElem.cpp 40 * OrbElem data encapsulated in engineering terms 41 */ 42 43 #include "OrbElem.hpp" 44 #include "StringUtils.hpp" 45 #include "CGCS2000Ellipsoid.hpp" 46 #include "EllipsoidModel.hpp" 47 #include "GPSEllipsoid.hpp" 48 #include "YDSTime.hpp" 49 #include "CivilTime.hpp" 50 #include "TimeSystem.hpp" 51 #include "TimeString.hpp" 52 #include "MathBase.hpp" 53 54 55 namespace gpstk 56 { 57 using namespace std; 58 using namespace gpstk; OrbElem()59 OrbElem::OrbElem() 60 : Cuc(0.0), Cus(0.0), Crc(0.0), Crs(0.0), Cic(0.0), Cis(0.0), M0(0.0), 61 dn(0.0), dndot(0.0), ecc(0.0), A(0.0), Adot(0.0), OMEGA0(0.0), 62 i0(0.0), w(0.0), OMEGAdot(0.0), idot(0.0), af0(0.0), af1(0.0), 63 af2(0.0), ctToc(CommonTime::BEGINNING_OF_TIME) 64 { 65 ctToc.setTimeSystem(TimeSystem::GPS); 66 beginValid.setTimeSystem(TimeSystem::GPS); 67 endValid.setTimeSystem(TimeSystem::GPS); 68 } 69 isValid(const CommonTime & ct) const70 bool OrbElem::isValid(const CommonTime& ct) const 71 { 72 if (!dataLoaded()) 73 { 74 InvalidRequest exc("Required data not stored."); 75 GPSTK_THROW(exc); 76 } 77 if (ct >= beginValid && ct <= endValid) return(true); 78 return(false); 79 } 80 81 isSameData(const OrbElem * right) const82 bool OrbElem::isSameData(const OrbElem* right) const 83 { 84 if (!OrbElemBase::isSameData(right)) return false; 85 if (Cuc != right->Cuc) return false; 86 if (Cus != right->Cus) return false; 87 if (Crc != right->Crc) return false; 88 if (Crs != right->Crs) return false; 89 if (Cic != right->Cic) return false; 90 if (Cis != right->Cis) return false; 91 if (M0 != right->M0) return false; 92 if (dn != right->dn) return false; 93 if (dndot != right->dndot) return false; 94 if (ecc != right->ecc) return false; 95 if (A != right->A) return false; 96 if (Adot != right->Adot) return false; 97 if (OMEGA0 != right->OMEGA0) return false; 98 if (i0 != right->i0) return false; 99 if (w != right->w) return false; 100 if (OMEGAdot != right->OMEGAdot) return false; 101 if (idot != right->idot) return false; 102 if (ctToc != right->ctToc) return false; 103 if (af0 != right->af0) return false; 104 if (af1 != right->af1) return false; 105 if (af2 != right->af2) return false; 106 return true; 107 } 108 compare(const OrbElem * right) const109 std::list<std::string> OrbElem::compare(const OrbElem* right) const 110 { 111 std::list<std::string> retList = OrbElemBase::compare(right); 112 if (Cuc != right->Cuc) retList.push_back("Cuc"); 113 if (Cus != right->Cus) retList.push_back("Cus"); 114 if (Crc != right->Crc) retList.push_back("Crc"); 115 if (Crs != right->Crs) retList.push_back("Crs"); 116 if (Cic != right->Cic) retList.push_back("Cic"); 117 if (Cis != right->Cis) retList.push_back("Cis"); 118 if (M0 != right->M0) retList.push_back("M0"); 119 if (dn != right->dn) retList.push_back("dn"); 120 if (dndot != right->dndot) retList.push_back("dndot"); 121 if (ecc != right->ecc) retList.push_back("ecc"); 122 if (A != right->A) retList.push_back("A"); 123 if (Adot != right->Adot) retList.push_back("Adot"); 124 if (OMEGA0 != right->OMEGA0) retList.push_back("OMEGA0"); 125 if (i0 != right->i0) retList.push_back("i0"); 126 if (w != right->w) retList.push_back("w"); 127 if (OMEGAdot != right->OMEGAdot) retList.push_back("OMEGAdot"); 128 if (idot != right->idot) retList.push_back("idot"); 129 if (ctToc != right->ctToc) retList.push_back("ctToc"); 130 if (af0 != right->af0) retList.push_back("af0"); 131 if (af1 != right->af1) retList.push_back("af1"); 132 if (af2 != right->af2) retList.push_back("af2"); 133 return retList; 134 } 135 svClockBias(const CommonTime & t) const136 double OrbElem::svClockBias(const CommonTime& t) const 137 { 138 if (!dataLoaded()) 139 { 140 InvalidRequest exc("Required data not stored."); 141 GPSTK_THROW(exc); 142 } 143 double dtc,elaptc; 144 elaptc = t - ctToc; 145 dtc = af0 + elaptc * ( af1 + elaptc * af2 ); 146 return dtc; 147 } 148 svClockBiasM(const CommonTime & t) const149 double OrbElem::svClockBiasM(const CommonTime& t) const 150 { 151 if (!dataLoaded()) 152 { 153 InvalidRequest exc("Required data not stored."); 154 GPSTK_THROW(exc); 155 } 156 double ret = svClockBias(t); 157 ret = ret*C_MPS; 158 return (ret); 159 } 160 svClockDrift(const CommonTime & t) const161 double OrbElem::svClockDrift(const CommonTime& t) const 162 { 163 if (!dataLoaded()) 164 { 165 InvalidRequest exc("Required data not stored."); 166 GPSTK_THROW(exc); 167 } 168 double drift,elaptc; 169 elaptc = t - ctToc; 170 drift = af1 + elaptc * af2; 171 return drift; 172 } 173 174 // Implements equations of motion as defined in IS-GPS-200. 175 // This code has its origins in 1980's FORTRAN code that has 176 // been ported to C, then C++, then became part of the toolkit. 177 // The original code was based on IS-GPS-200 Table 20-IV. 178 // In July 2013, the code was modified to conform to Table 30-II 179 // which includes additional time-dependent terms (A(dot) 180 // and delta n(dot)) that are in CNAV but not in LNAV. These 181 // changes should be backward compatible with LNAV as long as the 182 // Adot and dndot variables are appropriately set to 0.0 by the 183 // LNAV loaders. svXvt(const CommonTime & t) const184 Xvt OrbElem::svXvt(const CommonTime& t) const 185 { 186 if (!dataLoaded()) 187 { 188 InvalidRequest exc("Required data not stored."); 189 GPSTK_THROW(exc); 190 } 191 Xvt sv; 192 193 GPSWeekSecond gpsws = (ctToe); 194 double ToeSOW = gpsws.sow; 195 double ea; // eccentric anomaly // 196 double delea; // delta eccentric anomaly during iteration */ 197 double elapte; // elapsed time since Toe 198 //double elaptc; // elapsed time since Toc 199 double q,sinea,cosea; 200 double GSTA,GCTA; 201 double amm; 202 double meana; // mean anomaly 203 double F,G; // temporary real variables 204 double alat,talat,c2al,s2al,du,dr,di,U,R,truea,AINC; 205 double ANLON,cosu,sinu,xip,yip,can,san,cinc,sinc; 206 double xef,yef,zef,dek,dlk,div,domk,duv,drv; 207 double dxp,dyp,vxef,vyef,vzef; 208 209 // Several GNSSs use this algorithm. In some cases the physical 210 // constants need to be different. 211 EllipsoidModel *ell; 212 if (satID.system==SatelliteSystem::BeiDou) 213 ell = new CGCS2000Ellipsoid(); 214 else 215 ell = new GPSEllipsoid(); 216 217 // Compute time since ephemeris & clock epochs 218 elapte = t - ctToe; 219 double sqrtgm = SQRT(ell->gm()); 220 221 // Compute A at time of interest 222 double Ak = A + Adot * elapte; 223 224 double twoPI = 2.0e0 * PI; 225 double lecc; // eccentricity 226 double tdrinc; // dt inclination 227 228 lecc = ecc; 229 tdrinc = idot; 230 231 // Compute mean motion 232 double dnA = dn + 0.5 * dndot * elapte; 233 double Ahalf = SQRT(A); 234 amm = (sqrtgm / (A*Ahalf)) + dnA; // NOT Ak because this equation 235 // specifies A0, not Ak. 236 237 // In-plane angles 238 // meana - Mean anomaly 239 // ea - Eccentric anomaly 240 // truea - True anomaly 241 242 meana = M0 + elapte * amm; 243 meana = fmod(meana, twoPI); 244 245 ea = meana + lecc * ::sin(meana); 246 247 int loop_cnt = 1; 248 do { 249 F = meana - ( ea - lecc * ::sin(ea)); 250 G = 1.0 - lecc * ::cos(ea); 251 delea = F/G; 252 ea = ea + delea; 253 loop_cnt++; 254 } while ( (fabs(delea) > 1.0e-11 ) && (loop_cnt <= 20) ); 255 256 // Compute clock corrections 257 sv.relcorr = svRelativity(t); 258 sv.clkbias = svClockBias(t); 259 sv.clkdrift = svClockDrift(t); 260 // This appear to be only a string for naming 261 sv.frame = ReferenceFrame::WGS84; 262 263 // Compute true anomaly 264 q = SQRT( 1.0e0 - lecc*lecc); 265 sinea = ::sin(ea); 266 cosea = ::cos(ea); 267 G = 1.0e0 - lecc * cosea; 268 269 // G*SIN(TA) AND G*COS(TA) 270 GSTA = q * sinea; 271 GCTA = cosea - lecc; 272 273 // True anomaly 274 truea = atan2 ( GSTA, GCTA ); 275 276 // Argument of lat and correction terms (2nd harmonic) 277 alat = truea + w; 278 talat = 2.0e0 * alat; 279 c2al = ::cos( talat ); 280 s2al = ::sin( talat ); 281 282 du = c2al * Cuc + s2al * Cus; 283 dr = c2al * Crc + s2al * Crs; 284 di = c2al * Cic + s2al * Cis; 285 286 // U = updated argument of lat, R = radius, AINC = inclination 287 U = alat + du; 288 R = Ak*G + dr; 289 AINC = i0 + tdrinc * elapte + di; 290 291 // Longitude of ascending node (ANLON) 292 ANLON = OMEGA0 + (OMEGAdot - ell->angVelocity()) * 293 elapte - ell->angVelocity() * ToeSOW; 294 295 // In plane location 296 cosu = ::cos( U ); 297 sinu = ::sin( U ); 298 299 xip = R * cosu; 300 yip = R * sinu; 301 302 // Angles for rotation to earth fixed 303 can = ::cos( ANLON ); 304 san = ::sin( ANLON ); 305 cinc = ::cos( AINC ); 306 sinc = ::sin( AINC ); 307 308 // Earth fixed - meters 309 xef = xip*can - yip*cinc*san; 310 yef = xip*san + yip*cinc*can; 311 zef = yip*sinc; 312 313 sv.x[0] = xef; 314 sv.x[1] = yef; 315 sv.x[2] = zef; 316 317 // Compute velocity of rotation coordinates 318 dek = amm / G; 319 dlk = amm * q / (G*G); 320 div = tdrinc - 2.0e0 * dlk * 321 ( Cic * s2al - Cis * c2al ); 322 domk = OMEGAdot - ell->angVelocity(); 323 duv = dlk*(1.e0+ 2.e0 * (Cus*c2al - Cuc*s2al) ); 324 drv = Ak * lecc * dek * sinea - 2.e0 * dlk * 325 ( Crc * s2al - Crs * c2al ) + Adot * G; 326 327 dxp = drv*cosu - R*sinu*duv; 328 dyp = drv*sinu + R*cosu*duv; 329 330 // Calculate velocities 331 vxef = dxp*can - xip*san*domk - dyp*cinc*san 332 + yip*( sinc*san*div - cinc*can*domk); 333 vyef = dxp*san + xip*can*domk + dyp*cinc*can 334 - yip*( sinc*can*div + cinc*san*domk); 335 vzef = dyp*sinc + yip*cinc*div; 336 337 // Move results into output variables 338 sv.v[0] = vxef; 339 sv.v[1] = vyef; 340 sv.v[2] = vzef; 341 sv.health = isHealthy() ? Xvt::Healthy : Xvt::Unhealthy; 342 delete ell; 343 344 return sv; 345 } 346 svRelativity(const CommonTime & t) const347 double OrbElem::svRelativity(const CommonTime& t) const 348 { 349 if (!dataLoaded()) 350 { 351 InvalidRequest exc("Required data not stored."); 352 GPSTK_THROW(exc); 353 } 354 355 // Several GNSSs use this algorithm. In some cases the physical 356 // constants need to be different. 357 EllipsoidModel *ell; 358 if (satID.system==SatelliteSystem::BeiDou) 359 ell = new CGCS2000Ellipsoid(); 360 else 361 ell = new GPSEllipsoid(); 362 363 double twoPI = 2.0e0 * PI; 364 double sqrtgm = SQRT(ell->gm()); 365 delete ell; 366 double elapte = t - ctToe; 367 368 // Compute A at time of interest 369 double Ak = A + Adot * elapte; 370 371 double dnA = dn + 0.5 * dndot * elapte; 372 double Ahalf = SQRT(A); 373 double amm = (sqrtgm / (A*Ahalf)) + dnA;// NOT Ak because this equation 374 // specifies A0, not Ak. 375 double meana,F,G,delea; 376 377 meana = M0 + elapte * amm; 378 meana = fmod(meana, twoPI); 379 double ea = meana + ecc * ::sin(meana); 380 381 int loop_cnt = 1; 382 do { 383 F = meana - ( ea - ecc * ::sin(ea)); 384 G = 1.0 - ecc * ::cos(ea); 385 delea = F/G; 386 ea = ea + delea; 387 loop_cnt++; 388 } while ( (ABS(delea) > 1.0e-11 ) && (loop_cnt <= 20) ); 389 390 // Use Ak as we are interested in semi-major axis at this moment. 391 double dtr = REL_CONST * ecc * SQRT(Ak) * ::sin(ea); 392 return dtr; 393 } 394 shortcut(ostream & os,const long HOW)395 void OrbElem::shortcut(ostream & os, const long HOW ) 396 { 397 short DOW, hour, min, sec; 398 long SOD, SOW; 399 short SOH; 400 401 SOW = static_cast<long>( HOW ); 402 DOW = static_cast<short>( SOW / SEC_PER_DAY ); 403 SOD = SOW - static_cast<long>( DOW * SEC_PER_DAY ); 404 hour = static_cast<short>( SOD/3600 ); 405 406 SOH = static_cast<short>( SOD - (hour*3600) ); 407 min = SOH/60; 408 409 sec = SOH - min * 60; 410 switch (DOW) 411 { 412 case 0: os << "Sun-0"; break; 413 case 1: os << "Mon-1"; break; 414 case 2: os << "Tue-2"; break; 415 case 3: os << "Wed-3"; break; 416 case 4: os << "Thu-4"; break; 417 case 5: os << "Fri-5"; break; 418 case 6: os << "Sat-6"; break; 419 default: break; 420 } 421 422 os << ":" << setfill('0') 423 << setw(2) << hour 424 << ":" << setw(2) << min 425 << ":" << setw(2) << sec 426 << setfill(' '); 427 } 428 429 timeDisplay(ostream & os,const CommonTime & t)430 void OrbElem::timeDisplay( ostream & os, const CommonTime& t ) 431 { 432 os.setf(ios::dec, ios::basefield); 433 // Convert to CommonTime struct from GPS wk,SOW to M/D/Y, H:M:S. 434 GPSWeekSecond dummyTime; 435 dummyTime = GPSWeekSecond(t); 436 os << setw(4) << dummyTime.week << "("; 437 os << setw(4) << (dummyTime.week & 0x03FF) << ") "; 438 os << setw(6) << setfill(' ') << dummyTime.sow << " "; 439 440 switch (dummyTime.getDayOfWeek()) 441 { 442 case 0: os << "Sun-0"; break; 443 case 1: os << "Mon-1"; break; 444 case 2: os << "Tue-2"; break; 445 case 3: os << "Wed-3"; break; 446 case 4: os << "Thu-4"; break; 447 case 5: os << "Fri-5"; break; 448 case 6: os << "Sat-6"; break; 449 default: break; 450 } 451 os << printTime(t," %3j %5.0s %02m/%02d/%04Y %02H:%02M:%02S"); 452 } 453 dumpBody(ostream & s) const454 void OrbElem::dumpBody(ostream& s) const 455 { 456 if (!dataLoaded()) 457 { 458 InvalidRequest exc("Required data not stored."); 459 GPSTK_THROW(exc); 460 } 461 const ios::fmtflags oldFlags = s.flags(); 462 size_t precision = 8; 463 464 s.setf(ios::fixed, ios::floatfield); 465 s.setf(ios::right, ios::adjustfield); 466 s.setf(ios::uppercase); 467 s.precision(0); 468 s.fill(' '); 469 470 s << endl; 471 s << " TIMES OF INTEREST" 472 << endl << endl; 473 s << " Week(10bt) SOW DOW UTD SOD" 474 << " MM/DD/YYYY HH:MM:SS\n"; 475 s << "Begin Valid: "; 476 timeDisplay(s, beginValid); 477 s << endl; 478 s << "Clock Epoch: "; 479 timeDisplay(s, ctToc); 480 s << endl; 481 s << "Eph Epoch: "; 482 timeDisplay(s, ctToe); 483 s << endl; 484 s << "End Valid: "; 485 timeDisplay(s, endValid); 486 487 s.setf(ios::scientific, ios::floatfield); 488 s.precision(precision); 489 s.fill(' '); 490 unsigned fw = precision + 8; 491 492 s << endl 493 << endl 494 << " CLOCK PARAMETERS" 495 << endl 496 << endl 497 << "Bias T0: " << setw(fw) << af0 << " sec" << endl 498 << "Drift: " << setw(fw) << af1 << " sec/sec" << endl 499 << "Drift rate: " << setw(fw) << af2 << " sec/(sec**2)" << endl; 500 501 s << endl 502 << " ORBIT PARAMETERS" 503 << endl 504 << endl 505 << "Semi-major axis: " << setw(fw) << A << " m " 506 << setw(fw) << Adot << " m/sec" << endl 507 << "Motion correction: " << setw(fw) << dn << " rad/sec " 508 << setw(fw) << dndot << " rad/(sec**2)" << endl 509 << "Eccentricity: " << setw(fw) << ecc << endl 510 << "Arg of perigee: " << setw(fw) << w << " rad" << endl 511 << "Mean anomaly at epoch: " << setw(fw) << M0 << " rad" << endl 512 << "Right ascension: " << setw(fw) << OMEGA0 << " rad " 513 << setw(fw) << OMEGAdot << " rad/sec" << endl 514 << "Inclination: " << setw(fw) << i0 << " rad " 515 << setw(fw) << idot << " rad/sec" << endl; 516 517 s << endl 518 << " HARMONIC CORRECTIONS" 519 << endl 520 << endl 521 << "Radial Sine: " << setw(fw) << Crs << " m Cosine: " 522 << setw(fw) << Crc << " m" << endl 523 << "Inclination Sine: " << setw(fw) << Cis << " rad Cosine: " 524 << setw(fw) << Cic << " rad" << endl 525 << "In-track Sine: " << setw(fw) << Cus << " rad Cosine: " 526 << setw(fw) << Cuc << " rad" << endl; 527 528 s.flags(oldFlags); 529 } // end of dumpBody() 530 dumpHeader(ostream & s) const531 void OrbElem::dumpHeader(ostream& s) const 532 { 533 s << "****************************************************************" 534 << "************" << endl 535 << "Broadcast Ephemeris (Engineering Units) - " << getNameLong(); 536 s << endl; 537 538 s << endl; 539 s << "PRN : " << setw(2) << satID.id << " / " 540 << "SVN : " << setw(2); 541 std::string svn; 542 if (getSVN(satID, ctToe, svn)) 543 { 544 s << svn; 545 } 546 s << " " << endl 547 << endl; 548 } 549 dumpFooter(ostream & s) const550 void OrbElem::dumpFooter(ostream& s) const 551 {} 552 dump(ostream & s) const553 void OrbElem::dump(ostream& s) const 554 { 555 dumpHeader(s); 556 dumpBody(s); 557 dumpFooter(s); 558 } 559 560 } // namespace 561