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 /** 40 * @file BrcKeplerOrbit.cpp 41 * Ephemeris data encapsulated in engineering terms 42 */ 43 #include <stdio.h> 44 #include "BrcKeplerOrbit.hpp" 45 #include <cmath> 46 47 namespace gpstk 48 { 49 using namespace std; 50 using namespace gpstk; 51 BrcKeplerOrbit()52 BrcKeplerOrbit::BrcKeplerOrbit() 53 throw() 54 { 55 dataLoaded = false; 56 57 PRNID = 0; 58 59 satSys = ""; 60 61 healthy = false; 62 63 URAoe = 0; 64 65 Cuc = Cus = Crc = Crs = Cic = Cis = M0 = dn = dndot = 66 ecc = A = Ahalf =Adot = OMEGA0 = i0 = w = OMEGAdot = idot = 0.0; 67 } 68 BrcKeplerOrbit(const std::string satSysArg,const ObsID obsIDArg,const short PRNIDArg,const CommonTime beginFitArg,const CommonTime endFitArg,const CommonTime ToeArg,const short URAoeArg,const bool healthyArg,const double CucArg,const double CusArg,const double CrcArg,const double CrsArg,const double CicArg,const double CisArg,const double M0Arg,const double dnArg,const double dndotArg,const double eccArg,const double AArg,const double AhalfArg,const double AdotArg,const double OMEGA0Arg,const double i0Arg,const double wArg,const double OMEGAdotARg,const double idotArg)69 BrcKeplerOrbit::BrcKeplerOrbit(const std::string satSysArg, 70 const ObsID obsIDArg, 71 const short PRNIDArg, 72 const CommonTime beginFitArg, 73 const CommonTime endFitArg, 74 const CommonTime ToeArg, 75 const short URAoeArg, const bool healthyArg, 76 const double CucArg, const double CusArg, 77 const double CrcArg, const double CrsArg, 78 const double CicArg, const double CisArg, 79 const double M0Arg, const double dnArg, 80 const double dndotArg, const double eccArg, 81 const double AArg, const double AhalfArg, 82 const double AdotArg, const double OMEGA0Arg, 83 const double i0Arg, const double wArg, 84 const double OMEGAdotARg, 85 const double idotArg ) 86 { 87 loadData(satSysArg, obsIDArg, PRNIDArg, beginFitArg, endFitArg, ToeArg, 88 URAoeArg, healthyArg, CucArg, CusArg, CrcArg, 89 CrsArg, CicArg, CisArg, M0Arg, dnArg, dndotArg, eccArg, AArg, 90 AhalfArg, AdotArg, OMEGA0Arg, i0Arg, wArg, OMEGAdotARg, idotArg); 91 } 92 93 /// Legacy GPS Subframe 1-3 BrcKeplerOrbit(const ObsID obsIDArg,const short PRNID,const short fullweeknum,const long subframe1[10],const long subframe2[10],const long subframe3[10])94 BrcKeplerOrbit::BrcKeplerOrbit(const ObsID obsIDArg, const short PRNID, 95 const short fullweeknum, 96 const long subframe1[10], 97 const long subframe2[10], 98 const long subframe3[10] ) 99 { 100 loadData(obsIDArg, PRNID,fullweeknum, subframe1, subframe2, subframe3 ); 101 } 102 103 operator ==(const BrcKeplerOrbit & right) const104 bool BrcKeplerOrbit::operator==(const BrcKeplerOrbit& right) const throw() 105 { 106 return ((dataLoaded == right.dataLoaded) && 107 (satSys == right.satSys) && 108 (obsID == right.obsID) && 109 (PRNID == right.PRNID) && 110 (Toe == right.Toe) && 111 (URAoe == right.URAoe) && 112 (healthy == right.healthy) && 113 (Cuc == right.Cuc) && 114 (Cus == right.Cus) && 115 (Crc == right.Crc) && 116 (Crs == right.Crs) && 117 (Cic == right.Cic) && 118 (Cis == right.Cis) && 119 (M0 == right.M0) && 120 (dn == right.dn) && 121 (dndot == right.dndot) && 122 (ecc == right.ecc) && 123 (A == right.A) && 124 (Ahalf == right.Ahalf) && 125 (Adot == right.Adot) && 126 (OMEGA0 == right.OMEGA0) && 127 (i0 == right.i0) && 128 (w == right.w) && 129 (OMEGAdot == right.OMEGAdot) && 130 (idot == right.idot) && 131 (beginFit == right.beginFit) && 132 (endFit == right.endFit)); 133 } 134 135 loadData(const std::string satSysArg,const ObsID obsIDArg,const short PRNIDArg,const CommonTime beginFitArg,const CommonTime endFitArg,const CommonTime ToeArg,const short URAoeArg,const bool healthyArg,const double CucArg,const double CusArg,const double CrcArg,const double CrsArg,const double CicArg,const double CisArg,const double M0Arg,const double dnArg,const double dndotArg,const double eccArg,const double AArg,const double AhalfArg,const double AdotArg,const double OMEGA0Arg,const double i0Arg,const double wArg,const double OMEGAdotARg,const double idotArg)136 void BrcKeplerOrbit::loadData(const std::string satSysArg, 137 const ObsID obsIDArg, 138 const short PRNIDArg, 139 const CommonTime beginFitArg, 140 const CommonTime endFitArg, 141 const CommonTime ToeArg, 142 const short URAoeArg, const bool healthyArg, 143 const double CucArg, const double CusArg, 144 const double CrcArg, const double CrsArg, 145 const double CicArg, const double CisArg, 146 const double M0Arg, const double dnArg, 147 const double dndotArg, const double eccArg, 148 const double AArg, const double AhalfArg, 149 const double AdotArg, const double OMEGA0Arg, 150 const double i0Arg, const double wArg, 151 const double OMEGAdotARg, const double idotArg) 152 { 153 satSys = satSysArg; 154 obsID = obsIDArg; 155 PRNID = PRNIDArg; 156 beginFit = beginFitArg; 157 endFit = endFitArg; 158 Toe = ToeArg; 159 URAoe = URAoeArg; 160 healthy = healthyArg; 161 Cuc = CucArg; 162 Cus = CusArg; 163 Crc = CrcArg; 164 Crs = CrsArg; 165 Cic = CicArg; 166 Cis = CisArg; 167 M0 = M0Arg; 168 dn = dnArg; 169 dndot = dndotArg; 170 ecc = eccArg; 171 A = AArg; 172 Ahalf = AhalfArg; 173 Adot = AdotArg; 174 OMEGA0 = OMEGA0Arg; 175 i0 = i0Arg; 176 w = wArg; 177 OMEGAdot = OMEGAdotARg; 178 idot = idotArg; 179 dataLoaded = true; 180 } 181 loadData(const ObsID obsIDArg,const short PRNIDArg,const short fullweeknum,const long subframe1[10],const long subframe2[10],const long subframe3[10])182 void BrcKeplerOrbit::loadData(const ObsID obsIDArg, const short PRNIDArg, 183 const short fullweeknum, 184 const long subframe1[10], 185 const long subframe2[10], 186 const long subframe3[10]) 187 { 188 double ficked[60]; 189 190 //Load overhead members 191 satSys = "G"; 192 obsID = obsIDArg; 193 PRNID = PRNIDArg; 194 short iodc = 0; 195 196 //Convert Subframe 1 197 if (!subframeConvert(subframe1, fullweeknum, ficked)) 198 { 199 InvalidParameter exc("Subframe 1 not valid."); 200 GPSTK_THROW(exc); 201 } 202 203 short weeknum = static_cast<short>( ficked[5] ); 204 short accFlag = static_cast<short>( ficked[7] ); 205 short health = static_cast<short>( ficked[8] ); 206 URAoe = accFlag; 207 healthy = false; 208 if (health == 0) 209 healthy = true; 210 iodc = static_cast<short>( ldexp( ficked[9], -11 ) ); 211 212 //Convert Subframe 2 213 if (!subframeConvert(subframe2, fullweeknum, ficked)) 214 { 215 InvalidParameter exc("Subframe 2 not valid."); 216 GPSTK_THROW(exc); 217 } 218 219 Crs = ficked[6]; 220 dn = ficked[7]; 221 M0 = ficked[8]; 222 Cuc = ficked[9]; 223 ecc = ficked[10]; 224 Cus = ficked[11]; 225 Ahalf = ficked[12]; 226 A = Ahalf*Ahalf; 227 double ToeSOW = ficked[13]; 228 /* 229 double diff = Txmit - ToeSOW; 230 if (diff > HALFWEEK) // NOTE: This USED to be in DayTime, but DayTime is going away. Where is it now? 231 weeknum++; // Convert week # of transmission to week # of epoch time when Toc is forward across a week boundary 232 else if (diff < -HALFWEEK) 233 weeknum--; // Convert week # of transmission to week # of epoch time when Toc is back across a week boundary 234 */ 235 Toe = GPSWeekSecond(weeknum, ToeSOW, TimeSystem::GPS); 236 short fiti = static_cast<short>(ficked[14]); 237 short fitHours = getLegacyFitInterval(iodc, fiti); 238 long beginFitSOW = ToeSOW - (fitHours/2)*3600; 239 long endFitSOW = ToeSOW + (fitHours/2)*3600; 240 short beginFitWk = weeknum; 241 short endFitWk = weeknum; 242 if (beginFitSOW < 0) 243 { 244 beginFitSOW += FULLWEEK; 245 beginFitWk--; 246 } 247 beginFit = GPSWeekSecond(beginFitWk, beginFitSOW, TimeSystem::GPS); 248 249 if (endFitSOW >= FULLWEEK) 250 { 251 endFitSOW -= FULLWEEK; 252 endFitWk++; 253 } 254 endFit = GPSWeekSecond(endFitWk, endFitSOW, TimeSystem::GPS); 255 256 //Convert Subframe 3 257 if (!subframeConvert(subframe3, fullweeknum, ficked)) 258 { 259 InvalidParameter exc("Subframe3 not valid."); 260 GPSTK_THROW(exc); 261 } 262 263 Cic = ficked[5]; 264 OMEGA0 = ficked[6]; 265 Cis = ficked[7]; 266 i0 = ficked[8]; 267 Crc = ficked[9]; 268 w = ficked[10]; 269 OMEGAdot = ficked[11]; 270 idot = ficked[13]; 271 272 dndot = 0.0; 273 Adot = 0.0; 274 dataLoaded = true; 275 276 return; 277 } 278 hasData() const279 bool BrcKeplerOrbit::hasData() const 280 { 281 return(dataLoaded); 282 } 283 isHealthy() const284 bool BrcKeplerOrbit::isHealthy() const 285 { 286 if (!dataLoaded) 287 { 288 InvalidRequest exc("Required data not stored."); 289 GPSTK_THROW(exc); 290 } 291 return(healthy); 292 } 293 withinFitInterval(const CommonTime ct) const294 bool BrcKeplerOrbit::withinFitInterval(const CommonTime ct) const 295 { 296 if (!dataLoaded) 297 { 298 InvalidRequest exc("Required data not stored."); 299 GPSTK_THROW(exc); 300 } 301 if (ct >= beginFit && ct <= endFit) return(true); 302 return(false); 303 } 304 svXvt(const CommonTime & t) const305 Xvt BrcKeplerOrbit::svXvt(const CommonTime& t) const 306 { 307 Xvt sv; 308 309 GPSWeekSecond gpsws = (Toe); 310 double ToeSOW = gpsws.sow; 311 double ea; // eccentric anomaly // 312 double delea; // delta eccentric anomaly during iteration */ 313 double elapte; // elapsed time since Toe 314 //double elaptc; // elapsed time since Toc 315 double q,sinea,cosea; 316 double GSTA,GCTA; 317 double amm; 318 double meana; // mean anomaly 319 double F,G; // temporary real variables 320 double alat,talat,c2al,s2al,du,dr,di,U,R,truea,AINC; 321 double ANLON,cosu,sinu,xip,yip,can,san,cinc,sinc; 322 double xef,yef,zef,dek,dlk,div,domk,duv,drv; 323 double dxp,dyp,vxef,vyef,vzef; 324 GPSEllipsoid ell; 325 326 double sqrtgm = SQRT(ell.gm()); 327 328 // Check for ground transmitter 329 double twoPI = 2.0e0 * PI; 330 double lecc; // eccentricity 331 double tdrinc; // dt inclination 332 333 lecc = ecc; 334 tdrinc = idot; 335 336 // Compute time since ephemeris & clock epochs 337 elapte = t - getOrbitEpoch(); 338 //CommonTime orbEp = getOrbitEpoch(); 339 //elapte = t - orbEp; 340 341 // Compute mean motion 342 amm = (sqrtgm / (A*Ahalf)) + dn; 343 344 345 // In-plane angles 346 // meana - Mean anomaly 347 // ea - Eccentric anomaly 348 // truea - True anomaly 349 350 meana = M0 + elapte * amm; 351 meana = fmod(meana, twoPI); 352 353 ea = meana + lecc * ::sin(meana); 354 355 int loop_cnt = 1; 356 do { 357 F = meana - ( ea - lecc * ::sin(ea)); 358 G = 1.0 - lecc * ::cos(ea); 359 delea = F/G; 360 ea = ea + delea; 361 loop_cnt++; 362 } while ( (fabs(delea) > 1.0e-11 ) && (loop_cnt <= 20) ); 363 364 // Compute clock corrections 365 sv.relcorr = svRelativity(t); 366 // This appears to be only a string for naming 367 sv.frame = ReferenceFrame::WGS84; 368 369 // Compute true anomaly 370 q = SQRT( 1.0e0 - lecc*lecc); 371 sinea = ::sin(ea); 372 cosea = ::cos(ea); 373 G = 1.0e0 - lecc * cosea; 374 375 // G*SIN(TA) AND G*COS(TA) 376 GSTA = q * sinea; 377 GCTA = cosea - lecc; 378 379 // True anomaly 380 truea = atan2 ( GSTA, GCTA ); 381 382 // Argument of lat and correction terms (2nd harmonic) 383 alat = truea + w; 384 talat = 2.0e0 * alat; 385 c2al = ::cos( talat ); 386 s2al = ::sin( talat ); 387 388 du = c2al * Cuc + s2al * Cus; 389 dr = c2al * Crc + s2al * Crs; 390 di = c2al * Cic + s2al * Cis; 391 392 // U = updated argument of lat, R = radius, AINC = inclination 393 U = alat + du; 394 R = A*G + dr; 395 AINC = i0 + tdrinc * elapte + di; 396 397 // Longitude of ascending node (ANLON) 398 ANLON = OMEGA0 + (OMEGAdot - ell.angVelocity()) * 399 elapte - ell.angVelocity() * ToeSOW; 400 401 // In plane location 402 cosu = ::cos( U ); 403 sinu = ::sin( U ); 404 405 xip = R * cosu; 406 yip = R * sinu; 407 408 // Angles for rotation to earth fixed 409 can = ::cos( ANLON ); 410 san = ::sin( ANLON ); 411 cinc = ::cos( AINC ); 412 sinc = ::sin( AINC ); 413 414 // Earth fixed - meters 415 xef = xip*can - yip*cinc*san; 416 yef = xip*san + yip*cinc*can; 417 zef = yip*sinc; 418 419 sv.x[0] = xef; 420 sv.x[1] = yef; 421 sv.x[2] = zef; 422 423 // Compute velocity of rotation coordinates 424 dek = amm / G; 425 dlk = amm * q / (G*G); 426 div = tdrinc - 2.0e0 * dlk * 427 ( Cic * s2al - Cis * c2al ); 428 domk = OMEGAdot - ell.angVelocity(); 429 duv = dlk*(1.e0+ 2.e0 * (Cus*c2al - Cuc*s2al) ); 430 drv = A * lecc * dek * sinea - 2.e0 * dlk * 431 ( Crc * s2al - Crs * c2al ) + Adot * G; 432 433 dxp = drv*cosu - R*sinu*duv; 434 dyp = drv*sinu + R*cosu*duv; 435 436 // Calculate velocities 437 vxef = dxp*can - xip*san*domk - dyp*cinc*san 438 + yip*( sinc*san*div - cinc*can*domk); 439 vyef = dxp*san + xip*can*domk + dyp*cinc*can 440 - yip*( sinc*can*div + cinc*san*domk); 441 vzef = dyp*sinc + yip*cinc*div; 442 443 // Move results into output variables 444 sv.v[0] = vxef; 445 sv.v[1] = vyef; 446 sv.v[2] = vzef; 447 sv.health = healthy ? Xvt::Healthy : Xvt::Unhealthy; 448 449 return sv; 450 } 451 svRelativity(const CommonTime & t) const452 double BrcKeplerOrbit::svRelativity(const CommonTime& t) const 453 { 454 GPSEllipsoid ell; 455 double twoPI = 2.0e0 * PI; 456 double sqrtgm = SQRT(ell.gm()); 457 double elapte = t - getOrbitEpoch(); 458 double amm = (sqrtgm / (A*Ahalf)) + dn; 459 double meana,F,G,delea; 460 461 meana = M0 + elapte * amm; 462 meana = fmod(meana, twoPI); 463 double ea = meana + ecc * ::sin(meana); 464 465 int loop_cnt = 1; 466 do { 467 F = meana - ( ea - ecc * ::sin(ea)); 468 G = 1.0 - ecc * ::cos(ea); 469 delea = F/G; 470 ea = ea + delea; 471 loop_cnt++; 472 } while ( (ABS(delea) > 1.0e-11 ) && (loop_cnt <= 20) ); 473 double dtr = REL_CONST * ecc * Ahalf * ::sin(ea); 474 return dtr; 475 } 476 getOrbitEpoch() const477 CommonTime BrcKeplerOrbit::getOrbitEpoch() const 478 { 479 return Toe; 480 } 481 getBeginningOfFitInterval() const482 CommonTime BrcKeplerOrbit::getBeginningOfFitInterval() const 483 { 484 if (!dataLoaded) 485 { 486 InvalidRequest exc("Required data not stored."); 487 GPSTK_THROW(exc); 488 } 489 return beginFit; 490 } 491 getEndOfFitInterval() const492 CommonTime BrcKeplerOrbit::getEndOfFitInterval() const 493 { 494 if (!dataLoaded) 495 { 496 InvalidRequest exc("Required data not stored."); 497 GPSTK_THROW(exc); 498 } 499 return endFit; 500 } 501 getPRNID() const502 short BrcKeplerOrbit::getPRNID() const 503 { 504 if(!dataLoaded) 505 { 506 InvalidRequest exc("Required data not stored."); 507 GPSTK_THROW(exc); 508 } 509 return PRNID; 510 } 511 getObsID() const512 ObsID BrcKeplerOrbit::getObsID() const 513 { 514 if(!dataLoaded) 515 { 516 InvalidRequest exc("Required data not stored."); 517 GPSTK_THROW(exc); 518 } 519 return obsID; 520 } 521 getFullWeek() const522 short BrcKeplerOrbit::getFullWeek() const 523 { 524 if (!dataLoaded) 525 { 526 InvalidRequest exc("Required data not stored."); 527 GPSTK_THROW(exc); 528 } 529 GPSWeekSecond gpsws(Toe); 530 return (gpsws.week); 531 } 532 getAccuracy() const533 double BrcKeplerOrbit::getAccuracy() const 534 { 535 if (!dataLoaded) 536 { 537 InvalidRequest exc("Required data not stored."); 538 GPSTK_THROW(exc); 539 } 540 double accuracy = ura2CNAVaccuracy(URAoe); 541 return accuracy; 542 } 543 setAccuracy(const double & acc)544 void BrcKeplerOrbit::setAccuracy(const double& acc) 545 { 546 if (!dataLoaded) 547 { 548 InvalidRequest exc("Required data not stored."); 549 GPSTK_THROW(exc); 550 } 551 URAoe = accuracy2ura(acc); 552 } 553 getURAoe() const554 short BrcKeplerOrbit::getURAoe() const 555 { 556 if (!dataLoaded) 557 { 558 InvalidRequest exc("Required data not stored."); 559 GPSTK_THROW(exc); 560 } 561 return URAoe; 562 } 563 getCus() const564 double BrcKeplerOrbit::getCus() const 565 { 566 if (!dataLoaded) 567 { 568 InvalidRequest exc("Required data not stored."); 569 GPSTK_THROW(exc); 570 } 571 return Cus; 572 } 573 getCrs() const574 double BrcKeplerOrbit::getCrs() const 575 { 576 if (!dataLoaded) 577 { 578 InvalidRequest exc("Required data not stored."); 579 GPSTK_THROW(exc); 580 } 581 return Crs; 582 } 583 getCis() const584 double BrcKeplerOrbit::getCis() const 585 { 586 if (!dataLoaded) 587 { 588 InvalidRequest exc("Required data not stored."); 589 GPSTK_THROW(exc); 590 } 591 return Cis; 592 } 593 getCrc() const594 double BrcKeplerOrbit::getCrc() const 595 { 596 if (!dataLoaded) 597 { 598 InvalidRequest exc("Required data not stored."); 599 GPSTK_THROW(exc); 600 } 601 return Crc; 602 } 603 getCuc() const604 double BrcKeplerOrbit::getCuc() const 605 { 606 if (!dataLoaded) 607 { 608 InvalidRequest exc("Required data not stored."); 609 GPSTK_THROW(exc); 610 } 611 return Cuc; 612 } 613 getCic() const614 double BrcKeplerOrbit::getCic() const 615 { 616 if (!dataLoaded) 617 { 618 InvalidRequest exc("Required data not stored."); 619 GPSTK_THROW(exc); 620 } 621 return Cic; 622 } 623 getToe() const624 double BrcKeplerOrbit::getToe() const 625 { 626 if (!dataLoaded) 627 { 628 InvalidRequest exc("Required data not stored."); 629 GPSTK_THROW(exc); 630 } 631 GPSWeekSecond gpsws(Toe); 632 return gpsws.sow; 633 } 634 getM0() const635 double BrcKeplerOrbit::getM0() const 636 { 637 if (!dataLoaded) 638 { 639 InvalidRequest exc("Required data not stored."); 640 GPSTK_THROW(exc); 641 } 642 return M0; 643 } 644 getDn() const645 double BrcKeplerOrbit::getDn() const 646 { 647 if (!dataLoaded) 648 { 649 InvalidRequest exc("Required data not stored."); 650 GPSTK_THROW(exc); 651 } 652 return dn; 653 } 654 getEcc() const655 double BrcKeplerOrbit::getEcc() const 656 { 657 if (!dataLoaded) 658 { 659 InvalidRequest exc("Required data not stored."); 660 GPSTK_THROW(exc); 661 } 662 return ecc; 663 } 664 getA() const665 double BrcKeplerOrbit::getA() const 666 { 667 if (!dataLoaded) 668 { 669 InvalidRequest exc("Required data not stored."); 670 GPSTK_THROW(exc); 671 } 672 return A; 673 } 674 getAhalf() const675 double BrcKeplerOrbit::getAhalf() const 676 { 677 if (!dataLoaded) 678 { 679 InvalidRequest exc("Required data not stored."); 680 GPSTK_THROW(exc); 681 } 682 return Ahalf; 683 } 684 getAdot() const685 double BrcKeplerOrbit::getAdot() const 686 { 687 if (!dataLoaded) 688 { 689 InvalidRequest exc("Required data not stored."); 690 GPSTK_THROW(exc); 691 } 692 return Adot; 693 } 694 getDnDot() const695 double BrcKeplerOrbit::getDnDot() const 696 { 697 if (!dataLoaded) 698 { 699 InvalidRequest exc("Required data not stored."); 700 GPSTK_THROW(exc); 701 } 702 return dndot; 703 } 704 getOmega0() const705 double BrcKeplerOrbit::getOmega0() const 706 { 707 if (!dataLoaded) 708 { 709 InvalidRequest exc("Required data not stored."); 710 GPSTK_THROW(exc); 711 } 712 return OMEGA0; 713 } 714 getI0() const715 double BrcKeplerOrbit::getI0() const 716 { 717 if (!dataLoaded) 718 { 719 InvalidRequest exc("Required data not stored."); 720 GPSTK_THROW(exc); 721 } 722 return i0; 723 } 724 getW() const725 double BrcKeplerOrbit::getW() const 726 { 727 if (!dataLoaded) 728 { 729 InvalidRequest exc("Required data not stored."); 730 GPSTK_THROW(exc); 731 } 732 return w; 733 } 734 getOmegaDot() const735 double BrcKeplerOrbit::getOmegaDot() const 736 { 737 if (!dataLoaded) 738 { 739 InvalidRequest exc("Required data not stored."); 740 GPSTK_THROW(exc); 741 } 742 return OMEGAdot; 743 } 744 getIDot() const745 double BrcKeplerOrbit::getIDot() const 746 { 747 if (!dataLoaded) 748 { 749 InvalidRequest exc("Required data not stored."); 750 GPSTK_THROW(exc); 751 } 752 return idot; 753 } 754 timeDisplay(ostream & os,const CommonTime & t)755 static void timeDisplay( ostream & os, const CommonTime& t ) 756 { 757 // Convert to CommonTime struct from GPS wk,SOW to M/D/Y, H:M:S. 758 GPSWeekSecond dummyTime; 759 dummyTime = GPSWeekSecond(t); 760 os << setw(4) << dummyTime.week << "("; 761 os << setw(4) << (dummyTime.week & 0x03FF) << ") "; 762 os << setw(6) << setfill(' ') << dummyTime.sow << " "; 763 764 switch (dummyTime.getDayOfWeek()) 765 { 766 case 0: os << "Sun-0"; break; 767 case 1: os << "Mon-1"; break; 768 case 2: os << "Tue-2"; break; 769 case 3: os << "Wed-3"; break; 770 case 4: os << "Thu-4"; break; 771 case 5: os << "Fri-5"; break; 772 case 6: os << "Sat-6"; break; 773 default: break; 774 } 775 os << " " << (static_cast<YDSTime>(t)).printf("%3j %5.0s ") 776 << (static_cast<CivilTime>(t)).printf("%02m/%02d/%04Y %02H:%02M:%02S"); 777 } 778 #pragma clang diagnostic push 779 #pragma clang diagnostic ignored "-Wunused-function" shortcut(ostream & os,const long HOW)780 static void shortcut(ostream & os, const long HOW ) 781 { 782 short DOW, hour, min, sec; 783 long SOD, SOW; 784 short SOH; 785 786 SOW = static_cast<long>( HOW ); 787 DOW = static_cast<short>( SOW / SEC_PER_DAY ); 788 SOD = SOW - static_cast<long>( DOW * SEC_PER_DAY ); 789 hour = static_cast<short>( SOD/3600 ); 790 791 SOH = static_cast<short>( SOD - (hour*3600) ); 792 min = SOH/60; 793 794 sec = SOH - min * 60; 795 switch (DOW) 796 { 797 case 0: os << "Sun-0"; break; 798 case 1: os << "Mon-1"; break; 799 case 2: os << "Tue-2"; break; 800 case 3: os << "Wed-3"; break; 801 case 4: os << "Thu-4"; break; 802 case 5: os << "Fri-5"; break; 803 case 6: os << "Sat-6"; break; 804 default: break; 805 } 806 807 os << ":" << setfill('0') 808 << setw(2) << hour 809 << ":" << setw(2) << min 810 << ":" << setw(2) << sec 811 << setfill(' '); 812 } 813 #pragma clang diagnostic pop dump(ostream & s) const814 void BrcKeplerOrbit::dump(ostream& s) const 815 throw() 816 { 817 const ios::fmtflags oldFlags = s.flags(); 818 s.setf(ios::fixed, ios::floatfield); 819 s.setf(ios::right, ios::adjustfield); 820 s.setf(ios::uppercase); 821 s.precision(0); 822 s.fill(' '); 823 824 s << "****************************************************************" 825 << "************" << endl 826 << "Broadcast Ephemeris (Engineering Units)" << endl 827 << endl 828 << "PRN : " << setw(2) << PRNID << endl 829 << endl; 830 831 s << " Week(10bt) SOW DOW UTD SOD" 832 << " MM/DD/YYYY HH:MM:SS\n"; 833 834 s << endl; 835 s << "Eph Epoch: "; 836 timeDisplay(s, getOrbitEpoch()); 837 s << endl; 838 839 s.setf(ios::scientific, ios::floatfield); 840 s.precision(8); 841 842 s << endl 843 << " ORBIT PARAMETERS" 844 << endl 845 << endl 846 << "Semi-major axis: " << setw(16) << Ahalf << " m**.5" << endl 847 << "Motion correction: " << setw(16) << dn << " rad/sec" 848 << endl 849 << "Eccentricity: " << setw(16) << ecc << endl 850 << "Arg of perigee: " << setw(16) << w << " rad" << endl 851 << "Mean anomaly at epoch: " << setw(16) << M0 << " rad" << endl 852 << "Right ascension: " << setw(16) << OMEGA0 << " rad " 853 << setw(16) << OMEGAdot << " rad/sec" << endl 854 << "Inclination: " << setw(16) << i0 << " rad " 855 << setw(16) << idot << " rad/sec" << endl; 856 857 s << endl 858 << " HARMONIC CORRECTIONS" 859 << endl 860 << endl 861 << "Radial Sine: " << setw(16) << Crs << " m Cosine: " 862 << setw(16) << Crc << " m" << endl 863 << "Inclination Sine: " << setw(16) << Cis << " rad Cosine: " 864 << setw(16) << Cic << " rad" << endl 865 << "In-track Sine: " << setw(16) << Cus << " rad Cosine: " 866 << setw(16) << Cuc << " rad" << endl; 867 868 s << endl; 869 870 s.flags(oldFlags); 871 } // end of BrcKeplerOrbit::dump() 872 operator <<(ostream & s,const BrcKeplerOrbit & eph)873 ostream& operator<<(ostream& s, const BrcKeplerOrbit& eph) 874 { 875 eph.dump(s); 876 return s; 877 878 } // end of operator<< 879 880 } // namespace 881